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@6:033ad7377fee, 2018-07-14 (annotated)

Committer:
Anaesthetix
Date:
Sat Jul 14 10:30:20 2018 +0000
Revision:
6:033ad7377fee
Parent:
5:0f4204941604
Child:
8:981f7e2365b6
Changed overall PID gains to gains per axis.

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