The IMU FIFO has a fixed size, so it can fill up if not read frequently; it’s then configurable whether the IMU stops adding to the FIFO or overwrite what’s there already. Either way, this results in data corruption. I’d heard, and a quick test confirmed that the FIFO size is 512 bytes. The same test also revealed it takes 0.064s to empty a full FIFO byte-by-byte or 0.000125s (1/8000)s to read a single byte or 1.5ms to read the 12 bytes that are a full set of sensor data.
For the FIFO code to work, I need to read the FIFO and update the ESCs often enough in comparison to the sensor sampling rate to ensure the FIFO never overflows. The rate for updating the ESCs is arbitrary; experience say about 100Hz is a good value.
The other factor is how long the motion processing takes; again experience with the hardware interrupt code suggests this is around 2ms.
A flight loop looks like this: sleep, empty FIFO, motion_processing.
The question is how long to sleep in order to get the 100Hz ESC update frequency and not allow the FIFO to overflow?
esc_period = 100Hz = 0.01s read_period = 0.0015s motion_period = 0.002s num_samples = sampling_rate * dt esc_period ≈ sleep_period + n_samples x read_period + motion_period 0.01 ≈ sleep_period + sampling_rate x dt x 0.0015 + 0.002 sleep_period ≈ 0.01 - 0.002 - sampling_rate x dt x 0.0015
I’ve updated the code, and then collapsed it to a simplified version which is now on GitHub. With some basic PID tuning, flights are back to the equivalent quality Phoebe could do although I need to keep testing for finer tuning and confidence building.