I’ve spent the past couple months coding a flight controller for a DIY tricopter and am struggling to complete the project. The current problem I’m experiencing is that my main looping function takes 13200 microseconds to complete (~75Hz).
I’m attributing the copter’s inability to balance to the slow output rates since commercial flight controllers seem to loop at over 1kHz.
Can anyone give any advice on what could be slowing down my loop?
I’ve attached my code here, but it is very big and I am a self taught coder so despite my best efforts to make it neat and understandable, it probably isnt. So I will try to describe it here.
My Serial is set to a 115200 baud rate. When I dont run any commands except the Serial display commands, it takes 2630micros. I can reduce this to 1300 by doubling the Serial BAUD rate, but this has minimal effect on the time taken by the loop.
The program starts with 10 seconds of IMU+radio calibration, then has 6 main steps of normal loop (I isolated each step and measured the loop time by commenting everything else out, and wrote the time here too):
- Read IMU to compute Orientation (takes 10,800micros when I comment out everything else)
- Check Radio Inputs (2,600 micros)
- Calculate PID corrections (2,630micros)
- Convert PID values to Motor Outputs (2630micros)
- Write Motor Outputs (2630 micros)
Clearly, the IMU scanning portion of the program is the limiting factor so I will describe that in greater detail.
I’m using a breakout board for an accelerometer and another for a gyroscope, which sample at 250Hz and 200Hz respectively.
I use the Wire Library to communicate with both sensors using I2C, and I adapted code supplied on the websites that i ordered the components from.
When i call for orientation the following steps occur (found on tab “C_Orientation”:
program reads values from the accelerometer (when testing without this step, the loop is shortened by 1600micros)
Program converts measured acceleration into pitch and roll angles using trigonometry
Program updates an array which I use as a moving average filter to smooth the signals (removing this step saves around 200Micros
Same as step 1 but with gyroscope. this step also includes updating the moving average filter array (removing this step saves 5000micros)
Same as step 2 but with gyro, and using rotation matrices to compute euler angles(removing this step saves 3000micros)
Combine Gyroscope data with accelerometer data using a complementary filter to compute an accurate orientation (removing this step saves 800micros)
Clearly step 1, 4, and 5 are the ones causing the biggest delays, and seem to involve I2C communication, updating arrays, taking averages of arrays, and some long/float maths.
(These are found on tabs, C,D,E, and F of my sketch)
I would also comment that I tend to divide my code up into many smaller functions to make my program more understandable and organized, but this could perhaps contribute towards the slow speed.
Sorry for the long code and long description, please let me know if anyone has any idea of what could be causing the delay.
A_Setup.ino (2.2 KB)
B_Display_Functions.ino (8.88 KB)
C_Orientation.ino (1.32 KB)
D_Accel_Functions.ino (3.23 KB)
E_Gyro_Functions.ino (2.79 KB)
F_Gyro_Orientation.ino (5.51 KB)
G_Gyro_Setup_Read_Write.ino (3.39 KB)
H_PID.ino (4.57 KB)
Header.h (3.61 KB)
I_Convert_Motor_Output.ino (3.24 KB)
J_writeOutputs.ino (2.49 KB)
K_Radio_Reciever.ino (6.41 KB)
L_Misc_Functions.ino (507 Bytes)
Tricopter_2017.ino (6.57 KB)