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@30:021e13b62575, 2013-02-10 (annotated)
- Committer:
- maetugr
- Date:
- Sun Feb 10 22:08:10 2013 +0000
- Revision:
- 30:021e13b62575
- Parent:
- 29:8b7362a2ee14
- Child:
- 31:872d8b8c7812
newest changes because of the needed faster refresh rate for the ESCs 495Hz and the new build of the same hardware in X-configuration.; ; RC angle steering not yet finished
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 | 28:ba6ca9f4def4 | 16 | #define MAXPITCH 40 // maximal angle from horizontal that the PID is aming for |
maetugr | 28:ba6ca9f4def4 | 17 | #define YAWSPEED 2 // maximal speed of yaw rotation in degree per Rate |
maetugr | 15:753c5d6a63b3 | 18 | |
maetugr | 29:8b7362a2ee14 | 19 | #define P 0.35 // PID values |
maetugr | 29:8b7362a2ee14 | 20 | #define I 0 |
maetugr | 29:8b7362a2ee14 | 21 | #define D 0.5 |
maetugr | 25:0498d3041afa | 22 | |
maetugr | 14:cf260677ecde | 23 | //#define COMPASSCALIBRATE // decomment if you want to calibrate the Compass on start |
maetugr | 29:8b7362a2ee14 | 24 | #define PC_CONNECTED // decoment if you want to debug per USB/Bluetooth and your PC |
maetugr | 2:93f703d2c4d7 | 25 | |
maetugr | 14:cf260677ecde | 26 | Timer GlobalTimer; // global time to calculate processing speed |
maetugr | 28:ba6ca9f4def4 | 27 | Ticker Dutycycler; // timecontrolled interrupt to get data form IMU and RC |
maetugr | 14:cf260677ecde | 28 | |
maetugr | 14:cf260677ecde | 29 | // initialisation of hardware (see includes for more info) |
maetugr | 5:818c0668fd2d | 30 | LED LEDs; |
maetugr | 21:c2a2e7cbabdd | 31 | #ifdef PC_CONNECTED |
maetugr | 30:021e13b62575 | 32 | PC pc(USBTX, USBRX, 115200); // USB |
maetugr | 29:8b7362a2ee14 | 33 | //PC pc(p9, p10, 115200); // Bluetooth |
maetugr | 21:c2a2e7cbabdd | 34 | #endif |
maetugr | 25:0498d3041afa | 35 | LocalFileSystem local("local"); // Create the local filesystem under the name "local" |
maetugr | 30:021e13b62575 | 36 | //FILE *Logger; |
maetugr | 5:818c0668fd2d | 37 | L3G4200D Gyro(p28, p27); |
maetugr | 5:818c0668fd2d | 38 | ADXL345 Acc(p28, p27); |
maetugr | 11:9bf69bc6df45 | 39 | HMC5883 Comp(p28, p27); |
maetugr | 14:cf260677ecde | 40 | BMP085_old Alt(p28, p27); |
maetugr | 30:021e13b62575 | 41 | RC_Channel RC[] = {RC_Channel(p11,1), RC_Channel(p12,2), RC_Channel(p13,4), RC_Channel(p14,3)}; // no p19/p20 ! |
maetugr | 29:8b7362a2ee14 | 42 | 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 | 26:96a072233d7a | 43 | IMU_Filter IMU; // don't write () after constructor for no arguments! |
maetugr | 30:021e13b62575 | 44 | Mixer MIX(1); |
maetugr | 8:d25ecdcdbeb5 | 45 | |
maetugr | 21:c2a2e7cbabdd | 46 | // 0:X:Roll 1:Y:Pitch 2:Z:Yaw |
maetugr | 29:8b7362a2ee14 | 47 | PID Controller[] = {PID(P, I, D, 1000), PID(P, I, D, 1000), PID(0.2, 0, 0.1, 1000)}; |
maetugr | 14:cf260677ecde | 48 | |
maetugr | 22:d301b455a1ad | 49 | // global variables |
maetugr | 29:8b7362a2ee14 | 50 | bool armed = false; // this variable is for security (when false no motor rotates any more) |
maetugr | 26:96a072233d7a | 51 | unsigned long dt = 0; |
maetugr | 26:96a072233d7a | 52 | unsigned long time_for_dt = 0; |
maetugr | 12:67a06c9b69d5 | 53 | unsigned long dt_read_sensors = 0; |
maetugr | 12:67a06c9b69d5 | 54 | unsigned long time_read_sensors = 0; |
maetugr | 26:96a072233d7a | 55 | float tempangle = 0; // temporärer winkel für yaw mit kompass |
maetugr | 21:c2a2e7cbabdd | 56 | float controller_value[] = {0,0,0}; |
maetugr | 30:021e13b62575 | 57 | float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC |
maetugr | 21:c2a2e7cbabdd | 58 | |
maetugr | 28:ba6ca9f4def4 | 59 | void dutycycle() // method which is called by the Ticker Dutycycler every RATE seconds |
maetugr | 8:d25ecdcdbeb5 | 60 | { |
maetugr | 12:67a06c9b69d5 | 61 | time_read_sensors = GlobalTimer.read_us(); |
maetugr | 12:67a06c9b69d5 | 62 | |
maetugr | 15:753c5d6a63b3 | 63 | // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes |
maetugr | 14:cf260677ecde | 64 | Gyro.read(); |
maetugr | 15:753c5d6a63b3 | 65 | Acc.read(); // TODO: nicht jeder Sensor immer? höhe nicht so wichtig |
maetugr | 21:c2a2e7cbabdd | 66 | //Comp.read(); |
maetugr | 13:4737ee9ebfee | 67 | //Alt.Update(); TODO braucht zu lange zum auslesen! |
maetugr | 12:67a06c9b69d5 | 68 | |
maetugr | 12:67a06c9b69d5 | 69 | dt_read_sensors = GlobalTimer.read_us() - time_read_sensors; |
maetugr | 8:d25ecdcdbeb5 | 70 | |
maetugr | 8:d25ecdcdbeb5 | 71 | // meassure dt |
maetugr | 26:96a072233d7a | 72 | dt = GlobalTimer.read_us() - time_for_dt; // time in us since last loop |
maetugr | 26:96a072233d7a | 73 | time_for_dt = GlobalTimer.read_us(); // set new time for next measurement |
maetugr | 12:67a06c9b69d5 | 74 | |
maetugr | 26:96a072233d7a | 75 | IMU.compute(dt, Gyro.data, Acc.data); |
maetugr | 8:d25ecdcdbeb5 | 76 | |
maetugr | 21:c2a2e7cbabdd | 77 | // Arming / disarming |
maetugr | 29:8b7362a2ee14 | 78 | if(RC[3].read() < 20 && RC[2].read() > 850) { |
maetugr | 21:c2a2e7cbabdd | 79 | armed = true; |
maetugr | 25:0498d3041afa | 80 | #ifdef LOGGER |
maetugr | 25:0498d3041afa | 81 | if(Logger == NULL) |
maetugr | 25:0498d3041afa | 82 | Logger = fopen("/local/log.csv", "a"); |
maetugr | 25:0498d3041afa | 83 | #endif |
maetugr | 25:0498d3041afa | 84 | } |
maetugr | 29:8b7362a2ee14 | 85 | if((RC[3].read() < 30 && RC[2].read() < 30) || RC[2].read() < -10 || RC[3].read() < -10 || RC[1].read() < -10 || RC[0].read() < -10) { |
maetugr | 20:e116e596e540 | 86 | armed = false; |
maetugr | 25:0498d3041afa | 87 | #ifdef LOGGER |
maetugr | 25:0498d3041afa | 88 | if(Logger != NULL) { |
maetugr | 25:0498d3041afa | 89 | fclose(Logger); |
maetugr | 25:0498d3041afa | 90 | Logger = NULL; |
maetugr | 25:0498d3041afa | 91 | } |
maetugr | 25:0498d3041afa | 92 | #endif |
maetugr | 25:0498d3041afa | 93 | } |
maetugr | 20:e116e596e540 | 94 | |
maetugr | 30:021e13b62575 | 95 | for(int i=0;i<3;i++) // calculate new angle we want the QC to have |
maetugr | 30:021e13b62575 | 96 | RC_angle[i] = (RC[i].read()-500)*30/500.0; |
maetugr | 30:021e13b62575 | 97 | |
maetugr | 30:021e13b62575 | 98 | for(int i=0;i<3;i++) { |
maetugr | 29:8b7362a2ee14 | 99 | Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying |
maetugr | 30:021e13b62575 | 100 | 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 | 101 | } |
maetugr | 30:021e13b62575 | 102 | |
maetugr | 29:8b7362a2ee14 | 103 | |
maetugr | 21:c2a2e7cbabdd | 104 | if (armed) // for SECURITY! |
maetugr | 22:d301b455a1ad | 105 | { |
maetugr | 28:ba6ca9f4def4 | 106 | // RC controlling |
maetugr | 30:021e13b62575 | 107 | /*for(int i=0;i<3;i++) |
maetugr | 30:021e13b62575 | 108 | AnglePosition[i] -= (RC[i].read()-500)*2/500.0;*/ |
maetugr | 28:ba6ca9f4def4 | 109 | /*virt_angle[0] = IMU.angle[0] + (RC[0].read()-500)*MAXPITCH/500.0; // TODO: zuerst RC calibration |
maetugr | 28:ba6ca9f4def4 | 110 | virt_angle[1] = IMU.angle[1] + (RC[1].read()-500)*MAXPITCH/500.0; |
maetugr | 28:ba6ca9f4def4 | 111 | yawposition += (RC[3].read()-500)*YAWSPEED/500; |
maetugr | 28:ba6ca9f4def4 | 112 | virt_angle[2] = IMU.angle[2] + yawposition;*/ |
maetugr | 28:ba6ca9f4def4 | 113 | |
maetugr | 24:c5a3cba48498 | 114 | // PID controlling |
maetugr | 28:ba6ca9f4def4 | 115 | /*if (!(RC[0].read() == -100)) { // the RC must be there to controll // alte version mit setpoint, nicht nötig? granzen bei yaw los? :) |
maetugr | 28:ba6ca9f4def4 | 116 | Controller[0].setSetPoint(-((RC[0].read()-500)*MAXPITCH/500.0)); // set angles based on RC input |
maetugr | 28:ba6ca9f4def4 | 117 | Controller[1].setSetPoint(-((RC[1].read()-500)*MAXPITCH/500.0)); |
maetugr | 28:ba6ca9f4def4 | 118 | Controller[2].setSetPoint(-((RC[3].read()-500)*180.0/500.0)); |
maetugr | 28:ba6ca9f4def4 | 119 | }*/ |
maetugr | 30:021e13b62575 | 120 | |
maetugr | 28:ba6ca9f4def4 | 121 | |
maetugr | 29:8b7362a2ee14 | 122 | MIX.compute(dt, RC[3].read(), controller_value); // let the Mixer compute motorspeeds based on throttle and controller output |
maetugr | 28:ba6ca9f4def4 | 123 | |
maetugr | 28:ba6ca9f4def4 | 124 | for(int i=0;i<4;i++) // Set new motorspeeds |
maetugr | 26:96a072233d7a | 125 | ESC[i] = (int)MIX.Motor_speed[i]; |
maetugr | 25:0498d3041afa | 126 | |
maetugr | 25:0498d3041afa | 127 | #ifdef LOGGER |
maetugr | 25:0498d3041afa | 128 | // Writing Log |
maetugr | 25:0498d3041afa | 129 | for(int i = 0; i < 3; i++) { |
maetugr | 25:0498d3041afa | 130 | fprintf(Logger, "%f;", angle[i]); |
maetugr | 25:0498d3041afa | 131 | fprintf(Logger, "%f;", controller_value[i]); |
maetugr | 25:0498d3041afa | 132 | } |
maetugr | 25:0498d3041afa | 133 | fprintf(Logger, "\r\n"); |
maetugr | 25:0498d3041afa | 134 | #endif |
maetugr | 15:753c5d6a63b3 | 135 | } else { |
maetugr | 26:96a072233d7a | 136 | for(int i=0;i<4;i++) // for security reason, set every motor to zero speed |
maetugr | 28:ba6ca9f4def4 | 137 | ESC[i] = 0; |
maetugr | 21:c2a2e7cbabdd | 138 | } |
maetugr | 8:d25ecdcdbeb5 | 139 | } |
maetugr | 5:818c0668fd2d | 140 | |
maetugr | 26:96a072233d7a | 141 | int main() { // main programm for initialisation and debug output |
maetugr | 26:96a072233d7a | 142 | 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 | 143 | |
maetugr | 25:0498d3041afa | 144 | #ifdef LOGGER |
maetugr | 25:0498d3041afa | 145 | Logger = fopen("/local/log.csv", "w"); // Prepare Logfile |
maetugr | 25:0498d3041afa | 146 | for(int i = 0; i < 3; i++) { |
maetugr | 25:0498d3041afa | 147 | fprintf(Logger, "angle[%d];", i); |
maetugr | 25:0498d3041afa | 148 | fprintf(Logger, "controller_value[%d];", i); |
maetugr | 25:0498d3041afa | 149 | } |
maetugr | 25:0498d3041afa | 150 | fprintf(Logger, "\r\n"); |
maetugr | 25:0498d3041afa | 151 | fclose(Logger); |
maetugr | 25:0498d3041afa | 152 | Logger = NULL; |
maetugr | 25:0498d3041afa | 153 | #endif |
maetugr | 25:0498d3041afa | 154 | |
maetugr | 21:c2a2e7cbabdd | 155 | #ifdef PC_CONNECTED |
maetugr | 21:c2a2e7cbabdd | 156 | #ifdef COMPASSCALIBRATE |
maetugr | 21:c2a2e7cbabdd | 157 | pc.locate(10,5); |
maetugr | 21:c2a2e7cbabdd | 158 | pc.printf("CALIBRATING"); |
maetugr | 21:c2a2e7cbabdd | 159 | Comp.calibrate(60); |
maetugr | 21:c2a2e7cbabdd | 160 | #endif |
maetugr | 21:c2a2e7cbabdd | 161 | |
maetugr | 21:c2a2e7cbabdd | 162 | // init screen |
maetugr | 12:67a06c9b69d5 | 163 | pc.locate(10,5); |
maetugr | 21:c2a2e7cbabdd | 164 | pc.printf("Flybed v0.2"); |
maetugr | 12:67a06c9b69d5 | 165 | #endif |
maetugr | 1:5a64632b1eb9 | 166 | LEDs.roll(2); |
maetugr | 5:818c0668fd2d | 167 | |
maetugr | 21:c2a2e7cbabdd | 168 | // Start! |
maetugr | 2:93f703d2c4d7 | 169 | GlobalTimer.start(); |
maetugr | 28:ba6ca9f4def4 | 170 | Dutycycler.attach(&dutycycle, RATE); // start to process all RATEms |
maetugr | 12:67a06c9b69d5 | 171 | |
maetugr | 12:67a06c9b69d5 | 172 | while(1) { |
maetugr | 30:021e13b62575 | 173 | //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]); // serialplot of IMU |
maetugr | 30:021e13b62575 | 174 | #if 1 //pc.cls(); |
maetugr | 21:c2a2e7cbabdd | 175 | pc.locate(30,0); // PC output |
maetugr | 30:021e13b62575 | 176 | pc.printf("dt:%3.3fms dt_sensors:%dus Altitude:%6.1fm ", dt/1000.0, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure)); |
maetugr | 21:c2a2e7cbabdd | 177 | pc.locate(5,1); |
maetugr | 21:c2a2e7cbabdd | 178 | if(armed) |
maetugr | 21:c2a2e7cbabdd | 179 | pc.printf("ARMED!!!!!!!!!!!!!"); |
maetugr | 21:c2a2e7cbabdd | 180 | else |
maetugr | 21:c2a2e7cbabdd | 181 | pc.printf("DIS_ARMED "); |
maetugr | 21:c2a2e7cbabdd | 182 | pc.locate(5,3); |
maetugr | 26:96a072233d7a | 183 | pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", IMU.angle[0], IMU.angle[1], IMU.angle[2]); |
maetugr | 21:c2a2e7cbabdd | 184 | pc.locate(5,5); |
maetugr | 21:c2a2e7cbabdd | 185 | pc.printf("Gyro.data: X:%6.1f Y:%6.1f Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]); |
maetugr | 30:021e13b62575 | 186 | pc.locate(5,6); |
maetugr | 21:c2a2e7cbabdd | 187 | pc.printf("Acc.data: X:%6d Y:%6d Z:%6d", Acc.data[0], Acc.data[1], Acc.data[2]); |
maetugr | 21:c2a2e7cbabdd | 188 | pc.locate(5,11); |
maetugr | 21:c2a2e7cbabdd | 189 | pc.printf("PID Result:"); |
maetugr | 21:c2a2e7cbabdd | 190 | for(int i=0;i<3;i++) |
maetugr | 21:c2a2e7cbabdd | 191 | pc.printf(" %d: %6.1f", i, controller_value[i]); |
maetugr | 23:955a7c7ddf8b | 192 | pc.locate(5,14); |
maetugr | 30:021e13b62575 | 193 | pc.printf("RC angle: roll: %f pitch: %f yaw: %f ", RC_angle[0], RC_angle[1], RC_angle[2]); |
maetugr | 29:8b7362a2ee14 | 194 | pc.locate(5,16); |
maetugr | 29:8b7362a2ee14 | 195 | 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 | 23:955a7c7ddf8b | 196 | |
maetugr | 29:8b7362a2ee14 | 197 | // RC |
maetugr | 21:c2a2e7cbabdd | 198 | pc.locate(10,19); |
maetugr | 29:8b7362a2ee14 | 199 | pc.printf("RC0: %4d ", RC[0].read()); |
maetugr | 29:8b7362a2ee14 | 200 | pc.printf("RC1: %4d ", RC[1].read()); |
maetugr | 29:8b7362a2ee14 | 201 | pc.printf("RC2: %4d ", RC[2].read()); |
maetugr | 29:8b7362a2ee14 | 202 | pc.printf("RC3: %4d ", RC[3].read()); |
maetugr | 21:c2a2e7cbabdd | 203 | #endif |
maetugr | 21:c2a2e7cbabdd | 204 | if(armed){ |
maetugr | 21:c2a2e7cbabdd | 205 | LEDs.rollnext(); |
maetugr | 21:c2a2e7cbabdd | 206 | } else { |
maetugr | 26:96a072233d7a | 207 | for(int i=1;i<=4;i++) |
maetugr | 26:96a072233d7a | 208 | LEDs.set(i); |
maetugr | 21:c2a2e7cbabdd | 209 | } |
maetugr | 29:8b7362a2ee14 | 210 | wait(0.05); |
maetugr | 0:0c4fafa398b4 | 211 | } |
maetugr | 28:ba6ca9f4def4 | 212 | } |