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@4:fab65ad01ab4, 2018-07-12 (annotated)

Committer:
Anaesthetix
Date:
Thu Jul 12 13:53:55 2018 +0000
Revision:
4:fab65ad01ab4
Parent:
3:a49ab9168fb2
Child:
5:0f4204941604
Minor changes and comments

Who changed what in which revision?

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