I have successfully build a quadcopter using Arduino with altitude hold using SRF 05 ultrasonic sensor. Now, I am using an ADNS3080 optical flow sensor for position hold, however, it is quite weird to me that the quadcopter becomes EXTREMLY UNSTABLE when flying and reading the data from the optical flow (with no horizontal control yet).
Previously, I have the same phenomenon with the ultrasonic and find out that it is due to the delayMicroseconds that makes my quadcopter unstable. But now, I have implement a new delay function when reading through SPI interface.
void MyDelay(unsigned long MicroSeconds){
long start_micro = micros();
unsigned long current_micro = micros();
while(current_micro-start_micro < MicroSeconds){
current_micro = micros();
};
}
The phenomenon is like one motor is being cut down the power.
The reason why I do not use the delayMicroseconds is that previously, when I do an altitude hold with my quadcopter, it seems like the delayMicroseconds() function call during measuring the altitude from SRF05 will make the Servo.writeMicroseconds() malfunction. For example, if I write :
Then, as I guess, the pulse will actually becomes different from 1000 (1050 maybe, I do not have oscilloscope at my home). I actually overcome this by using writeMicroseconds() on the trigger pin . I think that the delayMicroseconds() use the Timer which has higher priority than the Timer used for writeMicroseconds().
Thus I have to rewrite into MyDelay() as shown before.
So why I cannot use blocking delay for my quadcopter ? My looptime for the quadcopter is about 10-12ms.
For your reference, here is the code that I use MyDelay():
byte read_register_ADNS3080 (byte address)
{
byte result, stat;
backup_spi_settings_ADNS3080();
digitalWrite(SS, LOW); // select
stat = SPI.transfer(address); // send the register address
//Serial.println(stat,HEX);
MyDelay(50);
result = SPI.transfer(0x00); // send a value of 0 to read the first byte returned
MyDelay(50);
digitalWrite(SS, HIGH); // de-select
restore_spi_settings_ADNS3080();
return result;
}
byte backup_spi_settings_ADNS3080()
{
orig_spi_settings_spcr = SPCR & (DORD | CPOL | CPHA); // store current spi values
orig_spi_settings_spsr = SPSR & SPI2X;
SPI.setBitOrder(MSBFIRST); // set the values that we need
SPI.setDataMode(SPI_MODE3);
SPI.setClockDivider(SPI_CLOCK_DIV8); // sensor running at 2Mhz - maximum speed
return orig_spi_settings_spcr;
}
byte restore_spi_settings_ADNS3080()
{
byte temp;
temp = SPSR; // restore SPSR
temp &= ~SPI2X;
temp |= orig_spi_settings_spsr;
SPSR = temp;
temp = SPCR; // restore SPCR
temp &= ~(DORD | CPOL | CPHA); // zero out the important bits
temp |= orig_spi_settings_spcr; // restore important bits
SPCR = temp;
return temp;
}
Well, by using delayMicroseconds() twice per byte transferred, you’ve pissed away 10% of your loop time budget. If you do too much of that, you will exceed the time budget and the quadcopter will become unstable. Just like you observed.
From a quick perusal of the datasheet, that chip doesn’t need you to add subjective-weeks of time between the SPI bytes. It does show 50us in a couple of places may be required.
Can you find something more productive for your Arduino to do while it’s waiting the requisite 50us?
It’s not that simple. The datasheet for the sensor says you must wait 50us between reading and writing. If this time period can fit into the time budget available, then there’s no reason not to use a delay. If it can’t, then the programming to allow this wait period to be filled with something useful is NOT easy.
We are only talking about a 20th of a millisecond delay.
At the very least, getting rid of the delays is the only way to prove whether they are the cause of the problem.
If you need a 50µsec gap between two successive actions maybe a reorganization of the flow of the program would provide that without any need for an interval.
OK, I think I will get rid of those delay now. Sure this will take away my beautiful loop flows, but let's see if that what I can get :).
Actually, one of the things I didn't give up my delay is that in the reference code from Ardupilot, they use function
hal.scheduler->delay_microseconds(50);
Originally, I did not realize that it was some kind of multi-threading in their HAL library. So that's may be the different between the Arduino delay function and the HAL delay function.
