Latest version of my quadcopter controller with an LPC1768 and MPU9250.
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@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?
User | Revision | Line number | New 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 | } |