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.

calccomp.h@0:0929d3d566cf, 2018-07-09 (annotated)

Committer:
Anaesthetix
Date:
Mon Jul 09 16:31:40 2018 +0000
Revision:
0:0929d3d566cf
Child:
2:61ad38ea550d
Latest version of my multicopter controller

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Anaesthetix 0:0929d3d566cf 1 #include "mbed.h"
Anaesthetix 0:0929d3d566cf 2
Anaesthetix 0:0929d3d566cf 3 DigitalOut led1(LED1); // for stick arming (leds are active low)
Anaesthetix 0:0929d3d566cf 4
Anaesthetix 0:0929d3d566cf 5 extern float Kp, Ki, Kd;
Anaesthetix 0:0929d3d566cf 6
Anaesthetix 0:0929d3d566cf 7 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
Anaesthetix 0:0929d3d566cf 8 {
Anaesthetix 0:0929d3d566cf 9 //Rx variables
Anaesthetix 0:0929d3d566cf 10 int ruddercenter = 1562;
Anaesthetix 0:0929d3d566cf 11 int elevcenter = 1554;
Anaesthetix 0:0929d3d566cf 12 int aileroncenter = 1550;
Anaesthetix 0:0929d3d566cf 13
Anaesthetix 0:0929d3d566cf 14 //Variables for calccomp
Anaesthetix 0:0929d3d566cf 15 float xcomp = 0;
Anaesthetix 0:0929d3d566cf 16 float ycomp = 0;
Anaesthetix 0:0929d3d566cf 17 int xcomp2 = 0;
Anaesthetix 0:0929d3d566cf 18 // int ycomp2 = 0;
Anaesthetix 0:0929d3d566cf 19 float ruddercomp = 0;
Anaesthetix 0:0929d3d566cf 20 // float ratio = 0.3;
Anaesthetix 0:0929d3d566cf 21 int rud = 0;
Anaesthetix 0:0929d3d566cf 22 int zcomp = 0;
Anaesthetix 0:0929d3d566cf 23 int throttle = 0;
Anaesthetix 0:0929d3d566cf 24 static signed int m1 = 0;
Anaesthetix 0:0929d3d566cf 25 static signed int m2 = 0;
Anaesthetix 0:0929d3d566cf 26 static signed int m3 = 0;
Anaesthetix 0:0929d3d566cf 27 static signed int m4 = 0;
Anaesthetix 0:0929d3d566cf 28 static bool armed = false;
Anaesthetix 0:0929d3d566cf 29 float xcntrl = 0;
Anaesthetix 0:0929d3d566cf 30 float ycntrl = 0;
Anaesthetix 0:0929d3d566cf 31
Anaesthetix 0:0929d3d566cf 32 // float KPx, KDx, KPy, KDy;
Anaesthetix 0:0929d3d566cf 33 // static float KIx = 0;
Anaesthetix 0:0929d3d566cf 34 // static float KIy = 0;
Anaesthetix 0:0929d3d566cf 35
Anaesthetix 0:0929d3d566cf 36 //Scale rx channels
Anaesthetix 0:0929d3d566cf 37 rud = (((float)(ctrl[0] - ruddercenter))/2.5);
Anaesthetix 0:0929d3d566cf 38 ycntrl = ((float)(ctrl[3] - elevcenter))/8;
Anaesthetix 0:0929d3d566cf 39 throttle = ctrl[2] - 20;
Anaesthetix 0:0929d3d566cf 40 xcntrl = ((float)(ctrl[1] - aileroncenter))/8;
Anaesthetix 0:0929d3d566cf 41
Anaesthetix 0:0929d3d566cf 42 //Limit throttle
Anaesthetix 0:0929d3d566cf 43 if(throttle > 1950) throttle = 1950;
Anaesthetix 0:0929d3d566cf 44
Anaesthetix 0:0929d3d566cf 45 //Start by mixing throttle
Anaesthetix 0:0929d3d566cf 46 m1 = throttle;
Anaesthetix 0:0929d3d566cf 47 m2 = throttle;
Anaesthetix 0:0929d3d566cf 48 m3 = throttle;
Anaesthetix 0:0929d3d566cf 49 m4 = throttle;
Anaesthetix 0:0929d3d566cf 50 /*
Anaesthetix 0:0929d3d566cf 51 // Calc PID values and prevent integral windup on KIx
Anaesthetix 0:0929d3d566cf 52 KPx = (angles[0] + xcntrl) * 1.05f;
Anaesthetix 0:0929d3d566cf 53 KIx = KIx + ((angles[0] + xcntrl) * 0.001f);
Anaesthetix 0:0929d3d566cf 54
Anaesthetix 0:0929d3d566cf 55 if(KIx > 30) KIx = 30;
Anaesthetix 0:0929d3d566cf 56 if(KIx < -30) KIx = -30;
Anaesthetix 0:0929d3d566cf 57 KDx = (angles[3] + (angles[0] + xcntrl)*0.8f) * 0.45f;
Anaesthetix 0:0929d3d566cf 58
Anaesthetix 0:0929d3d566cf 59 xcomp = KPx + KIx + KDx;
Anaesthetix 0:0929d3d566cf 60 xcomp2 = xcomp*-1;
Anaesthetix 0:0929d3d566cf 61 */
Anaesthetix 0:0929d3d566cf 62 //Calc aileron compensation and mix
Anaesthetix 0:0929d3d566cf 63 xcomp = ((((angles[0])-xcntrl)*Kp) + ((angles[3]+((angles[0]-xcntrl)*1.0f))*Kd));
Anaesthetix 0:0929d3d566cf 64 xcomp2 = (xcomp*-1);
Anaesthetix 0:0929d3d566cf 65 // xcomp2 = 0;
Anaesthetix 0:0929d3d566cf 66
Anaesthetix 0:0929d3d566cf 67 //Mix aileron
Anaesthetix 0:0929d3d566cf 68 m1 = m1 - xcomp2;
Anaesthetix 0:0929d3d566cf 69 m2 = m2 + xcomp2;
Anaesthetix 0:0929d3d566cf 70 m3 = m3 + xcomp2;
Anaesthetix 0:0929d3d566cf 71 m4 = m4 - xcomp2;
Anaesthetix 0:0929d3d566cf 72 /*
Anaesthetix 0:0929d3d566cf 73 // Calc PID values and prevent integral windup on KIx
Anaesthetix 0:0929d3d566cf 74 KPy = (angles[1] + ycntrl) * 1.05f;
Anaesthetix 0:0929d3d566cf 75 KIy = KIx + ((angles[1] + ycntrl) * 0.001f);
Anaesthetix 0:0929d3d566cf 76
Anaesthetix 0:0929d3d566cf 77 if(KIy > 30) KIy = 30;
Anaesthetix 0:0929d3d566cf 78 if(KIy < -30) KIy = -30;
Anaesthetix 0:0929d3d566cf 79 KDx = (angles[4] + (angles[1] + ycntrl)*0.8f) * 0.45f;
Anaesthetix 0:0929d3d566cf 80
Anaesthetix 0:0929d3d566cf 81 ycomp = KPy + KIy + KDy;
Anaesthetix 0:0929d3d566cf 82 */
Anaesthetix 0:0929d3d566cf 83 //ycntrl = ycntrl+5;
Anaesthetix 0:0929d3d566cf 84 //Calc elevator compensation and mix
Anaesthetix 0:0929d3d566cf 85 ycomp = ((((angles[1])+(ycntrl))*Kp) + ((angles[4]+((angles[1]+ycntrl)*1.0f))*Kd));
Anaesthetix 0:0929d3d566cf 86 ycomp = ycomp*-1;
Anaesthetix 0:0929d3d566cf 87 // ycomp2 = 0;
Anaesthetix 0:0929d3d566cf 88
Anaesthetix 0:0929d3d566cf 89 //Mix elevator
Anaesthetix 0:0929d3d566cf 90 m1 = m1 + ycomp;
Anaesthetix 0:0929d3d566cf 91 m2 = m2 + ycomp;
Anaesthetix 0:0929d3d566cf 92 m3 = m3 - ycomp;
Anaesthetix 0:0929d3d566cf 93 m4 = m4 - ycomp;
Anaesthetix 0:0929d3d566cf 94
Anaesthetix 0:0929d3d566cf 95
Anaesthetix 0:0929d3d566cf 96 //Calc rudder compensation and mix
Anaesthetix 0:0929d3d566cf 97 ruddercomp = (rud + (angles[5]*1.0f)); //has drift
Anaesthetix 0:0929d3d566cf 98 zcomp = ruddercomp*-1;
Anaesthetix 0:0929d3d566cf 99 //zcomp = 0;
Anaesthetix 0:0929d3d566cf 100
Anaesthetix 0:0929d3d566cf 101 //Mix rudder
Anaesthetix 0:0929d3d566cf 102 m1 = m1 - zcomp;
Anaesthetix 0:0929d3d566cf 103 m2 = m2 + zcomp;
Anaesthetix 0:0929d3d566cf 104 m3 = m3 - zcomp;
Anaesthetix 0:0929d3d566cf 105 m4 = m4 + zcomp;
Anaesthetix 0:0929d3d566cf 106
Anaesthetix 0:0929d3d566cf 107 //Prevent motors from stalling
Anaesthetix 0:0929d3d566cf 108 // if(m1 < 1075 && throttle > 1000) m1 = 1280;
Anaesthetix 0:0929d3d566cf 109 // if(m2 < 1075 && throttle > 1000) m2 = 1280;
Anaesthetix 0:0929d3d566cf 110 // if(m3 < 1075 && throttle > 1000) m3 = 1280;
Anaesthetix 0:0929d3d566cf 111 // if(m4 < 1075 && throttle > 1000) m4 = 1280;
Anaesthetix 0:0929d3d566cf 112
Anaesthetix 0:0929d3d566cf 113 //When throttle down or if not armed, stop motors
Anaesthetix 0:0929d3d566cf 114 if(throttle < 1100 || armed == false) {
Anaesthetix 0:0929d3d566cf 115 m1=1000;
Anaesthetix 0:0929d3d566cf 116 m2=1000;
Anaesthetix 0:0929d3d566cf 117 m3=1000;
Anaesthetix 0:0929d3d566cf 118 m4=1000;
Anaesthetix 0:0929d3d566cf 119 }
Anaesthetix 0:0929d3d566cf 120
Anaesthetix 0:0929d3d566cf 121 //Stick arming
Anaesthetix 0:0929d3d566cf 122 if(throttle < 1100 && rud > 100) {
Anaesthetix 0:0929d3d566cf 123 armed = true;
Anaesthetix 0:0929d3d566cf 124 led1 = 0;
Anaesthetix 0:0929d3d566cf 125 }
Anaesthetix 0:0929d3d566cf 126 if(throttle < 1100 && rud < -100) {
Anaesthetix 0:0929d3d566cf 127 armed = false;
Anaesthetix 0:0929d3d566cf 128 led1 = 1;
Anaesthetix 0:0929d3d566cf 129 }
Anaesthetix 0:0929d3d566cf 130
Anaesthetix 0:0929d3d566cf 131
Anaesthetix 0:0929d3d566cf 132
Anaesthetix 0:0929d3d566cf 133 //Output to motors
Anaesthetix 0:0929d3d566cf 134 motor[0] = m1;
Anaesthetix 0:0929d3d566cf 135 motor[1] = m2;
Anaesthetix 0:0929d3d566cf 136 motor[2] = m3;
Anaesthetix 0:0929d3d566cf 137 motor[3] = m4;
Anaesthetix 0:0929d3d566cf 138 }
Anaesthetix 0:0929d3d566cf 139