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@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?
User | Revision | Line number | New 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 | } |