Erik van de Coevering / Mbed 2 deprecated Multicopter_2018

Latest version of my quadcopter controller with an LPC1768 and MPU9250.

Dependencies:   mbed

Currently running on a custom PCB with 30.5 x 30.5mm mounts. There are also 2 PC apps that go with the software; one to set up the PID controller and one to balance the motors and props. If anyone is interested, send me a message and I'll upload them.

Diff: calccomp.h

Revision:
6:033ad7377fee
Parent:
5:0f4204941604
Child:
8:981f7e2365b6
--- a/calccomp.h	Fri Jul 13 14:27:30 2018 +0000
+++ b/calccomp.h	Sat Jul 14 10:30:20 2018 +0000
@@ -1,12 +1,11 @@
 // Coded by: Erik van de Coevering
 
 // Axis are mixed up, will fix soon.
-
 #include "mbed.h"
 
 DigitalOut led1(LED1); // for stick arming (leds are active low)
-extern float Kp, Ki, Kd;
 
+extern float KP_x, KI_x, KD_x, KP_y, KI_y, KD_y, KP_z, KI_z, KD_z;
 
 void calccomp(int* ctrl, float* angles, int* motor) //ctrl: 0-rud 1-elev 2-throttle 3-aileron      angles: 0-ax 1-ay 2-az 3-gx 4-gy 5-gz
 {
@@ -19,7 +18,6 @@
     float xcomp = 0;
     float ycomp = 0;
     int xcomp2 = 0;
-    float ruddercomp = 0;
     int rud = 0;
     int zcomp = 0;
     int throttle = 0;
@@ -51,55 +49,55 @@
 
     // Current values used on a 250 size mini racer (still needs tuning): P: 1.9, I: 2.4, D: 0.45
     // Calc PID values and prevent integral windup on KIx
-    KPx = (angles[0] - xcntrl) * Kp;
-    KIx = KIx + ((angles[0] - xcntrl) * Ki * 0.001);
+    KPx = (angles[0] - xcntrl) * KP_y;
+    KIx = KIx + ((angles[0] - xcntrl) * KI_y * 0.001);
 
     if(KIx > 50) KIx = 50;
     if(KIx < -50) KIx = -50;
-    KDx = (angles[3] + (angles[0] - xcntrl)) * Kd;
+    KDx = (angles[3] + (angles[0] - xcntrl)) * KD_y;
 
     xcomp = KPx + KIx + KDx;
     xcomp2 = xcomp*-1;
 
-    //Mix aileron
+    //Mix pitch
     m1 = m1 - xcomp2;
     m2 = m2 + xcomp2;
     m3 = m3 + xcomp2;
     m4 = m4 - xcomp2;
 
     // Calc PID values and prevent integral windup on KIy
-    KPy = (angles[1] + ycntrl) * Kp * 0.8f;
-    KIy = KIy + ((angles[1] + ycntrl) * Ki * 0.8f * 0.001);
+    KPy = (angles[1] + ycntrl) * KP_x;
+    KIy = KIy + ((angles[1] + ycntrl) * KI_x * 0.001);
 
     if(KIy > 50) KIy = 50;
     if(KIy < -50) KIy = -50;
-    KDy = (angles[4] + (angles[1] + ycntrl)) * Kd * 0.8f;
+    KDy = (angles[4] + (angles[1] + ycntrl)) * KD_x;
 
     ycomp = KPy + KIy + KDy;
     ycomp = ycomp*-1;
 
-    //Mix elevator
+    //Mix roll
     m1 = m1 + ycomp;
     m2 = m2 + ycomp;
     m3 = m3 - ycomp;
     m4 = m4 - ycomp;
 
 
-    //Yaw PID gains hardcoded for now
+    //Calc yaw compensation and mix
     error_z = angles[5] + rud;
 
-    KPz = error_z * 1.5f;
-    KIz = KIz + (error_z * 0.001f);
+    KPz = error_z * KP_z;
+    KIz = KIz + (error_z * 0.001f * KI_z);
 
-    if(KIz > 50) KIz = 50;
-    if(KIz < -50) KIz = -50;
+    if(KIz > 80) KIz = 80;
+    if(KIz < -80) KIz = -80;
 
-    KDz = (error_z - error_z1) * 0.3f;
+    KDz = (error_z - error_z1) * KD_z;
     error_z1 = error_z;
 
     zcomp = (KPz + KIz + KDz) * -1.0f;
 
-    //Mix rudder
+    //Mix yaw
     m1 = m1 - zcomp;
     m2 = m2 + zcomp;
     m3 = m3 - zcomp;
@@ -130,4 +128,4 @@
     motor[1] = m2;
     motor[2] = m3;
     motor[3] = m4;
-}
+}
\ No newline at end of file