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@31:872d8b8c7812, 2013-03-30 (annotated)

Committer:
maetugr
Date:
Sat Mar 30 09:17:44 2013 +0000
Revision:
31:872d8b8c7812
Parent:
30:021e13b62575
Child:
32:e2e02338805e
new goals for the eastern holidays

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