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:
3:a49ab9168fb2
Parent:
2:61ad38ea550d
Child:
4:fab65ad01ab4
--- a/calccomp.h	Mon Jul 09 19:18:20 2018 +0000
+++ b/calccomp.h	Tue Jul 10 13:19:53 2018 +0000
@@ -6,29 +6,30 @@
 
 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
 {
-		//Rx variables
-		int ruddercenter = 1562;
-		int elevcenter = 1554;
-		int aileroncenter = 1550;
-	
-		//Variables for calccomp
-		float xcomp = 0;
-		float ycomp = 0;
-		int xcomp2 = 0;
+    //Rx variables
+    int ruddercenter = 1562;
+    int elevcenter = 1554;
+    int aileroncenter = 1550;
+
+    //Variables for calccomp
+    float xcomp = 0;
+    float ycomp = 0;
+    int xcomp2 = 0;
 //		int ycomp2 = 0;
-		float ruddercomp = 0;
+    float ruddercomp = 0;
 //		float ratio = 0.3;
-		int rud = 0;
-		int zcomp = 0;
-		int throttle = 0;
-		static signed int m1 = 0;
-		static signed int m2 = 0;
-		static signed int m3 = 0;
-		static signed int m4 = 0;
-		static bool armed = false;
-		float xcntrl = 0;
-		float ycntrl = 0;
-			
+    int rud = 0;
+    int zcomp = 0;
+    int throttle = 0;
+    static signed int m1 = 0;
+    static signed int m2 = 0;
+    static signed int m3 = 0;
+    static signed int m4 = 0;
+    static bool armed = false;
+    float xcntrl = 0;
+    float ycntrl = 0;
+    float KPx, KPy, KIx, KIy, KDx, KDy;
+
     //Scale rx channels
     rud = (((float)(ctrl[0] - ruddercenter))/2.5);
     ycntrl = ((float)(ctrl[3] - elevcenter))/8;
@@ -43,45 +44,47 @@
     m2 = throttle;
     m3 = throttle;
     m4 = throttle;
-	/*
-		// Calc PID values and prevent integral windup on KIx
-		KPx = (angles[0] + xcntrl) * Kp;
-		KIx = KIx + ((angles[0] + xcntrl) * Kd);
-		
-		if(KIx > 30) KIx = 30;
-		if(KIx < -30) KIx = -30;
-		KDx = (angles[3] + (angles[0] + xcntrl)) Kd;
-		
-		xcomp = KPx + KIx + KDx;
-		xcomp2 = xcomp*-1;
-	*/
+
+    // Current values used on a 250 size mini racer (still needs tuning): P: 1.55, I: 2.4, D: 0.4
+    // Calc PID values and prevent integral windup on KIx
+    KPx = (angles[0] - xcntrl) * Kp;
+    KIx = KIx + ((angles[0] - xcntrl) * Ki * 0.001);
+
+    if(KIx > 30) KIx = 30;
+    if(KIx < -30) KIx = -30;
+    KDx = (angles[3] + (angles[0] - xcntrl)) * Kd;
+
+    xcomp = KPx + KIx + KDx;
+    xcomp2 = xcomp*-1;
+    /*
     //Calc aileron compensation and mix
     xcomp = ((((angles[0])-xcntrl)*Kp) + ((angles[3]+((angles[0]-xcntrl)*1.0f))*Kd));
     xcomp2 = (xcomp*-1);
- //   xcomp2 = 0;
-
+    //   xcomp2 = 0;
+    */
     //Mix aileron
     m1 = m1 - xcomp2;
     m2 = m2 + xcomp2;
     m3 = m3 + xcomp2;
     m4 = m4 - xcomp2;
-	/*
-		// Calc PID values and prevent integral windup on KIx
-		KPy = (angles[1] + ycntrl) * Kd;
-		KIy = KIx + ((angles[1] + ycntrl) * Ki);
-		
-		if(KIy > 30) KIy = 30;
-		if(KIy < -30) KIy = -30;
-		KDx = (angles[4] + (angles[1] + ycntrl)) * Kd;
-		
-		ycomp = KPy + KIy + KDy;
-	*/
+
+    // Calc PID values and prevent integral windup on KIx
+    KPy = (angles[1] + ycntrl) * Kp;
+    KIy = KIy + ((angles[1] + ycntrl) * Ki * 0.001);
+
+    if(KIy > 30) KIy = 30;
+    if(KIy < -30) KIy = -30;
+    KDy = (angles[4] + (angles[1] + ycntrl)) * Kd;
+
+    ycomp = KPy + KIy + KDy;
+    ycomp = ycomp*-1;
+    /*
     //ycntrl = ycntrl+5;
     //Calc elevator compensation and mix
     ycomp = ((((angles[1])+(ycntrl))*Kp) + ((angles[4]+((angles[1]+ycntrl)*1.0f))*Kd));
     ycomp = ycomp*-1;
-   // ycomp2 = 0;
-
+    // ycomp2 = 0;
+    */
     //Mix elevator
     m1 = m1 + ycomp;
     m2 = m2 + ycomp;
@@ -92,7 +95,7 @@
     //Calc rudder compensation and mix
     ruddercomp = (rud + (angles[5]*1.0f));  //has drift
     zcomp = ruddercomp*-1;
-		//zcomp = 0;
+    //zcomp = 0;
 
     //Mix rudder
     m1 = m1 - zcomp;
@@ -106,7 +109,7 @@
 //           if(m3 < 1075 && throttle > 1000) m3 = 1280;
 //           if(m4 < 1075 && throttle > 1000) m4 = 1280;
 
-		//When throttle down or if not armed, stop motors
+    //When throttle down or if not armed, stop motors
     if(throttle < 1100 || armed == false) {
         m1=1000;
         m2=1000;
@@ -125,10 +128,10 @@
     }
 
 
-		
+
     //Output to motors
-		motor[0] = m1;
-		motor[1] = m2;
-		motor[2] = m3;
-		motor[3] = m4;
+    motor[0] = m1;
+    motor[1] = m2;
+    motor[2] = m3;
+    motor[3] = m4;
 }