Matthias Grob / Mbed 2 deprecated FlyBed1

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.

Dependencies:   mbed MODI2C

main.cpp@33:fd98776b6cc7, 2013-04-04 (annotated)

Committer:
maetugr
Date:
Thu Apr 04 14:25:21 2013 +0000
Revision:
33:fd98776b6cc7
Parent:
32:e2e02338805e
Child:
34:3aa1cbcde59d
New version developed in eastern holidays, ported Madgwick Filter, added support for chaning PID values while flying over bluetooth, still not flying stable or even controllable

Who changed what in which revision?

UserRevisionLine numberNew 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 28:ba6ca9f4def4 17 #define YAWSPEED 2 // maximal speed of yaw rotation in degree per Rate
maetugr 15:753c5d6a63b3 18
maetugr 33:fd98776b6cc7 19 float P = 1.1; // PID values
maetugr 33:fd98776b6cc7 20 float I = 0.3;
maetugr 33:fd98776b6cc7 21 float D = 0.8;
maetugr 25:0498d3041afa 22
maetugr 29:8b7362a2ee14 23 #define PC_CONNECTED // decoment if you want to debug per USB/Bluetooth and your PC
maetugr 2:93f703d2c4d7 24
maetugr 14:cf260677ecde 25 Timer GlobalTimer; // global time to calculate processing speed
maetugr 28:ba6ca9f4def4 26 Ticker Dutycycler; // timecontrolled interrupt to get data form IMU and RC
maetugr 14:cf260677ecde 27
maetugr 14:cf260677ecde 28 // initialisation of hardware (see includes for more info)
maetugr 5:818c0668fd2d 29 LED LEDs;
maetugr 21:c2a2e7cbabdd 30 #ifdef PC_CONNECTED
maetugr 32:e2e02338805e 31 //PC pc(USBTX, USBRX, 115200); // USB
maetugr 33:fd98776b6cc7 32 PC pc(p9, p10, 115200); // Bluetooth
maetugr 21:c2a2e7cbabdd 33 #endif
maetugr 5:818c0668fd2d 34 L3G4200D Gyro(p28, p27);
maetugr 5:818c0668fd2d 35 ADXL345 Acc(p28, p27);
maetugr 11:9bf69bc6df45 36 HMC5883 Comp(p28, p27);
maetugr 14:cf260677ecde 37 BMP085_old Alt(p28, p27);
maetugr 30:021e13b62575 38 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 39 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 40 IMU_Filter IMU; // don't write () after constructor for no arguments!
maetugr 33:fd98776b6cc7 41 Mixer MIX(1); // 1 for X-Formation
maetugr 8:d25ecdcdbeb5 42
maetugr 21:c2a2e7cbabdd 43 // 0:X:Roll 1:Y:Pitch 2:Z:Yaw
maetugr 33:fd98776b6cc7 44 PID Controller[] = {PID(P, I, D, 1000), PID(P, I, D, 1000), PID(0.5, 0.01, 0, 1000)};
maetugr 14:cf260677ecde 45
maetugr 22:d301b455a1ad 46 // global variables
maetugr 33:fd98776b6cc7 47 bool armed = false; // this variable is for security (when false no motor rotates any more)
maetugr 33:fd98776b6cc7 48 float dt = 0;
maetugr 33:fd98776b6cc7 49 float time_for_dt = 0;
maetugr 33:fd98776b6cc7 50 float dt_read_sensors = 0;
maetugr 33:fd98776b6cc7 51 float time_read_sensors = 0;
maetugr 33:fd98776b6cc7 52 float controller_value[] = {0,0,0}; // The calculated answer form the Controller
maetugr 33:fd98776b6cc7 53 float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC
maetugr 21:c2a2e7cbabdd 54
maetugr 28:ba6ca9f4def4 55 void dutycycle() // method which is called by the Ticker Dutycycler every RATE seconds
maetugr 8:d25ecdcdbeb5 56 {
maetugr 33:fd98776b6cc7 57 time_read_sensors = GlobalTimer.read(); // start time measure for sensors
maetugr 12:67a06c9b69d5 58
maetugr 15:753c5d6a63b3 59 // read data from sensors // ATTENTION! the I2C option repeated true is important because otherwise interrupts while bus communications cause crashes
maetugr 14:cf260677ecde 60 Gyro.read();
maetugr 15:753c5d6a63b3 61 Acc.read(); // TODO: nicht jeder Sensor immer? höhe nicht so wichtig
maetugr 21:c2a2e7cbabdd 62 //Comp.read();
maetugr 13:4737ee9ebfee 63 //Alt.Update(); TODO braucht zu lange zum auslesen!
maetugr 12:67a06c9b69d5 64
maetugr 33:fd98776b6cc7 65 dt_read_sensors = GlobalTimer.read() - time_read_sensors; // stop time measure for sensors
maetugr 8:d25ecdcdbeb5 66
maetugr 33:fd98776b6cc7 67 // meassure dt for the filter
maetugr 33:fd98776b6cc7 68 dt = GlobalTimer.read() - time_for_dt; // time in us since last loop
maetugr 33:fd98776b6cc7 69 time_for_dt = GlobalTimer.read(); // set new time for next measurement
maetugr 12:67a06c9b69d5 70
maetugr 26:96a072233d7a 71 IMU.compute(dt, Gyro.data, Acc.data);
maetugr 8:d25ecdcdbeb5 72
maetugr 21:c2a2e7cbabdd 73 // Arming / disarming
maetugr 29:8b7362a2ee14 74 if(RC[3].read() < 20 && RC[2].read() > 850) {
maetugr 21:c2a2e7cbabdd 75 armed = true;
maetugr 25:0498d3041afa 76 }
maetugr 29:8b7362a2ee14 77 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 78 armed = false;
maetugr 25:0498d3041afa 79 }
maetugr 20:e116e596e540 80
maetugr 33:fd98776b6cc7 81 for(int i=0;i<2;i++) { // calculate new angle we want the QC to have
maetugr 31:872d8b8c7812 82 RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0;
maetugr 33:fd98776b6cc7 83 if (RC_angle[i] < -RC_SENSITIVITY-2)
maetugr 33:fd98776b6cc7 84 RC_angle[i] = 0;
maetugr 33:fd98776b6cc7 85 }
maetugr 33:fd98776b6cc7 86 //RC_angle[2] += (RC[3].read()-500)*YAWSPEED/500; // for yaw angle is integrated
maetugr 30:021e13b62575 87
maetugr 30:021e13b62575 88 for(int i=0;i<3;i++) {
maetugr 29:8b7362a2ee14 89 Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 30:021e13b62575 90 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 91 }
maetugr 30:021e13b62575 92
maetugr 29:8b7362a2ee14 93
maetugr 21:c2a2e7cbabdd 94 if (armed) // for SECURITY!
maetugr 22:d301b455a1ad 95 {
maetugr 28:ba6ca9f4def4 96 // RC controlling
maetugr 30:021e13b62575 97 /*for(int i=0;i<3;i++)
maetugr 30:021e13b62575 98 AnglePosition[i] -= (RC[i].read()-500)*2/500.0;*/
maetugr 28:ba6ca9f4def4 99 /*virt_angle[0] = IMU.angle[0] + (RC[0].read()-500)*MAXPITCH/500.0; // TODO: zuerst RC calibration
maetugr 28:ba6ca9f4def4 100 virt_angle[1] = IMU.angle[1] + (RC[1].read()-500)*MAXPITCH/500.0;
maetugr 28:ba6ca9f4def4 101 yawposition += (RC[3].read()-500)*YAWSPEED/500;
maetugr 28:ba6ca9f4def4 102 virt_angle[2] = IMU.angle[2] + yawposition;*/
maetugr 33:fd98776b6cc7 103
maetugr 33:fd98776b6cc7 104 MIX.compute(RC[3].read(), controller_value); // let the Mixer compute motorspeeds based on throttle and controller output
maetugr 28:ba6ca9f4def4 105
maetugr 28:ba6ca9f4def4 106 for(int i=0;i<4;i++) // Set new motorspeeds
maetugr 26:96a072233d7a 107 ESC[i] = (int)MIX.Motor_speed[i];
maetugr 25:0498d3041afa 108
maetugr 15:753c5d6a63b3 109 } else {
maetugr 26:96a072233d7a 110 for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
maetugr 28:ba6ca9f4def4 111 ESC[i] = 0;
maetugr 21:c2a2e7cbabdd 112 }
maetugr 8:d25ecdcdbeb5 113 }
maetugr 5:818c0668fd2d 114
maetugr 33:fd98776b6cc7 115 void commandexecuter(char* command) { // take new PID values on the fly
maetugr 31:872d8b8c7812 116 if (command[0] == 'p')
maetugr 33:fd98776b6cc7 117 P = atof(&command[1]);
maetugr 31:872d8b8c7812 118 if (command[0] == 'i')
maetugr 33:fd98776b6cc7 119 I = atof(&command[1]);
maetugr 31:872d8b8c7812 120 if (command[0] == 'd')
maetugr 33:fd98776b6cc7 121 D = atof(&command[1]);
maetugr 33:fd98776b6cc7 122 for(int i=0;i<2;i++) {
maetugr 33:fd98776b6cc7 123 Controller[i].setPID(P,I,D); // give the controller the new PID values
maetugr 31:872d8b8c7812 124 }
maetugr 31:872d8b8c7812 125 }
maetugr 31:872d8b8c7812 126
maetugr 26:96a072233d7a 127 int main() { // main programm for initialisation and debug output
maetugr 26:96a072233d7a 128 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 129
maetugr 21:c2a2e7cbabdd 130 #ifdef PC_CONNECTED
maetugr 21:c2a2e7cbabdd 131 // init screen
maetugr 12:67a06c9b69d5 132 pc.locate(10,5);
maetugr 21:c2a2e7cbabdd 133 pc.printf("Flybed v0.2");
maetugr 12:67a06c9b69d5 134 #endif
maetugr 1:5a64632b1eb9 135 LEDs.roll(2);
maetugr 5:818c0668fd2d 136
maetugr 33:fd98776b6cc7 137 Gyro.calibrate(50, 0.02);
maetugr 33:fd98776b6cc7 138 Acc.calibrate(50, 0.02);
maetugr 33:fd98776b6cc7 139
maetugr 21:c2a2e7cbabdd 140 // Start!
maetugr 2:93f703d2c4d7 141 GlobalTimer.start();
maetugr 28:ba6ca9f4def4 142 Dutycycler.attach(&dutycycle, RATE); // start to process all RATEms
maetugr 12:67a06c9b69d5 143
maetugr 12:67a06c9b69d5 144 while(1) {
maetugr 33:fd98776b6cc7 145 if (pc.readable()) // Get Serial input (polled because interrupts disturb I2C)
maetugr 33:fd98776b6cc7 146 pc.readcommand(&commandexecuter);
maetugr 33:fd98776b6cc7 147 //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 30:021e13b62575 148 #if 1 //pc.cls();
maetugr 33:fd98776b6cc7 149 pc.locate(20,0); // PC output
maetugr 33:fd98776b6cc7 150 pc.printf("dt:%3.5fs dt_sensors:%3.5fs Altitude:%6.1fm ", dt, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
maetugr 21:c2a2e7cbabdd 151 pc.locate(5,1);
maetugr 21:c2a2e7cbabdd 152 if(armed)
maetugr 21:c2a2e7cbabdd 153 pc.printf("ARMED!!!!!!!!!!!!!");
maetugr 21:c2a2e7cbabdd 154 else
maetugr 21:c2a2e7cbabdd 155 pc.printf("DIS_ARMED ");
maetugr 21:c2a2e7cbabdd 156 pc.locate(5,3);
maetugr 26:96a072233d7a 157 pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", IMU.angle[0], IMU.angle[1], IMU.angle[2]);
maetugr 31:872d8b8c7812 158 pc.locate(5,4);
maetugr 33:fd98776b6cc7 159 pc.printf("q0:%6.1f q1:%6.1f q2:%6.1f q3:%6.1f ", IMU.q0, IMU.q1, IMU.q2, IMU.q3);
maetugr 21:c2a2e7cbabdd 160 pc.locate(5,5);
maetugr 21:c2a2e7cbabdd 161 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 162 pc.locate(5,6);
maetugr 33:fd98776b6cc7 163 pc.printf("Acc.data: X:%6.1f Y:%6.1f Z:%6.1f", Acc.data[0], Acc.data[1], Acc.data[2]);
maetugr 33:fd98776b6cc7 164
maetugr 33:fd98776b6cc7 165 pc.locate(5,8);
maetugr 33:fd98776b6cc7 166 pc.printf("P:%6.1f I:%6.1f D:%6.1f ", P, I, D);
maetugr 33:fd98776b6cc7 167
maetugr 21:c2a2e7cbabdd 168 pc.locate(5,11);
maetugr 21:c2a2e7cbabdd 169 pc.printf("PID Result:");
maetugr 21:c2a2e7cbabdd 170 for(int i=0;i<3;i++)
maetugr 21:c2a2e7cbabdd 171 pc.printf(" %d: %6.1f", i, controller_value[i]);
maetugr 23:955a7c7ddf8b 172 pc.locate(5,14);
maetugr 30:021e13b62575 173 pc.printf("RC angle: roll: %f pitch: %f yaw: %f ", RC_angle[0], RC_angle[1], RC_angle[2]);
maetugr 29:8b7362a2ee14 174 pc.locate(5,16);
maetugr 29:8b7362a2ee14 175 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 176
maetugr 29:8b7362a2ee14 177 // RC
maetugr 21:c2a2e7cbabdd 178 pc.locate(10,19);
maetugr 33:fd98776b6cc7 179 pc.printf("RC0: %4d RC1: %4d RC2: %4d RC3: %4d ", RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
maetugr 31:872d8b8c7812 180
maetugr 31:872d8b8c7812 181 pc.locate(10,21);
maetugr 33:fd98776b6cc7 182 pc.printf("Commandline: %s ", pc.command);
maetugr 21:c2a2e7cbabdd 183 #endif
maetugr 21:c2a2e7cbabdd 184 if(armed){
maetugr 21:c2a2e7cbabdd 185 LEDs.rollnext();
maetugr 21:c2a2e7cbabdd 186 } else {
maetugr 26:96a072233d7a 187 for(int i=1;i<=4;i++)
maetugr 26:96a072233d7a 188 LEDs.set(i);
maetugr 21:c2a2e7cbabdd 189 }
maetugr 29:8b7362a2ee14 190 wait(0.05);
maetugr 0:0c4fafa398b4 191 }
maetugr 28:ba6ca9f4def4 192 }