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@13:4737ee9ebfee, 2012-10-25 (annotated)

Committer:
maetugr
Date:
Thu Oct 25 19:27:56 2012 +0000
Revision:
13:4737ee9ebfee
Parent:
12:67a06c9b69d5
Child:
14:cf260677ecde
Vor wechsel Comp -> Mag

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 7:9d4313510646 7 #include "BMP085.h" // Alt (Altitude sensor)
maetugr 7:9d4313510646 8 #include "RC_Channel.h" // RemoteControl Chnnels with PPM
maetugr 7:9d4313510646 9 #include "Servo.h" // Motor PPM
maetugr 13:4737ee9ebfee 10 #include "PID.h" // PID Library by Aaron Berk
maetugr 13:4737ee9ebfee 11 #include "IntCtrl.h" // Interrupt Control by Roland Elmiger
maetugr 0:0c4fafa398b4 12
maetugr 10:953afcbcebfc 13 #define PI 3.1415926535897932384626433832795
maetugr 10:953afcbcebfc 14 #define Rad2Deg 57.295779513082320876798154814105
maetugr 10:953afcbcebfc 15 #define RATE 0.02 // speed of Ticker/PID
maetugr 8:d25ecdcdbeb5 16
maetugr 13:4737ee9ebfee 17 //#define COMPASSCALIBRATE decomment if you want to calibrate the Compass on start
maetugr 12:67a06c9b69d5 18
maetugr 8:d25ecdcdbeb5 19 Timer GlobalTimer; // global time to calculate processing speed
maetugr 8:d25ecdcdbeb5 20 Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC
maetugr 10:953afcbcebfc 21 PID controller(1.0, 0.0, 0.0, RATE); // test PID controller for throttle
maetugr 10:953afcbcebfc 22 PID pid(1.0, 1.0, 0.0, RATE); // test PID controller for throttle
maetugr 13:4737ee9ebfee 23 //PID P:3,0 bis 3,5 I:0,010 und 0,050 D:5 und 25
maetugr 2:93f703d2c4d7 24
maetugr 5:818c0668fd2d 25 // initialisation of hardware
maetugr 5:818c0668fd2d 26 LED LEDs;
maetugr 10:953afcbcebfc 27 PC pc(USBTX, USBRX, 57600);
maetugr 5:818c0668fd2d 28 L3G4200D Gyro(p28, p27);
maetugr 5:818c0668fd2d 29 ADXL345 Acc(p28, p27);
maetugr 11:9bf69bc6df45 30 HMC5883 Comp(p28, p27);
maetugr 5:818c0668fd2d 31 BMP085 Alt(p28, p27);
maetugr 13:4737ee9ebfee 32 RC_Channel RC[] = {(p11), (p12), (p13), (p14)}; // noooo p19/p20!!!!
maetugr 10:953afcbcebfc 33 Servo Motor[] = {(p15), (p16), (p17), (p18)};
maetugr 8:d25ecdcdbeb5 34
maetugr 8:d25ecdcdbeb5 35 // variables for loop
maetugr 12:67a06c9b69d5 36 unsigned long dt_get_data = 0;
maetugr 12:67a06c9b69d5 37 unsigned long time_get_data = 0;
maetugr 12:67a06c9b69d5 38 unsigned long dt_read_sensors = 0;
maetugr 12:67a06c9b69d5 39 unsigned long time_read_sensors = 0;
maetugr 11:9bf69bc6df45 40 float angle[3] = {0,0,0}; // angle 0: x,roll / 1: y,pitch / 2: z,yaw
maetugr 8:d25ecdcdbeb5 41 float tempangle = 0;
maetugr 10:953afcbcebfc 42 int Motorvalue[3];
maetugr 10:953afcbcebfc 43
maetugr 10:953afcbcebfc 44 float pidtester;
maetugr 8:d25ecdcdbeb5 45
maetugr 8:d25ecdcdbeb5 46 void get_Data()
maetugr 8:d25ecdcdbeb5 47 {
maetugr 12:67a06c9b69d5 48 time_read_sensors = GlobalTimer.read_us();
maetugr 12:67a06c9b69d5 49
maetugr 13:4737ee9ebfee 50 GPIO_IntDisable(0, 18, 2); // abschalten der Pins 11-14 mit Göttis library
maetugr 13:4737ee9ebfee 51 GPIO_IntDisable(0, 17, 2);
maetugr 13:4737ee9ebfee 52 GPIO_IntDisable(0, 16, 2);
maetugr 13:4737ee9ebfee 53 GPIO_IntDisable(0, 15, 2);
maetugr 13:4737ee9ebfee 54 //__disable_irq(); // test, deactivate all interrupts, I2C working?
maetugr 8:d25ecdcdbeb5 55 // read data from sensors
maetugr 13:4737ee9ebfee 56 Gyro.read(); // einzeln testen! mit LEDs
maetugr 10:953afcbcebfc 57 Acc.read();
maetugr 11:9bf69bc6df45 58 Comp.read();
maetugr 13:4737ee9ebfee 59 //Alt.Update(); TODO braucht zu lange zum auslesen!
maetugr 13:4737ee9ebfee 60 //__enable_irq();
maetugr 13:4737ee9ebfee 61 GPIO_IntEnable(0, 18, 2); // schaltet enable wirklich wieder ein?? änderungsbedarf??
maetugr 13:4737ee9ebfee 62 GPIO_IntEnable(0, 17, 2);
maetugr 13:4737ee9ebfee 63 GPIO_IntEnable(0, 16, 2);
maetugr 13:4737ee9ebfee 64 GPIO_IntEnable(0, 15, 2);
maetugr 12:67a06c9b69d5 65
maetugr 12:67a06c9b69d5 66 dt_read_sensors = GlobalTimer.read_us() - time_read_sensors;
maetugr 8:d25ecdcdbeb5 67
maetugr 8:d25ecdcdbeb5 68 // meassure dt
maetugr 12:67a06c9b69d5 69 dt_get_data = GlobalTimer.read_us() - time_get_data; // time in us since last loop
maetugr 12:67a06c9b69d5 70 time_get_data = GlobalTimer.read_us(); // set new time for next measurement
maetugr 8:d25ecdcdbeb5 71
maetugr 8:d25ecdcdbeb5 72 // calculate angles for roll, pitch an yaw
maetugr 12:67a06c9b69d5 73 angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt_get_data/15000000.0;
maetugr 12:67a06c9b69d5 74 angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt_get_data/15000000.0;// TODO Offset accelerometer einstellen
maetugr 12:67a06c9b69d5 75 tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt_get_data/15000000.0;
maetugr 12:67a06c9b69d5 76 angle[2] += Gyro.data[2] *dt_get_data/15000000.0; // gyro only here
maetugr 12:67a06c9b69d5 77
maetugr 12:67a06c9b69d5 78 // TODO Read RC data
maetugr 8:d25ecdcdbeb5 79
maetugr 12:67a06c9b69d5 80 // calculate new motorspeeds
maetugr 12:67a06c9b69d5 81 /*
maetugr 12:67a06c9b69d5 82 Motor[0] = 1000 + (100 + (angle[0] * 500/90)) * (RC[1].read() - 1000) / 1000;
maetugr 12:67a06c9b69d5 83 Motor[1] = 1000 + (100 + (angle[1] * 500/90)) * (RC[1].read() - 1000) / 1000;
maetugr 12:67a06c9b69d5 84 Motor[2] = 1000 + (100 - (angle[0] * 500/90)) * (RC[1].read() - 1000) / 1000;
maetugr 12:67a06c9b69d5 85 Motor[3] = 1000 + (100 - (angle[1] * 500/90)) * (RC[1].read() - 1000) / 1000;
maetugr 12:67a06c9b69d5 86 */
maetugr 8:d25ecdcdbeb5 87 }
maetugr 5:818c0668fd2d 88
maetugr 2:93f703d2c4d7 89
maetugr 0:0c4fafa398b4 90 int main() {
maetugr 12:67a06c9b69d5 91 //NVIC_SetPriority(TIMER3_IRQn, 2); // set priorty of tickers below hardware interrupts
maetugr 12:67a06c9b69d5 92
maetugr 12:67a06c9b69d5 93 #ifdef COMPASSCALIBRATE
maetugr 12:67a06c9b69d5 94 pc.locate(10,5);
maetugr 12:67a06c9b69d5 95 pc.printf("CALIBRATING");
maetugr 12:67a06c9b69d5 96 Comp.calibrate(60);
maetugr 12:67a06c9b69d5 97 #endif
maetugr 12:67a06c9b69d5 98
maetugr 6:179752756e9f 99 // init screen
maetugr 6:179752756e9f 100 pc.locate(10,5);
maetugr 6:179752756e9f 101 pc.printf("Flybed v0.2");
maetugr 1:5a64632b1eb9 102 LEDs.roll(2);
maetugr 5:818c0668fd2d 103
maetugr 9:4e0c3936c756 104 // Start!
maetugr 2:93f703d2c4d7 105 GlobalTimer.start();
maetugr 12:67a06c9b69d5 106 Datagetter.attach(&get_Data, RATE); // start to get data all RATEms
maetugr 12:67a06c9b69d5 107
maetugr 12:67a06c9b69d5 108 while(1) {
maetugr 9:4e0c3936c756 109 pc.locate(10,5); // PC output
maetugr 12:67a06c9b69d5 110 pc.printf("dt:%dms dt_sensors:%dus Altitude:%6.1fm ", dt_get_data/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
maetugr 7:9d4313510646 111 pc.locate(10,8);
maetugr 6:179752756e9f 112 pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]);
maetugr 10:953afcbcebfc 113 pc.locate(10,9);
maetugr 10:953afcbcebfc 114 pc.printf("ACC: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", Acc.angle[0], Acc.angle[1], Acc.angle[2]);
maetugr 7:9d4313510646 115 pc.locate(10,10);
maetugr 12:67a06c9b69d5 116 pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp.get_angle(), tempangle);
maetugr 7:9d4313510646 117 pc.locate(10,12);
maetugr 12:67a06c9b69d5 118 pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.get_angle());
maetugr 7:9d4313510646 119 pc.locate(10,13);
maetugr 11:9bf69bc6df45 120 pc.printf("Comp_scale: %6.4f %6.4f %6.4f ", Comp.scale[0], Comp.scale[1], Comp.scale[2]);
maetugr 10:953afcbcebfc 121 pc.locate(10,15);
maetugr 10:953afcbcebfc 122 pc.printf("pidtester: %6.1f RC: %d %d %d %d ", pidtester, RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
maetugr 12:67a06c9b69d5 123
maetugr 11:9bf69bc6df45 124 pc.locate(10,19);
maetugr 12:67a06c9b69d5 125 pc.printf("RC0: %d :[", RC[0].read());
maetugr 12:67a06c9b69d5 126 for (int i = 0; i < (RC[0].read() - 1000)/17; i++)
maetugr 12:67a06c9b69d5 127 pc.printf("=");
maetugr 12:67a06c9b69d5 128 pc.printf(" ");
maetugr 12:67a06c9b69d5 129 pc.locate(10,20);
maetugr 12:67a06c9b69d5 130 pc.printf("RC1: %d :[", RC[1].read());
maetugr 12:67a06c9b69d5 131 for (int i = 0; i < (RC[1].read() - 1000)/17; i++)
maetugr 12:67a06c9b69d5 132 pc.printf("=");
maetugr 12:67a06c9b69d5 133 pc.printf(" ");
maetugr 12:67a06c9b69d5 134 pc.locate(10,21);
maetugr 12:67a06c9b69d5 135 pc.printf("RC2: %d :[", RC[2].read());
maetugr 12:67a06c9b69d5 136 for (int i = 0; i < (RC[2].read() - 1000)/17; i++)
maetugr 12:67a06c9b69d5 137 pc.printf("=");
maetugr 12:67a06c9b69d5 138 pc.printf(" ");
maetugr 12:67a06c9b69d5 139 pc.locate(10,22);
maetugr 12:67a06c9b69d5 140 pc.printf("RC3: %d :[", RC[3].read());
maetugr 12:67a06c9b69d5 141 for (int i = 0; i < (RC[3].read() - 1000)/17; i++)
maetugr 12:67a06c9b69d5 142 pc.printf("=");
maetugr 12:67a06c9b69d5 143 pc.printf(" ");
maetugr 10:953afcbcebfc 144 LEDs.rollnext();
maetugr 0:0c4fafa398b4 145 }
maetugr 0:0c4fafa398b4 146 }