Speeding up loop speed on a DIY Tricopter Drone Flight Controller

[StefL]

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):

1. Read IMU to compute Orientation (takes 10,800micros when I comment out everything else)
2. Check Radio Inputs (2,600 micros)
3. Calculate PID corrections (2,630micros)
4. Convert PID values to Motor Outputs (2630micros)
5. 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":

1. program reads values from the accelerometer (when testing without this step, the loop is shortened by 1600micros)

2. Program converts measured acceleration into pitch and roll angles using trigonometry

3. Program updates an array which I use as a moving average filter to smooth the signals (removing this step saves around 200Micros

4. Same as step 1 but with gyroscope. this step also includes updating the moving average filter array (removing this step saves 5000micros)

5. Same as step 2 but with gyro, and using rotation matrices to compute euler angles(removing this step saves 3000micros)

6. 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.

Stefan

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)

[system]

I'm not going to guess which of those files contains the loop() function.

Zip up your project, and attach the zip file, so we can unzip it and open it in the IDE.

Tell us which file contains loop().

OK, I lied. I found loop(). You could call calibrate() from setup(). Then, there is no need to check, on every pass through loop() if calibration is necessary.

I looked at the acceler_ometer functions, and nearly went cross___eyed trying to read that crap. The i and i variables are local to the function they are defined in. Quit with the stupid underscores trying to make unique variables unique. The compiler ALREADY knows that i__ and i are different variables.

Why do you attach and detach the ESCs on every pass through loop()?

[StefL]

Firstly, thanks for taking the time to read the code. I'm truly sorry about the mess of things, and I did take quite some time trying to neaten it up before I uploaded it for help here. It was even worse.

Anyway, I've made those fixes and attached a them in a zip file here.

Regarding the variabes crap, I wrote the _i variables before I figured out the local vs global variables, and overlooked it while neatening up the code. sorry about that, its fixed now.

Regarding the calibrate function, I repeatedly loop the calibrate function for 10 seconds to get enough data to take an average. I couldnt seem to get this working with a while loop during the setup, so thats why I resorted to that ugly if loop at the start. Please let me know if theres a better way of doing it!

After getting cut by a propeller that unexpectedly came to life while I was testing, I attach and detach the ESCs as a safe means of turning them on/off, which I control with a switch on my transmitter. Technically I only need to do that when the switch is toggled, but I felt like that would complicated my code. It didn't seem to slow it down too much so I just stuck with it.

The main delay of my loop seems to be in the Orientation functions, so are there any big errors I'm making there?

Stefan

Tricopter_2017.zip (18.1 KB)