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:
5:0f4204941604
Parent:
4:fab65ad01ab4
Child:
6:033ad7377fee
--- a/calccomp.h	Thu Jul 12 13:53:55 2018 +0000
+++ b/calccomp.h	Fri Jul 13 14:27:30 2018 +0000
@@ -1,10 +1,12 @@
-// Coded by Erik van de Coevering
+// 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, Ki, Kd;
 
 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
 {
@@ -28,7 +30,9 @@
     static bool armed = false;
     float xcntrl = 0;
     float ycntrl = 0;
-    float KPx, KPy, KIx, KIy, KDx, KDy;
+    float KPx, KPy, KIx, KIy, KDx, KDy, KPz, KIz, KDz;
+    float error_z = 0;
+    float error_z1 = 0;
 
     //Scale rx channels
     rud = (((float)(ctrl[0] - ruddercenter))/2.5);
@@ -45,7 +49,7 @@
     m3 = throttle;
     m4 = throttle;
 
-    // Current values used on a 250 size mini racer (still needs tuning): P: 1.45, I: 1.5, D: 0.34
+    // 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);
@@ -64,12 +68,12 @@
     m4 = m4 - xcomp2;
 
     // Calc PID values and prevent integral windup on KIy
-    KPy = (angles[1] + ycntrl) * Kp;
-    KIy = KIy + ((angles[1] + ycntrl) * Ki * 0.001);
+    KPy = (angles[1] + ycntrl) * Kp * 0.8f;
+    KIy = KIy + ((angles[1] + ycntrl) * Ki * 0.8f * 0.001);
 
     if(KIy > 50) KIy = 50;
     if(KIy < -50) KIy = -50;
-    KDy = (angles[4] + (angles[1] + ycntrl)) * Kd;
+    KDy = (angles[4] + (angles[1] + ycntrl)) * Kd * 0.8f;
 
     ycomp = KPy + KIy + KDy;
     ycomp = ycomp*-1;
@@ -81,10 +85,19 @@
     m4 = m4 - ycomp;
 
 
-    //Calc rudder compensation and mix
-    ruddercomp = (rud + (angles[5]*1.0f));  //has drift
-    zcomp = ruddercomp*-1;
-    //zcomp = 0;
+    //Yaw PID gains hardcoded for now
+    error_z = angles[5] + rud;
+
+    KPz = error_z * 1.5f;
+    KIz = KIz + (error_z * 0.001f);
+
+    if(KIz > 50) KIz = 50;
+    if(KIz < -50) KIz = -50;
+
+    KDz = (error_z - error_z1) * 0.3f;
+    error_z1 = error_z;
+
+    zcomp = (KPz + KIz + KDz) * -1.0f;
 
     //Mix rudder
     m1 = m1 - zcomp;