Erik van de Coevering / Mbed 2 deprecated Multicopter_2018

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

calccomp.h@8:981f7e2365b6, 2018-07-31 (annotated)

Committer:
Anaesthetix
Date:
Tue Jul 31 20:36:57 2018 +0000
Revision:
8:981f7e2365b6
Parent:
6:033ad7377fee 
Switched from Madgwick to Mahony as I'm having trouble with slow oscillations caused by the madgwick filter. Fixed an error on the PID algorithm also.

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 0:0929d3d566cf 3 #include "mbed.h"
Anaesthetix 8:981f7e2365b6 4 #include "MAfilter.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 8:981f7e2365b6 9 float KPx, KPy, KIx, KIy, KDx, KDy, KPz, KIz, KDz, error_x1, error_x, error_y1, error_y;
Anaesthetix 8:981f7e2365b6 10 MAfilter10 ma_errorx, ma_errory;
Anaesthetix 8:981f7e2365b6 11 static signed int m1 = 0;
Anaesthetix 8:981f7e2365b6 12 static signed int m2 = 0;
Anaesthetix 8:981f7e2365b6 13 static signed int m3 = 0;
Anaesthetix 8:981f7e2365b6 14 static signed int m4 = 0;
Anaesthetix 0:0929d3d566cf 15
Anaesthetix 0:0929d3d566cf 16 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 17 {
Anaesthetix 3:a49ab9168fb2 18 //Rx variables
Anaesthetix 3:a49ab9168fb2 19 int ruddercenter = 1562;
Anaesthetix 3:a49ab9168fb2 20 int elevcenter = 1554;
Anaesthetix 3:a49ab9168fb2 21 int aileroncenter = 1550;
Anaesthetix 3:a49ab9168fb2 22
Anaesthetix 3:a49ab9168fb2 23 //Variables for calccomp
Anaesthetix 3:a49ab9168fb2 24 float xcomp = 0;
Anaesthetix 3:a49ab9168fb2 25 float ycomp = 0;
Anaesthetix 3:a49ab9168fb2 26 int xcomp2 = 0;
Anaesthetix 3:a49ab9168fb2 27 int rud = 0;
Anaesthetix 3:a49ab9168fb2 28 int zcomp = 0;
Anaesthetix 3:a49ab9168fb2 29 int throttle = 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 error_z = 0;
Anaesthetix 5:0f4204941604 34 float error_z1 = 0;
Anaesthetix 3:a49ab9168fb2 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 3:a49ab9168fb2 50
Anaesthetix 8:981f7e2365b6 51 // Current values used on a 250 size mini racer (still needs tuning): P: 2.7, I: 2.0, D: 0.4
Anaesthetix 3:a49ab9168fb2 52 // Calc PID values and prevent integral windup on KIx
Anaesthetix 8:981f7e2365b6 53 error_x = angles[0] - xcntrl;
Anaesthetix 8:981f7e2365b6 54 KPx = error_x * KP_y;
Anaesthetix 8:981f7e2365b6 55 KIx = KIx + (error_x * 0.001f * KI_y); // PID gains mixed up, will fix soon.
Anaesthetix 3:a49ab9168fb2 56
Anaesthetix 8:981f7e2365b6 57 if(KIx > 10) KIx = 10;
Anaesthetix 8:981f7e2365b6 58 if(KIx < -10) KIx = -10;
Anaesthetix 8:981f7e2365b6 59
Anaesthetix 8:981f7e2365b6 60 KDx = (angles[3] + (error_x)) * KD_y; // Should be a derative of the error, but this way feels much more responsive
Anaesthetix 3:a49ab9168fb2 61
Anaesthetix 3:a49ab9168fb2 62 xcomp = KPx + KIx + KDx;
Anaesthetix 3:a49ab9168fb2 63 xcomp2 = xcomp*-1;
Anaesthetix 4:fab65ad01ab4 64
Anaesthetix 6:033ad7377fee 65 //Mix pitch
Anaesthetix 0:0929d3d566cf 66 m1 = m1 - xcomp2;
Anaesthetix 0:0929d3d566cf 67 m2 = m2 + xcomp2;
Anaesthetix 0:0929d3d566cf 68 m3 = m3 + xcomp2;
Anaesthetix 0:0929d3d566cf 69 m4 = m4 - xcomp2;
Anaesthetix 3:a49ab9168fb2 70
Anaesthetix 4:fab65ad01ab4 71 // Calc PID values and prevent integral windup on KIy
Anaesthetix 8:981f7e2365b6 72 error_y = angles[1] + ycntrl;
Anaesthetix 8:981f7e2365b6 73 KPy = error_y * KP_x;
Anaesthetix 8:981f7e2365b6 74 KIy = KIy + (error_y * 0.001f * KI_x);
Anaesthetix 3:a49ab9168fb2 75
Anaesthetix 8:981f7e2365b6 76 if(KIy > 10) KIy = 10;
Anaesthetix 8:981f7e2365b6 77 if(KIy < -10) KIy = -10;
Anaesthetix 8:981f7e2365b6 78 KDy = (angles[4] + (error_y)) * KD_x;
Anaesthetix 3:a49ab9168fb2 79
Anaesthetix 3:a49ab9168fb2 80 ycomp = KPy + KIy + KDy;
Anaesthetix 3:a49ab9168fb2 81 ycomp = ycomp*-1;
Anaesthetix 4:fab65ad01ab4 82
Anaesthetix 6:033ad7377fee 83 //Mix roll
Anaesthetix 0:0929d3d566cf 84 m1 = m1 + ycomp;
Anaesthetix 0:0929d3d566cf 85 m2 = m2 + ycomp;
Anaesthetix 0:0929d3d566cf 86 m3 = m3 - ycomp;
Anaesthetix 0:0929d3d566cf 87 m4 = m4 - ycomp;
Anaesthetix 0:0929d3d566cf 88
Anaesthetix 0:0929d3d566cf 89
Anaesthetix 6:033ad7377fee 90 //Calc yaw compensation and mix
Anaesthetix 5:0f4204941604 91 error_z = angles[5] + rud;
Anaesthetix 5:0f4204941604 92
Anaesthetix 6:033ad7377fee 93 KPz = error_z * KP_z;
Anaesthetix 6:033ad7377fee 94 KIz = KIz + (error_z * 0.001f * KI_z);
Anaesthetix 5:0f4204941604 95
Anaesthetix 8:981f7e2365b6 96 if(KIz > 20) KIz = 20;
Anaesthetix 8:981f7e2365b6 97 if(KIz < -20) KIz = -20;
Anaesthetix 5:0f4204941604 98
Anaesthetix 6:033ad7377fee 99 KDz = (error_z - error_z1) * KD_z;
Anaesthetix 8:981f7e2365b6 100
Anaesthetix 5:0f4204941604 101 error_z1 = error_z;
Anaesthetix 5:0f4204941604 102
Anaesthetix 8:981f7e2365b6 103 zcomp = (KPz + KDz) * -1.0f;
Anaesthetix 0:0929d3d566cf 104
Anaesthetix 6:033ad7377fee 105 //Mix yaw
Anaesthetix 0:0929d3d566cf 106 m1 = m1 - zcomp;
Anaesthetix 0:0929d3d566cf 107 m2 = m2 + zcomp;
Anaesthetix 0:0929d3d566cf 108 m3 = m3 - zcomp;
Anaesthetix 0:0929d3d566cf 109 m4 = m4 + zcomp;
Anaesthetix 0:0929d3d566cf 110
Anaesthetix 3:a49ab9168fb2 111 //When throttle down or if not armed, stop motors
Anaesthetix 0:0929d3d566cf 112 if(throttle < 1100 || armed == false) {
Anaesthetix 0:0929d3d566cf 113 m1=1000;
Anaesthetix 0:0929d3d566cf 114 m2=1000;
Anaesthetix 0:0929d3d566cf 115 m3=1000;
Anaesthetix 0:0929d3d566cf 116 m4=1000;
Anaesthetix 0:0929d3d566cf 117 }
Anaesthetix 0:0929d3d566cf 118
Anaesthetix 0:0929d3d566cf 119 //Stick arming
Anaesthetix 0:0929d3d566cf 120 if(throttle < 1100 && rud > 100) {
Anaesthetix 0:0929d3d566cf 121 armed = true;
Anaesthetix 0:0929d3d566cf 122 led1 = 0;
Anaesthetix 0:0929d3d566cf 123 }
Anaesthetix 0:0929d3d566cf 124 if(throttle < 1100 && rud < -100) {
Anaesthetix 0:0929d3d566cf 125 armed = false;
Anaesthetix 0:0929d3d566cf 126 led1 = 1;
Anaesthetix 0:0929d3d566cf 127 }
Anaesthetix 0:0929d3d566cf 128
Anaesthetix 0:0929d3d566cf 129
Anaesthetix 3:a49ab9168fb2 130
Anaesthetix 0:0929d3d566cf 131 //Output to motors
Anaesthetix 3:a49ab9168fb2 132 motor[0] = m1;
Anaesthetix 3:a49ab9168fb2 133 motor[1] = m2;
Anaesthetix 3:a49ab9168fb2 134 motor[2] = m3;
Anaesthetix 3:a49ab9168fb2 135 motor[3] = m4;
Anaesthetix 8:981f7e2365b6 136 }