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@2:61ad38ea550d, 2018-07-09 (annotated)
- Committer:
- Anaesthetix
- Date:
- Mon Jul 09 19:18:20 2018 +0000
- Revision:
- 2:61ad38ea550d
- Parent:
- 0:0929d3d566cf
- Child:
- 3:a49ab9168fb2
Minor changes
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Anaesthetix | 0:0929d3d566cf | 1 | #include "mbed.h" |
Anaesthetix | 0:0929d3d566cf | 2 | |
Anaesthetix | 0:0929d3d566cf | 3 | DigitalOut led1(LED1); // for stick arming (leds are active low) |
Anaesthetix | 0:0929d3d566cf | 4 | |
Anaesthetix | 0:0929d3d566cf | 5 | extern float Kp, Ki, Kd; |
Anaesthetix | 0:0929d3d566cf | 6 | |
Anaesthetix | 0:0929d3d566cf | 7 | 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 | 8 | { |
Anaesthetix | 0:0929d3d566cf | 9 | //Rx variables |
Anaesthetix | 0:0929d3d566cf | 10 | int ruddercenter = 1562; |
Anaesthetix | 0:0929d3d566cf | 11 | int elevcenter = 1554; |
Anaesthetix | 0:0929d3d566cf | 12 | int aileroncenter = 1550; |
Anaesthetix | 0:0929d3d566cf | 13 | |
Anaesthetix | 0:0929d3d566cf | 14 | //Variables for calccomp |
Anaesthetix | 0:0929d3d566cf | 15 | float xcomp = 0; |
Anaesthetix | 0:0929d3d566cf | 16 | float ycomp = 0; |
Anaesthetix | 0:0929d3d566cf | 17 | int xcomp2 = 0; |
Anaesthetix | 0:0929d3d566cf | 18 | // int ycomp2 = 0; |
Anaesthetix | 0:0929d3d566cf | 19 | float ruddercomp = 0; |
Anaesthetix | 0:0929d3d566cf | 20 | // float ratio = 0.3; |
Anaesthetix | 0:0929d3d566cf | 21 | int rud = 0; |
Anaesthetix | 0:0929d3d566cf | 22 | int zcomp = 0; |
Anaesthetix | 0:0929d3d566cf | 23 | int throttle = 0; |
Anaesthetix | 0:0929d3d566cf | 24 | static signed int m1 = 0; |
Anaesthetix | 0:0929d3d566cf | 25 | static signed int m2 = 0; |
Anaesthetix | 0:0929d3d566cf | 26 | static signed int m3 = 0; |
Anaesthetix | 0:0929d3d566cf | 27 | static signed int m4 = 0; |
Anaesthetix | 0:0929d3d566cf | 28 | static bool armed = false; |
Anaesthetix | 0:0929d3d566cf | 29 | float xcntrl = 0; |
Anaesthetix | 0:0929d3d566cf | 30 | float ycntrl = 0; |
Anaesthetix | 2:61ad38ea550d | 31 | |
Anaesthetix | 0:0929d3d566cf | 32 | //Scale rx channels |
Anaesthetix | 0:0929d3d566cf | 33 | rud = (((float)(ctrl[0] - ruddercenter))/2.5); |
Anaesthetix | 0:0929d3d566cf | 34 | ycntrl = ((float)(ctrl[3] - elevcenter))/8; |
Anaesthetix | 0:0929d3d566cf | 35 | throttle = ctrl[2] - 20; |
Anaesthetix | 0:0929d3d566cf | 36 | xcntrl = ((float)(ctrl[1] - aileroncenter))/8; |
Anaesthetix | 0:0929d3d566cf | 37 | |
Anaesthetix | 0:0929d3d566cf | 38 | //Limit throttle |
Anaesthetix | 0:0929d3d566cf | 39 | if(throttle > 1950) throttle = 1950; |
Anaesthetix | 0:0929d3d566cf | 40 | |
Anaesthetix | 0:0929d3d566cf | 41 | //Start by mixing throttle |
Anaesthetix | 0:0929d3d566cf | 42 | m1 = throttle; |
Anaesthetix | 0:0929d3d566cf | 43 | m2 = throttle; |
Anaesthetix | 0:0929d3d566cf | 44 | m3 = throttle; |
Anaesthetix | 0:0929d3d566cf | 45 | m4 = throttle; |
Anaesthetix | 0:0929d3d566cf | 46 | /* |
Anaesthetix | 0:0929d3d566cf | 47 | // Calc PID values and prevent integral windup on KIx |
Anaesthetix | 2:61ad38ea550d | 48 | KPx = (angles[0] + xcntrl) * Kp; |
Anaesthetix | 2:61ad38ea550d | 49 | KIx = KIx + ((angles[0] + xcntrl) * Kd); |
Anaesthetix | 0:0929d3d566cf | 50 | |
Anaesthetix | 0:0929d3d566cf | 51 | if(KIx > 30) KIx = 30; |
Anaesthetix | 0:0929d3d566cf | 52 | if(KIx < -30) KIx = -30; |
Anaesthetix | 2:61ad38ea550d | 53 | KDx = (angles[3] + (angles[0] + xcntrl)) Kd; |
Anaesthetix | 0:0929d3d566cf | 54 | |
Anaesthetix | 0:0929d3d566cf | 55 | xcomp = KPx + KIx + KDx; |
Anaesthetix | 0:0929d3d566cf | 56 | xcomp2 = xcomp*-1; |
Anaesthetix | 0:0929d3d566cf | 57 | */ |
Anaesthetix | 0:0929d3d566cf | 58 | //Calc aileron compensation and mix |
Anaesthetix | 0:0929d3d566cf | 59 | xcomp = ((((angles[0])-xcntrl)*Kp) + ((angles[3]+((angles[0]-xcntrl)*1.0f))*Kd)); |
Anaesthetix | 0:0929d3d566cf | 60 | xcomp2 = (xcomp*-1); |
Anaesthetix | 0:0929d3d566cf | 61 | // xcomp2 = 0; |
Anaesthetix | 0:0929d3d566cf | 62 | |
Anaesthetix | 0:0929d3d566cf | 63 | //Mix aileron |
Anaesthetix | 0:0929d3d566cf | 64 | m1 = m1 - xcomp2; |
Anaesthetix | 0:0929d3d566cf | 65 | m2 = m2 + xcomp2; |
Anaesthetix | 0:0929d3d566cf | 66 | m3 = m3 + xcomp2; |
Anaesthetix | 0:0929d3d566cf | 67 | m4 = m4 - xcomp2; |
Anaesthetix | 0:0929d3d566cf | 68 | /* |
Anaesthetix | 0:0929d3d566cf | 69 | // Calc PID values and prevent integral windup on KIx |
Anaesthetix | 2:61ad38ea550d | 70 | KPy = (angles[1] + ycntrl) * Kd; |
Anaesthetix | 2:61ad38ea550d | 71 | KIy = KIx + ((angles[1] + ycntrl) * Ki); |
Anaesthetix | 0:0929d3d566cf | 72 | |
Anaesthetix | 0:0929d3d566cf | 73 | if(KIy > 30) KIy = 30; |
Anaesthetix | 0:0929d3d566cf | 74 | if(KIy < -30) KIy = -30; |
Anaesthetix | 2:61ad38ea550d | 75 | KDx = (angles[4] + (angles[1] + ycntrl)) * Kd; |
Anaesthetix | 0:0929d3d566cf | 76 | |
Anaesthetix | 0:0929d3d566cf | 77 | ycomp = KPy + KIy + KDy; |
Anaesthetix | 0:0929d3d566cf | 78 | */ |
Anaesthetix | 0:0929d3d566cf | 79 | //ycntrl = ycntrl+5; |
Anaesthetix | 0:0929d3d566cf | 80 | //Calc elevator compensation and mix |
Anaesthetix | 0:0929d3d566cf | 81 | ycomp = ((((angles[1])+(ycntrl))*Kp) + ((angles[4]+((angles[1]+ycntrl)*1.0f))*Kd)); |
Anaesthetix | 0:0929d3d566cf | 82 | ycomp = ycomp*-1; |
Anaesthetix | 0:0929d3d566cf | 83 | // ycomp2 = 0; |
Anaesthetix | 0:0929d3d566cf | 84 | |
Anaesthetix | 0:0929d3d566cf | 85 | //Mix elevator |
Anaesthetix | 0:0929d3d566cf | 86 | m1 = m1 + ycomp; |
Anaesthetix | 0:0929d3d566cf | 87 | m2 = m2 + ycomp; |
Anaesthetix | 0:0929d3d566cf | 88 | m3 = m3 - ycomp; |
Anaesthetix | 0:0929d3d566cf | 89 | m4 = m4 - ycomp; |
Anaesthetix | 0:0929d3d566cf | 90 | |
Anaesthetix | 0:0929d3d566cf | 91 | |
Anaesthetix | 0:0929d3d566cf | 92 | //Calc rudder compensation and mix |
Anaesthetix | 0:0929d3d566cf | 93 | ruddercomp = (rud + (angles[5]*1.0f)); //has drift |
Anaesthetix | 0:0929d3d566cf | 94 | zcomp = ruddercomp*-1; |
Anaesthetix | 0:0929d3d566cf | 95 | //zcomp = 0; |
Anaesthetix | 0:0929d3d566cf | 96 | |
Anaesthetix | 0:0929d3d566cf | 97 | //Mix rudder |
Anaesthetix | 0:0929d3d566cf | 98 | m1 = m1 - zcomp; |
Anaesthetix | 0:0929d3d566cf | 99 | m2 = m2 + zcomp; |
Anaesthetix | 0:0929d3d566cf | 100 | m3 = m3 - zcomp; |
Anaesthetix | 0:0929d3d566cf | 101 | m4 = m4 + zcomp; |
Anaesthetix | 0:0929d3d566cf | 102 | |
Anaesthetix | 0:0929d3d566cf | 103 | //Prevent motors from stalling |
Anaesthetix | 0:0929d3d566cf | 104 | // if(m1 < 1075 && throttle > 1000) m1 = 1280; |
Anaesthetix | 0:0929d3d566cf | 105 | // if(m2 < 1075 && throttle > 1000) m2 = 1280; |
Anaesthetix | 0:0929d3d566cf | 106 | // if(m3 < 1075 && throttle > 1000) m3 = 1280; |
Anaesthetix | 0:0929d3d566cf | 107 | // if(m4 < 1075 && throttle > 1000) m4 = 1280; |
Anaesthetix | 0:0929d3d566cf | 108 | |
Anaesthetix | 0:0929d3d566cf | 109 | //When throttle down or if not armed, stop motors |
Anaesthetix | 0:0929d3d566cf | 110 | if(throttle < 1100 || armed == false) { |
Anaesthetix | 0:0929d3d566cf | 111 | m1=1000; |
Anaesthetix | 0:0929d3d566cf | 112 | m2=1000; |
Anaesthetix | 0:0929d3d566cf | 113 | m3=1000; |
Anaesthetix | 0:0929d3d566cf | 114 | m4=1000; |
Anaesthetix | 0:0929d3d566cf | 115 | } |
Anaesthetix | 0:0929d3d566cf | 116 | |
Anaesthetix | 0:0929d3d566cf | 117 | //Stick arming |
Anaesthetix | 0:0929d3d566cf | 118 | if(throttle < 1100 && rud > 100) { |
Anaesthetix | 0:0929d3d566cf | 119 | armed = true; |
Anaesthetix | 0:0929d3d566cf | 120 | led1 = 0; |
Anaesthetix | 0:0929d3d566cf | 121 | } |
Anaesthetix | 0:0929d3d566cf | 122 | if(throttle < 1100 && rud < -100) { |
Anaesthetix | 0:0929d3d566cf | 123 | armed = false; |
Anaesthetix | 0:0929d3d566cf | 124 | led1 = 1; |
Anaesthetix | 0:0929d3d566cf | 125 | } |
Anaesthetix | 0:0929d3d566cf | 126 | |
Anaesthetix | 0:0929d3d566cf | 127 | |
Anaesthetix | 0:0929d3d566cf | 128 | |
Anaesthetix | 0:0929d3d566cf | 129 | //Output to motors |
Anaesthetix | 0:0929d3d566cf | 130 | motor[0] = m1; |
Anaesthetix | 0:0929d3d566cf | 131 | motor[1] = m2; |
Anaesthetix | 0:0929d3d566cf | 132 | motor[2] = m3; |
Anaesthetix | 0:0929d3d566cf | 133 | motor[3] = m4; |
Anaesthetix | 0:0929d3d566cf | 134 | } |