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