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@14:cf260677ecde, 2012-10-27 (annotated)
- Committer:
- maetugr
- Date:
- Sat Oct 27 10:53:43 2012 +0000
- Revision:
- 14:cf260677ecde
- Parent:
- 13:4737ee9ebfee
- Child:
- 15:753c5d6a63b3
I2C Workaround gefunden!!! erster Test geklappt, vor Umschreibung
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 | 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 | 14:cf260677ecde | 13 | #define PI 3.1415926535897932384626433832795 // ratio of a circle's circumference to its diameter |
maetugr | 14:cf260677ecde | 14 | #define Rad2Deg 57.295779513082320876798154814105 // ratio between radians and degree (360/2Pi) |
maetugr | 14:cf260677ecde | 15 | #define RATE 0.0007 // speed of the interrupt for Sensors and PID |
maetugr | 12:67a06c9b69d5 | 16 | |
maetugr | 14:cf260677ecde | 17 | //#define COMPASSCALIBRATE // decomment if you want to calibrate the Compass on start |
maetugr | 2:93f703d2c4d7 | 18 | |
maetugr | 14:cf260677ecde | 19 | Timer GlobalTimer; // global time to calculate processing speed |
maetugr | 14:cf260677ecde | 20 | Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC |
maetugr | 14:cf260677ecde | 21 | |
maetugr | 14:cf260677ecde | 22 | // initialisation of hardware (see includes for more info) |
maetugr | 5:818c0668fd2d | 23 | LED LEDs; |
maetugr | 10:953afcbcebfc | 24 | PC pc(USBTX, USBRX, 57600); |
maetugr | 5:818c0668fd2d | 25 | L3G4200D Gyro(p28, p27); |
maetugr | 5:818c0668fd2d | 26 | ADXL345 Acc(p28, p27); |
maetugr | 11:9bf69bc6df45 | 27 | HMC5883 Comp(p28, p27); |
maetugr | 14:cf260677ecde | 28 | BMP085_old Alt(p28, p27); |
maetugr | 13:4737ee9ebfee | 29 | RC_Channel RC[] = {(p11), (p12), (p13), (p14)}; // noooo p19/p20!!!! |
maetugr | 10:953afcbcebfc | 30 | Servo Motor[] = {(p15), (p16), (p17), (p18)}; |
maetugr | 8:d25ecdcdbeb5 | 31 | |
maetugr | 14:cf260677ecde | 32 | PID controller(1.0, 0.0, 0.0, RATE); // test PID controller for throttle |
maetugr | 14:cf260677ecde | 33 | PID pid(1.0, 1.0, 0.0, RATE); // test PID controller for throttle |
maetugr | 14:cf260677ecde | 34 | //PID P:3,0 bis 3,5 I:0,010 und 0,050 D:5 und 25 |
maetugr | 14:cf260677ecde | 35 | |
maetugr | 14:cf260677ecde | 36 | |
maetugr | 14:cf260677ecde | 37 | // global varibles |
maetugr | 14:cf260677ecde | 38 | unsigned long dt_get_data = 0; // TODO: dt namen |
maetugr | 12:67a06c9b69d5 | 39 | unsigned long time_get_data = 0; |
maetugr | 12:67a06c9b69d5 | 40 | unsigned long dt_read_sensors = 0; |
maetugr | 12:67a06c9b69d5 | 41 | unsigned long time_read_sensors = 0; |
maetugr | 14:cf260677ecde | 42 | float angle[3] = {0,0,0}; // calculated values of the position [0: x,roll | 1: y,pitch | 2: z,yaw] |
maetugr | 14:cf260677ecde | 43 | float tempangle = 0; // temporärer winkel für yaw ohne kompass |
maetugr | 10:953afcbcebfc | 44 | float pidtester; |
maetugr | 8:d25ecdcdbeb5 | 45 | |
maetugr | 14:cf260677ecde | 46 | void get_Data() // method which is called by the Ticker Datagetter every RATE seconds |
maetugr | 8:d25ecdcdbeb5 | 47 | { |
maetugr | 12:67a06c9b69d5 | 48 | time_read_sensors = GlobalTimer.read_us(); |
maetugr | 12:67a06c9b69d5 | 49 | |
maetugr | 14:cf260677ecde | 50 | // read data from sensors |
maetugr | 14:cf260677ecde | 51 | /*GPIO_IntDisable(0, 18, 2); // abschalten der Pins 11-14 mit Göttis library |
maetugr | 13:4737ee9ebfee | 52 | GPIO_IntDisable(0, 17, 2); |
maetugr | 13:4737ee9ebfee | 53 | GPIO_IntDisable(0, 16, 2); |
maetugr | 14:cf260677ecde | 54 | GPIO_IntDisable(0, 15, 2);*/ |
maetugr | 13:4737ee9ebfee | 55 | //__disable_irq(); // test, deactivate all interrupts, I2C working? |
maetugr | 14:cf260677ecde | 56 | Gyro.read(); |
maetugr | 14:cf260677ecde | 57 | //Acc.read(); |
maetugr | 14:cf260677ecde | 58 | //Comp.read(); |
maetugr | 13:4737ee9ebfee | 59 | //Alt.Update(); TODO braucht zu lange zum auslesen! |
maetugr | 13:4737ee9ebfee | 60 | //__enable_irq(); |
maetugr | 14:cf260677ecde | 61 | /*GPIO_IntEnable(0, 18, 2); // schaltet die PINs wieder ein |
maetugr | 13:4737ee9ebfee | 62 | GPIO_IntEnable(0, 17, 2); |
maetugr | 13:4737ee9ebfee | 63 | GPIO_IntEnable(0, 16, 2); |
maetugr | 14:cf260677ecde | 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 | 14:cf260677ecde | 89 | int main() { // main programm only used for initialisation and debug output |
maetugr | 14:cf260677ecde | 90 | NVIC_SetPriority(TIMER3_IRQn, 2); // set priorty of tickers below hardware interrupts |
maetugr | 14:cf260677ecde | 91 | /*NVIC_SetPriority(I2C0_IRQn, 1); //I2C Priorität? |
maetugr | 14:cf260677ecde | 92 | NVIC_SetPriority(I2C1_IRQn, 1); |
maetugr | 14:cf260677ecde | 93 | NVIC_SetPriority(I2C2_IRQn, 1);*/ |
maetugr | 12:67a06c9b69d5 | 94 | |
maetugr | 12:67a06c9b69d5 | 95 | #ifdef COMPASSCALIBRATE |
maetugr | 12:67a06c9b69d5 | 96 | pc.locate(10,5); |
maetugr | 12:67a06c9b69d5 | 97 | pc.printf("CALIBRATING"); |
maetugr | 12:67a06c9b69d5 | 98 | Comp.calibrate(60); |
maetugr | 12:67a06c9b69d5 | 99 | #endif |
maetugr | 12:67a06c9b69d5 | 100 | |
maetugr | 6:179752756e9f | 101 | // init screen |
maetugr | 6:179752756e9f | 102 | pc.locate(10,5); |
maetugr | 6:179752756e9f | 103 | pc.printf("Flybed v0.2"); |
maetugr | 1:5a64632b1eb9 | 104 | LEDs.roll(2); |
maetugr | 5:818c0668fd2d | 105 | |
maetugr | 14:cf260677ecde | 106 | // Start! TODO: Motor und RC start (armed....?) |
maetugr | 2:93f703d2c4d7 | 107 | GlobalTimer.start(); |
maetugr | 12:67a06c9b69d5 | 108 | Datagetter.attach(&get_Data, RATE); // start to get data all RATEms |
maetugr | 12:67a06c9b69d5 | 109 | |
maetugr | 12:67a06c9b69d5 | 110 | while(1) { |
maetugr | 9:4e0c3936c756 | 111 | pc.locate(10,5); // PC output |
maetugr | 12:67a06c9b69d5 | 112 | pc.printf("dt:%dms dt_sensors:%dus Altitude:%6.1fm ", dt_get_data/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure)); |
maetugr | 7:9d4313510646 | 113 | pc.locate(10,8); |
maetugr | 6:179752756e9f | 114 | pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]); |
maetugr | 10:953afcbcebfc | 115 | pc.locate(10,9); |
maetugr | 10:953afcbcebfc | 116 | pc.printf("ACC: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", Acc.angle[0], Acc.angle[1], Acc.angle[2]); |
maetugr | 7:9d4313510646 | 117 | pc.locate(10,10); |
maetugr | 12:67a06c9b69d5 | 118 | pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp.get_angle(), tempangle); |
maetugr | 7:9d4313510646 | 119 | pc.locate(10,12); |
maetugr | 12:67a06c9b69d5 | 120 | 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 | 121 | pc.locate(10,13); |
maetugr | 14:cf260677ecde | 122 | //pc.printf("Comp_scale: %6.4f %6.4f %6.4f ", Comp.scale[0], Comp.scale[1], Comp.scale[2]); no more accessible its private |
maetugr | 10:953afcbcebfc | 123 | pc.locate(10,15); |
maetugr | 10:953afcbcebfc | 124 | 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 | 125 | |
maetugr | 11:9bf69bc6df45 | 126 | pc.locate(10,19); |
maetugr | 14:cf260677ecde | 127 | pc.printf("RC0: %4d :[", RC[0].read()); |
maetugr | 12:67a06c9b69d5 | 128 | for (int i = 0; i < (RC[0].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 129 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 130 | pc.printf(" "); |
maetugr | 12:67a06c9b69d5 | 131 | pc.locate(10,20); |
maetugr | 14:cf260677ecde | 132 | pc.printf("RC1: %4d :[", RC[1].read()); |
maetugr | 12:67a06c9b69d5 | 133 | for (int i = 0; i < (RC[1].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 134 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 135 | pc.printf(" "); |
maetugr | 12:67a06c9b69d5 | 136 | pc.locate(10,21); |
maetugr | 14:cf260677ecde | 137 | pc.printf("RC2: %4d :[", RC[2].read()); |
maetugr | 12:67a06c9b69d5 | 138 | for (int i = 0; i < (RC[2].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 139 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 140 | pc.printf(" "); |
maetugr | 12:67a06c9b69d5 | 141 | pc.locate(10,22); |
maetugr | 14:cf260677ecde | 142 | pc.printf("RC3: %4d :[", RC[3].read()); |
maetugr | 12:67a06c9b69d5 | 143 | for (int i = 0; i < (RC[3].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 144 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 145 | pc.printf(" "); |
maetugr | 10:953afcbcebfc | 146 | LEDs.rollnext(); |
maetugr | 0:0c4fafa398b4 | 147 | } |
maetugr | 0:0c4fafa398b4 | 148 | } |