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@29:8b7362a2ee14, 2012-12-15 (annotated)

Committer:
maetugr
Date:
Sat Dec 15 08:42:36 2012 +0000
Revision:
29:8b7362a2ee14
Parent:
28:ba6ca9f4def4
Child:
30:021e13b62575
gyro only version running ESCs on 490Hz now

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 7:9d4313510646 8 #include "RC_Channel.h" // RemoteControl Chnnels 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 15:753c5d6a63b3 14 #define RATE 0.02 // speed of the interrupt for Sensors and PID
maetugr 29:8b7362a2ee14 15 #define PPM_FREQU 490 // 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 29:8b7362a2ee14 32 PC pc(USBTX, USBRX, 38400); // 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 25:0498d3041afa 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 27:9e546fa47c33 41 RC_Channel RC[] = {RC_Channel(p11,1), RC_Channel(p12,2), RC_Channel(p13,3), RC_Channel(p14,4)}; // 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 26:96a072233d7a 44 Mixer MIX;
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 29:8b7362a2ee14 57 float AnglePosition[] = {0,0,0};
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 29:8b7362a2ee14 80 for(int i=0;i<3;i++)
maetugr 29:8b7362a2ee14 81 AnglePosition[i] = IMU.angle[i];
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 29:8b7362a2ee14 97 for(int i=0;i<3;i++)
maetugr 29:8b7362a2ee14 98 Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
maetugr 29:8b7362a2ee14 99
maetugr 21:c2a2e7cbabdd 100 if (armed) // for SECURITY!
maetugr 22:d301b455a1ad 101 {
maetugr 28:ba6ca9f4def4 102 // RC controlling
maetugr 29:8b7362a2ee14 103 for(int i=0;i<3;i++)
maetugr 29:8b7362a2ee14 104 AnglePosition[i] -= (RC[i].read()-500)*2/500.0;
maetugr 28:ba6ca9f4def4 105 /*virt_angle[0] = IMU.angle[0] + (RC[0].read()-500)*MAXPITCH/500.0; // TODO: zuerst RC calibration
maetugr 28:ba6ca9f4def4 106 virt_angle[1] = IMU.angle[1] + (RC[1].read()-500)*MAXPITCH/500.0;
maetugr 28:ba6ca9f4def4 107 yawposition += (RC[3].read()-500)*YAWSPEED/500;
maetugr 28:ba6ca9f4def4 108 virt_angle[2] = IMU.angle[2] + yawposition;*/
maetugr 28:ba6ca9f4def4 109
maetugr 24:c5a3cba48498 110 // PID controlling
maetugr 28:ba6ca9f4def4 111 /*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 112 Controller[0].setSetPoint(-((RC[0].read()-500)*MAXPITCH/500.0)); // set angles based on RC input
maetugr 28:ba6ca9f4def4 113 Controller[1].setSetPoint(-((RC[1].read()-500)*MAXPITCH/500.0));
maetugr 28:ba6ca9f4def4 114 Controller[2].setSetPoint(-((RC[3].read()-500)*180.0/500.0));
maetugr 28:ba6ca9f4def4 115 }*/
maetugr 29:8b7362a2ee14 116 for(int i=0;i<3;i++)
maetugr 29:8b7362a2ee14 117 controller_value[i] = Controller[i].compute(AnglePosition[i], IMU.angle[i]); // gove the controller the actual angle and get his advice to correct
maetugr 28:ba6ca9f4def4 118
maetugr 29:8b7362a2ee14 119 MIX.compute(dt, RC[3].read(), controller_value); // let the Mixer compute motorspeeds based on throttle and controller output
maetugr 28:ba6ca9f4def4 120
maetugr 28:ba6ca9f4def4 121 for(int i=0;i<4;i++) // Set new motorspeeds
maetugr 26:96a072233d7a 122 ESC[i] = (int)MIX.Motor_speed[i];
maetugr 25:0498d3041afa 123
maetugr 25:0498d3041afa 124 #ifdef LOGGER
maetugr 25:0498d3041afa 125 // Writing Log
maetugr 25:0498d3041afa 126 for(int i = 0; i < 3; i++) {
maetugr 25:0498d3041afa 127 fprintf(Logger, "%f;", angle[i]);
maetugr 25:0498d3041afa 128 fprintf(Logger, "%f;", controller_value[i]);
maetugr 25:0498d3041afa 129 }
maetugr 25:0498d3041afa 130 fprintf(Logger, "\r\n");
maetugr 25:0498d3041afa 131 #endif
maetugr 15:753c5d6a63b3 132 } else {
maetugr 26:96a072233d7a 133 for(int i=0;i<4;i++) // for security reason, set every motor to zero speed
maetugr 28:ba6ca9f4def4 134 ESC[i] = 0;
maetugr 21:c2a2e7cbabdd 135 }
maetugr 8:d25ecdcdbeb5 136 }
maetugr 5:818c0668fd2d 137
maetugr 26:96a072233d7a 138 int main() { // main programm for initialisation and debug output
maetugr 26:96a072233d7a 139 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 140
maetugr 25:0498d3041afa 141 #ifdef LOGGER
maetugr 25:0498d3041afa 142 Logger = fopen("/local/log.csv", "w"); // Prepare Logfile
maetugr 25:0498d3041afa 143 for(int i = 0; i < 3; i++) {
maetugr 25:0498d3041afa 144 fprintf(Logger, "angle[%d];", i);
maetugr 25:0498d3041afa 145 fprintf(Logger, "controller_value[%d];", i);
maetugr 25:0498d3041afa 146 }
maetugr 25:0498d3041afa 147 fprintf(Logger, "\r\n");
maetugr 25:0498d3041afa 148 fclose(Logger);
maetugr 25:0498d3041afa 149 Logger = NULL;
maetugr 25:0498d3041afa 150 #endif
maetugr 25:0498d3041afa 151
maetugr 26:96a072233d7a 152 //Controller[2].setInputLimits(-180.0, 180.0); // yaw 360 grad TODO: Yawsteuerung mit -180 bis 180 grad
maetugr 12:67a06c9b69d5 153
maetugr 21:c2a2e7cbabdd 154 #ifdef PC_CONNECTED
maetugr 21:c2a2e7cbabdd 155 #ifdef COMPASSCALIBRATE
maetugr 21:c2a2e7cbabdd 156 pc.locate(10,5);
maetugr 21:c2a2e7cbabdd 157 pc.printf("CALIBRATING");
maetugr 21:c2a2e7cbabdd 158 Comp.calibrate(60);
maetugr 21:c2a2e7cbabdd 159 #endif
maetugr 21:c2a2e7cbabdd 160
maetugr 21:c2a2e7cbabdd 161 // init screen
maetugr 12:67a06c9b69d5 162 pc.locate(10,5);
maetugr 21:c2a2e7cbabdd 163 pc.printf("Flybed v0.2");
maetugr 12:67a06c9b69d5 164 #endif
maetugr 1:5a64632b1eb9 165 LEDs.roll(2);
maetugr 5:818c0668fd2d 166
maetugr 21:c2a2e7cbabdd 167 // Start!
maetugr 2:93f703d2c4d7 168 GlobalTimer.start();
maetugr 28:ba6ca9f4def4 169 Dutycycler.attach(&dutycycle, RATE); // start to process all RATEms
maetugr 12:67a06c9b69d5 170
maetugr 29:8b7362a2ee14 171 int count = 0;
maetugr 12:67a06c9b69d5 172 while(1) {
maetugr 29:8b7362a2ee14 173 //pc.printf("%6.1f,%6.1f,%6.1f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2]);
maetugr 29:8b7362a2ee14 174 #if 1
maetugr 29:8b7362a2ee14 175 if(count == 20){
maetugr 29:8b7362a2ee14 176 //pc.cls();
maetugr 29:8b7362a2ee14 177 count = 0;
maetugr 29:8b7362a2ee14 178 }
maetugr 21:c2a2e7cbabdd 179 pc.locate(30,0); // PC output
maetugr 26:96a072233d7a 180 pc.printf("dt:%dms dt_sensors:%dus Altitude:%6.1fm ", dt/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
maetugr 21:c2a2e7cbabdd 181 pc.locate(5,1);
maetugr 21:c2a2e7cbabdd 182 if(armed)
maetugr 21:c2a2e7cbabdd 183 pc.printf("ARMED!!!!!!!!!!!!!");
maetugr 21:c2a2e7cbabdd 184 else
maetugr 21:c2a2e7cbabdd 185 pc.printf("DIS_ARMED ");
maetugr 21:c2a2e7cbabdd 186 pc.locate(5,3);
maetugr 26:96a072233d7a 187 pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", IMU.angle[0], IMU.angle[1], IMU.angle[2]);
maetugr 21:c2a2e7cbabdd 188 pc.locate(5,5);
maetugr 21:c2a2e7cbabdd 189 pc.printf("Gyro.data: X:%6.1f Y:%6.1f Z:%6.1f", Gyro.data[0], Gyro.data[1], Gyro.data[2]);
maetugr 21:c2a2e7cbabdd 190 pc.locate(5,8);
maetugr 21:c2a2e7cbabdd 191 pc.printf("Acc.data: X:%6d Y:%6d Z:%6d", Acc.data[0], Acc.data[1], Acc.data[2]);
maetugr 21:c2a2e7cbabdd 192 pc.locate(5,11);
maetugr 21:c2a2e7cbabdd 193 pc.printf("PID Result:");
maetugr 21:c2a2e7cbabdd 194 for(int i=0;i<3;i++)
maetugr 21:c2a2e7cbabdd 195 pc.printf(" %d: %6.1f", i, controller_value[i]);
maetugr 23:955a7c7ddf8b 196 pc.locate(5,14);
maetugr 29:8b7362a2ee14 197 pc.printf("RC controll: roll: %f pitch: %f yaw: %f ", AnglePosition[0], AnglePosition[1], AnglePosition[2]);
maetugr 29:8b7362a2ee14 198 pc.locate(5,16);
maetugr 29:8b7362a2ee14 199 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 200
maetugr 29:8b7362a2ee14 201 // RC
maetugr 21:c2a2e7cbabdd 202 pc.locate(10,19);
maetugr 29:8b7362a2ee14 203 pc.printf("RC0: %4d ", RC[0].read());
maetugr 29:8b7362a2ee14 204 pc.printf("RC1: %4d ", RC[1].read());
maetugr 29:8b7362a2ee14 205 pc.printf("RC2: %4d ", RC[2].read());
maetugr 29:8b7362a2ee14 206 pc.printf("RC3: %4d ", RC[3].read());
maetugr 21:c2a2e7cbabdd 207 #endif
maetugr 21:c2a2e7cbabdd 208 if(armed){
maetugr 21:c2a2e7cbabdd 209 LEDs.rollnext();
maetugr 21:c2a2e7cbabdd 210 } else {
maetugr 26:96a072233d7a 211 for(int i=1;i<=4;i++)
maetugr 26:96a072233d7a 212 LEDs.set(i);
maetugr 21:c2a2e7cbabdd 213 }
maetugr 29:8b7362a2ee14 214 wait(0.05);
maetugr 29:8b7362a2ee14 215 count++;
maetugr 0:0c4fafa398b4 216 }
maetugr 28:ba6ca9f4def4 217 }