NOT FINISHED YET!!! My first try to get a self built fully working Quadrocopter based on an mbed, a self built frame and some other more or less cheap parts.
main.cpp@37:34917f7c10ae, 2013-06-12 (annotated)
- Committer:
- maetugr
- Date:
- Wed Jun 12 16:42:32 2013 +0000
- Revision:
- 37:34917f7c10ae
- Parent:
- 36:128c55793728
- Child:
- 38:ff95fd524c9e
Solved -180 to 180 degree YAW-range-issue, not physicaly tested yet
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
maetugr | 7:9d4313510646 | 1 | #include "mbed.h" // Standard Library |
maetugr | 7:9d4313510646 | 2 | #include "LED.h" // LEDs framework for blinking ;) |
maetugr | 13:4737ee9ebfee | 3 | #include "PC.h" // Serial Port via USB by Roland Elmiger for debugging with Terminal (driver needed: https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe) |
maetugr | 7:9d4313510646 | 4 | #include "L3G4200D.h" // Gyro (Gyroscope) |
maetugr | 7:9d4313510646 | 5 | #include "ADXL345.h" // Acc (Accelerometer) |
maetugr | 7:9d4313510646 | 6 | #include "HMC5883.h" // Comp (Compass) |
maetugr | 14:cf260677ecde | 7 | #include "BMP085_old.h" // Alt (Altitude sensor) |
maetugr | 30:021e13b62575 | 8 | #include "RC_Channel.h" // RemoteControl Channels with PPM |
maetugr | 15:753c5d6a63b3 | 9 | #include "Servo_PWM.h" // Motor PPM using PwmOut |
maetugr | 13:4737ee9ebfee | 10 | #include "PID.h" // PID Library by Aaron Berk |
maetugr | 26:96a072233d7a | 11 | #include "IMU_Filter.h" // Class to calculate position angles |
maetugr | 26:96a072233d7a | 12 | #include "Mixer.h" // Class to calculate motorspeeds from Angles, Regulation and RC-Signals |
maetugr | 0:0c4fafa398b4 | 13 | |
maetugr | 30:021e13b62575 | 14 | #define RATE 0.002 // speed of the interrupt for Sensors and PID |
maetugr | 30:021e13b62575 | 15 | #define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz) |
maetugr | 33:fd98776b6cc7 | 16 | #define RC_SENSITIVITY 30 // maximal angle from horizontal that the PID is aming for |
maetugr | 36:128c55793728 | 17 | #define YAWSPEED 0.2 // maximal speed of yaw rotation in degree per Rate |
maetugr | 37:34917f7c10ae | 18 | #define INTEGRAL_MAX 300 |
maetugr | 15:753c5d6a63b3 | 19 | |
maetugr | 34:3aa1cbcde59d | 20 | // RC |
maetugr | 34:3aa1cbcde59d | 21 | #define AILERON 0 |
maetugr | 34:3aa1cbcde59d | 22 | #define ELEVATOR 1 |
maetugr | 34:3aa1cbcde59d | 23 | #define RUDDER 2 |
maetugr | 34:3aa1cbcde59d | 24 | #define THROTTLE 3 |
maetugr | 34:3aa1cbcde59d | 25 | // Axes |
maetugr | 34:3aa1cbcde59d | 26 | #define ROLL 0 |
maetugr | 34:3aa1cbcde59d | 27 | #define PITCH 1 |
maetugr | 34:3aa1cbcde59d | 28 | #define YAW 2 |
maetugr | 25:0498d3041afa | 29 | |
maetugr | 29:8b7362a2ee14 | 30 | #define PC_CONNECTED // decoment if you want to debug per USB/Bluetooth and your PC |
maetugr | 2:93f703d2c4d7 | 31 | |
maetugr | 34:3aa1cbcde59d | 32 | // Global variables |
maetugr | 34:3aa1cbcde59d | 33 | bool armed = false; // this variable is for security (when false no motor rotates any more) |
maetugr | 37:34917f7c10ae | 34 | bool RC_present = false; // this variable shows if an RC is present |
maetugr | 34:3aa1cbcde59d | 35 | float dt = 0; |
maetugr | 34:3aa1cbcde59d | 36 | float time_for_dt = 0; |
maetugr | 34:3aa1cbcde59d | 37 | float dt_read_sensors = 0; |
maetugr | 34:3aa1cbcde59d | 38 | float time_read_sensors = 0; |
maetugr | 34:3aa1cbcde59d | 39 | float controller_value[] = {0,0,0}; // The calculated answer form the Controller |
maetugr | 34:3aa1cbcde59d | 40 | float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC |
maetugr | 37:34917f7c10ae | 41 | float RC_yaw_adding; // temporary variable to take the desired yaw adjustment |
maetugr | 34:3aa1cbcde59d | 42 | |
maetugr | 35:2a9465fedb99 | 43 | float P = 4.0; // PID values |
maetugr | 34:3aa1cbcde59d | 44 | float I = 0; |
maetugr | 37:34917f7c10ae | 45 | float D = 1.0; |
maetugr | 34:3aa1cbcde59d | 46 | |
maetugr | 14:cf260677ecde | 47 | Timer GlobalTimer; // global time to calculate processing speed |
maetugr | 34:3aa1cbcde59d | 48 | Ticker Dutycycler; // timecontrolled interrupt for exact timed control loop |
maetugr | 14:cf260677ecde | 49 | |
maetugr | 34:3aa1cbcde59d | 50 | // Initialisation of hardware (see includes for more info) |
maetugr | 5:818c0668fd2d | 51 | LED LEDs; |
maetugr | 21:c2a2e7cbabdd | 52 | #ifdef PC_CONNECTED |
maetugr | 37:34917f7c10ae | 53 | PC pc(USBTX, USBRX, 115200); // USB |
maetugr | 35:2a9465fedb99 | 54 | //PC pc(p9, p10, 115200); // Bluetooth |
maetugr | 21:c2a2e7cbabdd | 55 | #endif |
maetugr | 5:818c0668fd2d | 56 | L3G4200D Gyro(p28, p27); |
maetugr | 5:818c0668fd2d | 57 | ADXL345 Acc(p28, p27); |
maetugr | 11:9bf69bc6df45 | 58 | HMC5883 Comp(p28, p27); |
maetugr | 14:cf260677ecde | 59 | BMP085_old Alt(p28, p27); |
maetugr | 34:3aa1cbcde59d | 60 | RC_Channel RC[] = {RC_Channel(p5,1), RC_Channel(p6,2), RC_Channel(p8,4), RC_Channel(p7,3)}; // no p19/p20 ! |
maetugr | 34:3aa1cbcde59d | 61 | Servo_PWM ESC[] = {Servo_PWM(p21,PPM_FREQU), Servo_PWM(p22,PPM_FREQU), Servo_PWM(p23,PPM_FREQU), Servo_PWM(p24,PPM_FREQU)}; // p21 - p26 only because PWM needed! |
maetugr | 34:3aa1cbcde59d | 62 | IMU_Filter IMU; // (don't write () after constructor for no arguments!) |
maetugr | 34:3aa1cbcde59d | 63 | Mixer MIX(1); // 0 for +-Formation, 1 for X-Formation |
maetugr | 37:34917f7c10ae | 64 | PID Controller[] = {PID(P, I, D, INTEGRAL_MAX), PID(P, I, D, INTEGRAL_MAX), PID(1.0, 0, 0, INTEGRAL_MAX)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw |
maetugr | 21:c2a2e7cbabdd | 65 | |
maetugr | 28:ba6ca9f4def4 | 66 | void dutycycle() // method which is called by the Ticker Dutycycler every RATE seconds |
maetugr | 8:d25ecdcdbeb5 | 67 | { |
maetugr | 33:fd98776b6cc7 | 68 | time_read_sensors = GlobalTimer.read(); // start time measure for sensors |
maetugr | 12:67a06c9b69d5 | 69 | |
maetugr | 15:753c5d6a63b3 | 70 | // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes |
maetugr | 14:cf260677ecde | 71 | Gyro.read(); |
maetugr | 34:3aa1cbcde59d | 72 | Acc.read(); |
maetugr | 34:3aa1cbcde59d | 73 | //Comp.read(); // TODO: not every loop every sensor? altitude not that important |
maetugr | 34:3aa1cbcde59d | 74 | //Alt.Update(); // TODO needs very long to read because of waits |
maetugr | 12:67a06c9b69d5 | 75 | |
maetugr | 37:34917f7c10ae | 76 | //pc.printf("%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f\r\n", Gyro.data[0], Gyro.data[1], Gyro.data[2], Acc.data[0], Acc.data[1], Acc.data[2]); |
maetugr | 35:2a9465fedb99 | 77 | |
maetugr | 33:fd98776b6cc7 | 78 | dt_read_sensors = GlobalTimer.read() - time_read_sensors; // stop time measure for sensors |
maetugr | 8:d25ecdcdbeb5 | 79 | |
maetugr | 33:fd98776b6cc7 | 80 | // meassure dt for the filter |
maetugr | 33:fd98776b6cc7 | 81 | dt = GlobalTimer.read() - time_for_dt; // time in us since last loop |
maetugr | 33:fd98776b6cc7 | 82 | time_for_dt = GlobalTimer.read(); // set new time for next measurement |
maetugr | 12:67a06c9b69d5 | 83 | |
maetugr | 26:96a072233d7a | 84 | IMU.compute(dt, Gyro.data, Acc.data); |
maetugr | 35:2a9465fedb99 | 85 | //pc.printf("%f,%f,%f,%3.5fs,%3.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], dt, dt_read_sensors); |
maetugr | 35:2a9465fedb99 | 86 | |
maetugr | 37:34917f7c10ae | 87 | if(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100) |
maetugr | 37:34917f7c10ae | 88 | RC_present = false; |
maetugr | 37:34917f7c10ae | 89 | else |
maetugr | 37:34917f7c10ae | 90 | RC_present = true; |
maetugr | 37:34917f7c10ae | 91 | |
maetugr | 21:c2a2e7cbabdd | 92 | // Arming / disarming |
maetugr | 34:3aa1cbcde59d | 93 | if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) { |
maetugr | 21:c2a2e7cbabdd | 94 | armed = true; |
maetugr | 25:0498d3041afa | 95 | } |
maetugr | 37:34917f7c10ae | 96 | if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || !RC_present) { |
maetugr | 20:e116e596e540 | 97 | armed = false; |
maetugr | 25:0498d3041afa | 98 | } |
maetugr | 20:e116e596e540 | 99 | |
maetugr | 37:34917f7c10ae | 100 | // RC Angle ROLL-PITCH-Part |
maetugr | 33:fd98776b6cc7 | 101 | for(int i=0;i<2;i++) { // calculate new angle we want the QC to have |
maetugr | 37:34917f7c10ae | 102 | if (RC_present) |
maetugr | 37:34917f7c10ae | 103 | RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0; |
maetugr | 37:34917f7c10ae | 104 | else |
maetugr | 33:fd98776b6cc7 | 105 | RC_angle[i] = 0; |
maetugr | 33:fd98776b6cc7 | 106 | } |
maetugr | 37:34917f7c10ae | 107 | |
maetugr | 37:34917f7c10ae | 108 | // RC Angle YAW-Part |
maetugr | 37:34917f7c10ae | 109 | if (RC_present && RC[THROTTLE].read() > 20) |
maetugr | 37:34917f7c10ae | 110 | RC_yaw_adding = (RC[RUDDER].read()-500)*YAWSPEED/500; |
maetugr | 37:34917f7c10ae | 111 | else |
maetugr | 37:34917f7c10ae | 112 | RC_yaw_adding = 0; |
maetugr | 37:34917f7c10ae | 113 | |
maetugr | 37:34917f7c10ae | 114 | while(RC_angle[YAW] + RC_yaw_adding < -180 || RC_angle[YAW] + RC_yaw_adding > 180) { // make shure it's in the cycle -180 to 180 |
maetugr | 37:34917f7c10ae | 115 | if(RC_angle[YAW] + RC_yaw_adding < -180) |
maetugr | 37:34917f7c10ae | 116 | RC_yaw_adding += 360; |
maetugr | 37:34917f7c10ae | 117 | if(RC_angle[YAW] + RC_yaw_adding > 180) |
maetugr | 37:34917f7c10ae | 118 | RC_yaw_adding -= 360; |
maetugr | 37:34917f7c10ae | 119 | } |
maetugr | 37:34917f7c10ae | 120 | RC_angle[YAW] += RC_yaw_adding; // for yaw angle it's integrated |
maetugr | 30:021e13b62575 | 121 | |
maetugr | 34:3aa1cbcde59d | 122 | // PID controlling |
maetugr | 37:34917f7c10ae | 123 | for(int i=0;i<2;i++) { |
maetugr | 29:8b7362a2ee14 | 124 | Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying |
maetugr | 37:34917f7c10ae | 125 | controller_value[i] = Controller[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual angle and get his advice to correct |
maetugr | 30:021e13b62575 | 126 | } |
maetugr | 37:34917f7c10ae | 127 | Controller[YAW].setIntegrate(armed); // same for YAW |
maetugr | 37:34917f7c10ae | 128 | if (abs(RC_angle[YAW] - IMU.angle[YAW]) > 180) // for YAW a special calculation because of range -180 to 180 |
maetugr | 37:34917f7c10ae | 129 | if (RC_angle[YAW] > IMU.angle[YAW]) |
maetugr | 37:34917f7c10ae | 130 | controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW] - 360, IMU.angle[YAW]); |
maetugr | 37:34917f7c10ae | 131 | else |
maetugr | 37:34917f7c10ae | 132 | controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW] + 360, IMU.angle[YAW]); |
maetugr | 37:34917f7c10ae | 133 | else |
maetugr | 37:34917f7c10ae | 134 | controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW], IMU.angle[YAW]); |
maetugr | 29:8b7362a2ee14 | 135 | |
maetugr | 21:c2a2e7cbabdd | 136 | if (armed) // for SECURITY! |
maetugr | 22:d301b455a1ad | 137 | { |
maetugr | 34:3aa1cbcde59d | 138 | MIX.compute(RC[THROTTLE].read(), controller_value); // let the Mixer compute motorspeeds based on throttle and controller output |
maetugr | 28:ba6ca9f4def4 | 139 | for(int i=0;i<4;i++) // Set new motorspeeds |
maetugr | 26:96a072233d7a | 140 | ESC[i] = (int)MIX.Motor_speed[i]; |
maetugr | 25:0498d3041afa | 141 | |
maetugr | 15:753c5d6a63b3 | 142 | } else { |
maetugr | 26:96a072233d7a | 143 | for(int i=0;i<4;i++) // for security reason, set every motor to zero speed |
maetugr | 28:ba6ca9f4def4 | 144 | ESC[i] = 0; |
maetugr | 21:c2a2e7cbabdd | 145 | } |
maetugr | 8:d25ecdcdbeb5 | 146 | } |
maetugr | 5:818c0668fd2d | 147 | |
maetugr | 33:fd98776b6cc7 | 148 | void commandexecuter(char* command) { // take new PID values on the fly |
maetugr | 31:872d8b8c7812 | 149 | if (command[0] == 'p') |
maetugr | 33:fd98776b6cc7 | 150 | P = atof(&command[1]); |
maetugr | 31:872d8b8c7812 | 151 | if (command[0] == 'i') |
maetugr | 33:fd98776b6cc7 | 152 | I = atof(&command[1]); |
maetugr | 31:872d8b8c7812 | 153 | if (command[0] == 'd') |
maetugr | 33:fd98776b6cc7 | 154 | D = atof(&command[1]); |
maetugr | 33:fd98776b6cc7 | 155 | for(int i=0;i<2;i++) { |
maetugr | 33:fd98776b6cc7 | 156 | Controller[i].setPID(P,I,D); // give the controller the new PID values |
maetugr | 31:872d8b8c7812 | 157 | } |
maetugr | 31:872d8b8c7812 | 158 | } |
maetugr | 31:872d8b8c7812 | 159 | |
maetugr | 26:96a072233d7a | 160 | int main() { // main programm for initialisation and debug output |
maetugr | 26:96a072233d7a | 161 | NVIC_SetPriority(TIMER3_IRQn, 1); // set priorty of tickers below hardware interrupts (standard priority is 0)(this is to prevent the RC interrupt from waiting until ticker is finished) |
maetugr | 15:753c5d6a63b3 | 162 | |
maetugr | 21:c2a2e7cbabdd | 163 | #ifdef PC_CONNECTED |
maetugr | 21:c2a2e7cbabdd | 164 | // init screen |
maetugr | 12:67a06c9b69d5 | 165 | pc.locate(10,5); |
maetugr | 21:c2a2e7cbabdd | 166 | pc.printf("Flybed v0.2"); |
maetugr | 12:67a06c9b69d5 | 167 | #endif |
maetugr | 1:5a64632b1eb9 | 168 | LEDs.roll(2); |
maetugr | 5:818c0668fd2d | 169 | |
maetugr | 33:fd98776b6cc7 | 170 | Gyro.calibrate(50, 0.02); |
maetugr | 33:fd98776b6cc7 | 171 | Acc.calibrate(50, 0.02); |
maetugr | 33:fd98776b6cc7 | 172 | |
maetugr | 21:c2a2e7cbabdd | 173 | // Start! |
maetugr | 2:93f703d2c4d7 | 174 | GlobalTimer.start(); |
maetugr | 28:ba6ca9f4def4 | 175 | Dutycycler.attach(&dutycycle, RATE); // start to process all RATEms |
maetugr | 12:67a06c9b69d5 | 176 | |
maetugr | 12:67a06c9b69d5 | 177 | while(1) { |
maetugr | 35:2a9465fedb99 | 178 | #ifdef PC_CONNECTED |
maetugr | 35:2a9465fedb99 | 179 | if (pc.readable()) // Get Serial input (polled because interrupts disturb I2C) |
maetugr | 35:2a9465fedb99 | 180 | pc.readcommand(&commandexecuter); |
maetugr | 35:2a9465fedb99 | 181 | //pc.printf("%f %f %f %f %f %f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], controller_value[0], controller_value[1], controller_value[2]); // For live plot in MATLAB of IMU |
maetugr | 35:2a9465fedb99 | 182 | //pc.printf("%f,%f,%f,%f,%f,%f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], controller_value[0], controller_value[1], controller_value[2]); |
maetugr | 37:34917f7c10ae | 183 | #if 1 //pc.cls(); |
maetugr | 35:2a9465fedb99 | 184 | pc.locate(20,0); // PC output |
maetugr | 35:2a9465fedb99 | 185 | pc.printf("dt:%3.5fs dt_sensors:%3.5fs Altitude:%6.1fm ", dt, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure)); |
maetugr | 35:2a9465fedb99 | 186 | pc.locate(5,1); |
maetugr | 35:2a9465fedb99 | 187 | if(armed) |
maetugr | 35:2a9465fedb99 | 188 | pc.printf("ARMED!!!!!!!!!!!!!"); |
maetugr | 35:2a9465fedb99 | 189 | else |
maetugr | 35:2a9465fedb99 | 190 | pc.printf("DIS_ARMED "); |
maetugr | 35:2a9465fedb99 | 191 | pc.locate(5,3); |
maetugr | 35:2a9465fedb99 | 192 | pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", IMU.angle[0], IMU.angle[1], IMU.angle[2]); |
maetugr | 35:2a9465fedb99 | 193 | pc.locate(5,4); |
maetugr | 35:2a9465fedb99 | 194 | pc.printf("q0:%6.1f q1:%6.1f q2:%6.1f q3:%6.1f ", IMU.q0, IMU.q1, IMU.q2, IMU.q3); |
maetugr | 35:2a9465fedb99 | 195 | pc.locate(5,5); |
maetugr | 35:2a9465fedb99 | 196 | pc.printf("Gyro.data: X:%6.1f Y:%6.1f Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]); |
maetugr | 35:2a9465fedb99 | 197 | pc.locate(5,6); |
maetugr | 35:2a9465fedb99 | 198 | pc.printf("Acc.data: X:%6.1f Y:%6.1f Z:%6.1f", Acc.data[0], Acc.data[1], Acc.data[2]); |
maetugr | 35:2a9465fedb99 | 199 | |
maetugr | 35:2a9465fedb99 | 200 | pc.locate(5,8); |
maetugr | 35:2a9465fedb99 | 201 | pc.printf("P:%6.1f I:%6.1f D:%6.1f ", P, I, D); |
maetugr | 35:2a9465fedb99 | 202 | |
maetugr | 35:2a9465fedb99 | 203 | pc.locate(5,11); |
maetugr | 35:2a9465fedb99 | 204 | pc.printf("PID Result:"); |
maetugr | 35:2a9465fedb99 | 205 | for(int i=0;i<3;i++) |
maetugr | 35:2a9465fedb99 | 206 | pc.printf(" %d: %6.1f", i, controller_value[i]); |
maetugr | 35:2a9465fedb99 | 207 | pc.locate(5,14); |
maetugr | 35:2a9465fedb99 | 208 | pc.printf("RC angle: roll: %f pitch: %f yaw: %f ", RC_angle[0], RC_angle[1], RC_angle[2]); |
maetugr | 35:2a9465fedb99 | 209 | pc.locate(5,16); |
maetugr | 35:2a9465fedb99 | 210 | pc.printf("Motor: 0:%d 1:%d 2:%d 3:%d ", (int)MIX.Motor_speed[0], (int)MIX.Motor_speed[1], (int)MIX.Motor_speed[2], (int)MIX.Motor_speed[3]); |
maetugr | 35:2a9465fedb99 | 211 | |
maetugr | 35:2a9465fedb99 | 212 | // RC |
maetugr | 35:2a9465fedb99 | 213 | pc.locate(10,19); |
maetugr | 35:2a9465fedb99 | 214 | pc.printf("RC0: %4d RC1: %4d RC2: %4d RC3: %4d ", RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read()); |
maetugr | 35:2a9465fedb99 | 215 | |
maetugr | 35:2a9465fedb99 | 216 | pc.locate(10,21); |
maetugr | 35:2a9465fedb99 | 217 | pc.printf("Commandline: %s ", pc.command); |
maetugr | 35:2a9465fedb99 | 218 | #endif |
maetugr | 21:c2a2e7cbabdd | 219 | #endif |
maetugr | 21:c2a2e7cbabdd | 220 | if(armed){ |
maetugr | 21:c2a2e7cbabdd | 221 | LEDs.rollnext(); |
maetugr | 21:c2a2e7cbabdd | 222 | } else { |
maetugr | 26:96a072233d7a | 223 | for(int i=1;i<=4;i++) |
maetugr | 26:96a072233d7a | 224 | LEDs.set(i); |
maetugr | 21:c2a2e7cbabdd | 225 | } |
maetugr | 29:8b7362a2ee14 | 226 | wait(0.05); |
maetugr | 0:0c4fafa398b4 | 227 | } |
maetugr | 28:ba6ca9f4def4 | 228 | } |