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@12:67a06c9b69d5, 2012-10-20 (annotated)
- Committer:
- maetugr
- Date:
- Sat Oct 20 17:28:28 2012 +0000
- Revision:
- 12:67a06c9b69d5
- Parent:
- 11:9bf69bc6df45
- Child:
- 13:4737ee9ebfee
mit unterbrochenem RC signal, kompass fertig mit speicherung!
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 | 10:953afcbcebfc | 3 | #include "PC.h" // Serial Port via USB for debugging in TeraTerm (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 | 10:953afcbcebfc | 10 | #include "PID.h" // PID Library from Aaron Berk |
maetugr | 0:0c4fafa398b4 | 11 | |
maetugr | 10:953afcbcebfc | 12 | #define PI 3.1415926535897932384626433832795 |
maetugr | 10:953afcbcebfc | 13 | #define Rad2Deg 57.295779513082320876798154814105 |
maetugr | 10:953afcbcebfc | 14 | #define RATE 0.02 // speed of Ticker/PID |
maetugr | 8:d25ecdcdbeb5 | 15 | |
maetugr | 12:67a06c9b69d5 | 16 | //#define COMPASSCALIBRATE |
maetugr | 12:67a06c9b69d5 | 17 | |
maetugr | 8:d25ecdcdbeb5 | 18 | Timer GlobalTimer; // global time to calculate processing speed |
maetugr | 8:d25ecdcdbeb5 | 19 | Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC |
maetugr | 10:953afcbcebfc | 20 | PID controller(1.0, 0.0, 0.0, RATE); // test PID controller for throttle |
maetugr | 10:953afcbcebfc | 21 | PID pid(1.0, 1.0, 0.0, RATE); // test PID controller for throttle |
maetugr | 2:93f703d2c4d7 | 22 | |
maetugr | 5:818c0668fd2d | 23 | // initialisation of hardware |
maetugr | 5:818c0668fd2d | 24 | LED LEDs; |
maetugr | 10:953afcbcebfc | 25 | PC pc(USBTX, USBRX, 57600); |
maetugr | 5:818c0668fd2d | 26 | L3G4200D Gyro(p28, p27); |
maetugr | 5:818c0668fd2d | 27 | ADXL345 Acc(p28, p27); |
maetugr | 11:9bf69bc6df45 | 28 | HMC5883 Comp(p28, p27); |
maetugr | 5:818c0668fd2d | 29 | BMP085 Alt(p28, p27); |
maetugr | 8:d25ecdcdbeb5 | 30 | RC_Channel RC[] = {(p10), (p11), (p12), (p13)}; // noooo p19/p20!!!! |
maetugr | 10:953afcbcebfc | 31 | Servo Motor[] = {(p15), (p16), (p17), (p18)}; |
maetugr | 8:d25ecdcdbeb5 | 32 | |
maetugr | 8:d25ecdcdbeb5 | 33 | // variables for loop |
maetugr | 12:67a06c9b69d5 | 34 | unsigned long dt_get_data = 0; |
maetugr | 12:67a06c9b69d5 | 35 | unsigned long time_get_data = 0; |
maetugr | 12:67a06c9b69d5 | 36 | unsigned long dt_read_sensors = 0; |
maetugr | 12:67a06c9b69d5 | 37 | unsigned long time_read_sensors = 0; |
maetugr | 11:9bf69bc6df45 | 38 | float angle[3] = {0,0,0}; // angle 0: x,roll / 1: y,pitch / 2: z,yaw |
maetugr | 8:d25ecdcdbeb5 | 39 | float tempangle = 0; |
maetugr | 10:953afcbcebfc | 40 | int Motorvalue[3]; |
maetugr | 10:953afcbcebfc | 41 | |
maetugr | 10:953afcbcebfc | 42 | float pidtester; |
maetugr | 8:d25ecdcdbeb5 | 43 | |
maetugr | 8:d25ecdcdbeb5 | 44 | void get_Data() |
maetugr | 8:d25ecdcdbeb5 | 45 | { |
maetugr | 12:67a06c9b69d5 | 46 | time_read_sensors = GlobalTimer.read_us(); |
maetugr | 12:67a06c9b69d5 | 47 | |
maetugr | 8:d25ecdcdbeb5 | 48 | // read data from sensors |
maetugr | 10:953afcbcebfc | 49 | Gyro.read(); |
maetugr | 10:953afcbcebfc | 50 | Acc.read(); |
maetugr | 11:9bf69bc6df45 | 51 | Comp.read(); |
maetugr | 12:67a06c9b69d5 | 52 | //Alt.Update(); |
maetugr | 12:67a06c9b69d5 | 53 | |
maetugr | 12:67a06c9b69d5 | 54 | dt_read_sensors = GlobalTimer.read_us() - time_read_sensors; |
maetugr | 8:d25ecdcdbeb5 | 55 | |
maetugr | 8:d25ecdcdbeb5 | 56 | // meassure dt |
maetugr | 12:67a06c9b69d5 | 57 | dt_get_data = GlobalTimer.read_us() - time_get_data; // time in us since last loop |
maetugr | 12:67a06c9b69d5 | 58 | time_get_data = GlobalTimer.read_us(); // set new time for next measurement |
maetugr | 8:d25ecdcdbeb5 | 59 | |
maetugr | 8:d25ecdcdbeb5 | 60 | // calculate angles for roll, pitch an yaw |
maetugr | 12:67a06c9b69d5 | 61 | angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt_get_data/15000000.0; |
maetugr | 12:67a06c9b69d5 | 62 | angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt_get_data/15000000.0;// TODO Offset accelerometer einstellen |
maetugr | 12:67a06c9b69d5 | 63 | tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt_get_data/15000000.0; |
maetugr | 12:67a06c9b69d5 | 64 | angle[2] += Gyro.data[2] *dt_get_data/15000000.0; // gyro only here |
maetugr | 12:67a06c9b69d5 | 65 | |
maetugr | 12:67a06c9b69d5 | 66 | // TODO Read RC data |
maetugr | 8:d25ecdcdbeb5 | 67 | |
maetugr | 12:67a06c9b69d5 | 68 | // calculate new motorspeeds |
maetugr | 12:67a06c9b69d5 | 69 | /* |
maetugr | 12:67a06c9b69d5 | 70 | Motor[0] = 1000 + (100 + (angle[0] * 500/90)) * (RC[1].read() - 1000) / 1000; |
maetugr | 12:67a06c9b69d5 | 71 | Motor[1] = 1000 + (100 + (angle[1] * 500/90)) * (RC[1].read() - 1000) / 1000; |
maetugr | 12:67a06c9b69d5 | 72 | Motor[2] = 1000 + (100 - (angle[0] * 500/90)) * (RC[1].read() - 1000) / 1000; |
maetugr | 12:67a06c9b69d5 | 73 | Motor[3] = 1000 + (100 - (angle[1] * 500/90)) * (RC[1].read() - 1000) / 1000; |
maetugr | 12:67a06c9b69d5 | 74 | */ |
maetugr | 8:d25ecdcdbeb5 | 75 | } |
maetugr | 5:818c0668fd2d | 76 | |
maetugr | 2:93f703d2c4d7 | 77 | |
maetugr | 0:0c4fafa398b4 | 78 | int main() { |
maetugr | 12:67a06c9b69d5 | 79 | //NVIC_SetPriority(TIMER3_IRQn, 2); // set priorty of tickers below hardware interrupts |
maetugr | 12:67a06c9b69d5 | 80 | |
maetugr | 12:67a06c9b69d5 | 81 | #ifdef COMPASSCALIBRATE |
maetugr | 12:67a06c9b69d5 | 82 | pc.locate(10,5); |
maetugr | 12:67a06c9b69d5 | 83 | pc.printf("CALIBRATING"); |
maetugr | 12:67a06c9b69d5 | 84 | Comp.calibrate(60); |
maetugr | 12:67a06c9b69d5 | 85 | #endif |
maetugr | 12:67a06c9b69d5 | 86 | |
maetugr | 6:179752756e9f | 87 | // init screen |
maetugr | 6:179752756e9f | 88 | pc.locate(10,5); |
maetugr | 6:179752756e9f | 89 | pc.printf("Flybed v0.2"); |
maetugr | 1:5a64632b1eb9 | 90 | LEDs.roll(2); |
maetugr | 5:818c0668fd2d | 91 | |
maetugr | 9:4e0c3936c756 | 92 | // Start! |
maetugr | 2:93f703d2c4d7 | 93 | GlobalTimer.start(); |
maetugr | 12:67a06c9b69d5 | 94 | Datagetter.attach(&get_Data, RATE); // start to get data all RATEms |
maetugr | 12:67a06c9b69d5 | 95 | |
maetugr | 12:67a06c9b69d5 | 96 | while(1) { |
maetugr | 9:4e0c3936c756 | 97 | pc.locate(10,5); // PC output |
maetugr | 12:67a06c9b69d5 | 98 | pc.printf("dt:%dms dt_sensors:%dus Altitude:%6.1fm ", dt_get_data/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure)); |
maetugr | 7:9d4313510646 | 99 | pc.locate(10,8); |
maetugr | 6:179752756e9f | 100 | pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]); |
maetugr | 10:953afcbcebfc | 101 | pc.locate(10,9); |
maetugr | 10:953afcbcebfc | 102 | pc.printf("ACC: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", Acc.angle[0], Acc.angle[1], Acc.angle[2]); |
maetugr | 7:9d4313510646 | 103 | pc.locate(10,10); |
maetugr | 12:67a06c9b69d5 | 104 | pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp.get_angle(), tempangle); |
maetugr | 7:9d4313510646 | 105 | pc.locate(10,12); |
maetugr | 12:67a06c9b69d5 | 106 | 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 | 107 | pc.locate(10,13); |
maetugr | 11:9bf69bc6df45 | 108 | pc.printf("Comp_scale: %6.4f %6.4f %6.4f ", Comp.scale[0], Comp.scale[1], Comp.scale[2]); |
maetugr | 10:953afcbcebfc | 109 | pc.locate(10,15); |
maetugr | 10:953afcbcebfc | 110 | 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 | 111 | |
maetugr | 11:9bf69bc6df45 | 112 | pc.locate(10,19); |
maetugr | 12:67a06c9b69d5 | 113 | pc.printf("RC0: %d :[", RC[0].read()); |
maetugr | 12:67a06c9b69d5 | 114 | for (int i = 0; i < (RC[0].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 115 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 116 | pc.printf(" "); |
maetugr | 12:67a06c9b69d5 | 117 | pc.locate(10,20); |
maetugr | 12:67a06c9b69d5 | 118 | pc.printf("RC1: %d :[", RC[1].read()); |
maetugr | 12:67a06c9b69d5 | 119 | for (int i = 0; i < (RC[1].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 120 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 121 | pc.printf(" "); |
maetugr | 12:67a06c9b69d5 | 122 | pc.locate(10,21); |
maetugr | 12:67a06c9b69d5 | 123 | pc.printf("RC2: %d :[", RC[2].read()); |
maetugr | 12:67a06c9b69d5 | 124 | for (int i = 0; i < (RC[2].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 125 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 126 | pc.printf(" "); |
maetugr | 12:67a06c9b69d5 | 127 | pc.locate(10,22); |
maetugr | 12:67a06c9b69d5 | 128 | pc.printf("RC3: %d :[", RC[3].read()); |
maetugr | 12:67a06c9b69d5 | 129 | for (int i = 0; i < (RC[3].read() - 1000)/17; i++) |
maetugr | 12:67a06c9b69d5 | 130 | pc.printf("="); |
maetugr | 12:67a06c9b69d5 | 131 | pc.printf(" "); |
maetugr | 10:953afcbcebfc | 132 | LEDs.rollnext(); |
maetugr | 0:0c4fafa398b4 | 133 | } |
maetugr | 0:0c4fafa398b4 | 134 | } |