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@5:0f4204941604, 2018-07-13 (annotated)

Committer:
Anaesthetix
Date:
Fri Jul 13 14:27:30 2018 +0000
Revision:
5:0f4204941604
Parent:
4:fab65ad01ab4
Child:
6:033ad7377fee
Yaw PID added

Who changed what in which revision?

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