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@9:4e0c3936c756, 2012-10-16 (annotated)
- Committer:
- maetugr
- Date:
- Tue Oct 16 10:21:32 2012 +0000
- Revision:
- 9:4e0c3936c756
- Parent:
- 8:d25ecdcdbeb5
- Child:
- 10:953afcbcebfc
init von baro und compass in konstruktor geschrieben; vor einbetten der sensorwertarrays in die klassen
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 | 7:9d4313510646 | 3 | #include "L3G4200D.h" // Gyro (Gyroscope) |
maetugr | 7:9d4313510646 | 4 | #include "ADXL345.h" // Acc (Accelerometer) |
maetugr | 7:9d4313510646 | 5 | #include "HMC5883.h" // Comp (Compass) |
maetugr | 7:9d4313510646 | 6 | #include "BMP085.h" // Alt (Altitude sensor) |
maetugr | 7:9d4313510646 | 7 | #include "RC_Channel.h" // RemoteControl Chnnels with PPM |
maetugr | 7:9d4313510646 | 8 | #include "Servo.h" // Motor PPM |
maetugr | 7:9d4313510646 | 9 | #include "PC.h" // Serial Port via USB for debugging |
maetugr | 0:0c4fafa398b4 | 10 | |
maetugr | 8:d25ecdcdbeb5 | 11 | #define PI 3.1415926535897932384626433832795 |
maetugr | 8:d25ecdcdbeb5 | 12 | #define Rad2Deg 57.295779513082320876798154814105 |
maetugr | 8:d25ecdcdbeb5 | 13 | |
maetugr | 8:d25ecdcdbeb5 | 14 | Timer GlobalTimer; // global time to calculate processing speed |
maetugr | 8:d25ecdcdbeb5 | 15 | Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC |
maetugr | 2:93f703d2c4d7 | 16 | |
maetugr | 5:818c0668fd2d | 17 | // initialisation of hardware |
maetugr | 5:818c0668fd2d | 18 | LED LEDs; |
maetugr | 8:d25ecdcdbeb5 | 19 | PC pc(USBTX, USBRX, 57600); //Achtung: Treiber auf PC fuer Serial-mbed installieren. hier herunterladen https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe |
maetugr | 5:818c0668fd2d | 20 | L3G4200D Gyro(p28, p27); |
maetugr | 5:818c0668fd2d | 21 | ADXL345 Acc(p28, p27); |
maetugr | 5:818c0668fd2d | 22 | HMC5883 Comp(p28, p27, GlobalTimer); |
maetugr | 5:818c0668fd2d | 23 | BMP085 Alt(p28, p27); |
maetugr | 8:d25ecdcdbeb5 | 24 | RC_Channel RC[] = {(p10), (p11), (p12), (p13)}; // noooo p19/p20!!!! |
maetugr | 8:d25ecdcdbeb5 | 25 | //Servo Motor[] = {(p15), (p16), (p17), (p18)}; |
maetugr | 8:d25ecdcdbeb5 | 26 | |
maetugr | 8:d25ecdcdbeb5 | 27 | // variables for loop |
maetugr | 8:d25ecdcdbeb5 | 28 | unsigned long dt = 0; |
maetugr | 8:d25ecdcdbeb5 | 29 | unsigned long time_loop = 0; |
maetugr | 8:d25ecdcdbeb5 | 30 | float angle[3] = {0,0,0}; // angle of calculated situation |
maetugr | 8:d25ecdcdbeb5 | 31 | float Gyro_data[3]; |
maetugr | 8:d25ecdcdbeb5 | 32 | int Acc_data[3]; |
maetugr | 8:d25ecdcdbeb5 | 33 | float Acc_angle[2] = {0,0}; |
maetugr | 8:d25ecdcdbeb5 | 34 | float Comp_angle = 0; |
maetugr | 8:d25ecdcdbeb5 | 35 | float tempangle = 0; |
maetugr | 8:d25ecdcdbeb5 | 36 | |
maetugr | 8:d25ecdcdbeb5 | 37 | void get_Data() |
maetugr | 8:d25ecdcdbeb5 | 38 | { |
maetugr | 8:d25ecdcdbeb5 | 39 | // read data from sensors |
maetugr | 8:d25ecdcdbeb5 | 40 | Gyro.read(Gyro_data); |
maetugr | 8:d25ecdcdbeb5 | 41 | Acc.read(Acc_data); |
maetugr | 8:d25ecdcdbeb5 | 42 | Comp.Update(); |
maetugr | 8:d25ecdcdbeb5 | 43 | Alt.Update(); |
maetugr | 5:818c0668fd2d | 44 | |
maetugr | 8:d25ecdcdbeb5 | 45 | // calculate the angles for roll and pitch from accelerometer |
maetugr | 8:d25ecdcdbeb5 | 46 | Acc_angle[0] = Rad2Deg * atan2((float)Acc_data[1], (float)Acc_data[2]); |
maetugr | 8:d25ecdcdbeb5 | 47 | Acc_angle[1] = 3 -Rad2Deg * atan2((float)Acc_data[0], (float)Acc_data[2]); // TODO Offset accelerometer einstellen |
maetugr | 8:d25ecdcdbeb5 | 48 | |
maetugr | 8:d25ecdcdbeb5 | 49 | //calculate angle for yaw from compass |
maetugr | 8:d25ecdcdbeb5 | 50 | Comp_angle = Comp.getAngle(Comp.Mag[0], Comp.Mag[1]); |
maetugr | 8:d25ecdcdbeb5 | 51 | |
maetugr | 8:d25ecdcdbeb5 | 52 | // meassure dt |
maetugr | 8:d25ecdcdbeb5 | 53 | dt = GlobalTimer.read_us() - time_loop; // time in us since last loop |
maetugr | 8:d25ecdcdbeb5 | 54 | time_loop = GlobalTimer.read_us(); // set new time for next measurement |
maetugr | 8:d25ecdcdbeb5 | 55 | |
maetugr | 8:d25ecdcdbeb5 | 56 | // calculate angles for roll, pitch an yaw |
maetugr | 8:d25ecdcdbeb5 | 57 | angle[0] += (Acc_angle[0] - angle[0])/50 + Gyro_data[0] *dt/15000000.0; |
maetugr | 8:d25ecdcdbeb5 | 58 | angle[1] += (Acc_angle[1] - angle[1])/50 + Gyro_data[1] *dt/15000000.0; |
maetugr | 8:d25ecdcdbeb5 | 59 | tempangle += (Comp_angle - tempangle)/50 - Gyro_data[2] *dt/15000000.0; |
maetugr | 9:4e0c3936c756 | 60 | angle[2] -= Gyro_data[2] *dt/15000000.0; // gyro only here |
maetugr | 8:d25ecdcdbeb5 | 61 | |
maetugr | 8:d25ecdcdbeb5 | 62 | // Read RC data |
maetugr | 8:d25ecdcdbeb5 | 63 | //RC[0].read() // TODO: RC daten lesen und einberechnen! |
maetugr | 8:d25ecdcdbeb5 | 64 | |
maetugr | 8:d25ecdcdbeb5 | 65 | LEDs.rollnext(); |
maetugr | 8:d25ecdcdbeb5 | 66 | } |
maetugr | 5:818c0668fd2d | 67 | |
maetugr | 2:93f703d2c4d7 | 68 | |
maetugr | 0:0c4fafa398b4 | 69 | int main() { |
maetugr | 6:179752756e9f | 70 | // init screen |
maetugr | 6:179752756e9f | 71 | pc.locate(10,5); |
maetugr | 6:179752756e9f | 72 | pc.printf("Flybed v0.2"); |
maetugr | 1:5a64632b1eb9 | 73 | LEDs.roll(2); |
maetugr | 5:818c0668fd2d | 74 | |
maetugr | 9:4e0c3936c756 | 75 | // Start! |
maetugr | 2:93f703d2c4d7 | 76 | GlobalTimer.start(); |
maetugr | 8:d25ecdcdbeb5 | 77 | Datagetter.attach(&get_Data, 0.02); // start to get data all 10ms |
maetugr | 0:0c4fafa398b4 | 78 | while(1) { |
maetugr | 9:4e0c3936c756 | 79 | pc.locate(10,5); // PC output |
maetugr | 8:d25ecdcdbeb5 | 80 | pc.printf("dt:%dms %6.1fm ", dt/1000, Alt.CalcAltitude(Alt.Pressure)); |
maetugr | 7:9d4313510646 | 81 | pc.locate(10,8); |
maetugr | 6:179752756e9f | 82 | pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]); |
maetugr | 7:9d4313510646 | 83 | pc.locate(10,10); |
maetugr | 8:d25ecdcdbeb5 | 84 | pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp_angle, tempangle); |
maetugr | 7:9d4313510646 | 85 | pc.locate(10,12); |
maetugr | 7:9d4313510646 | 86 | pc.printf("Comp_Raw: %6.1f %6.1f %6.1f ", Comp.RawMag[0], Comp.RawMag[1], Comp.RawMag[2]); |
maetugr | 7:9d4313510646 | 87 | pc.locate(10,13); |
maetugr | 7:9d4313510646 | 88 | pc.printf("Comp_Mag: %6.1f %6.1f %6.1f ", Comp.Mag[0], Comp.Mag[1], Comp.Mag[2]); |
maetugr | 8:d25ecdcdbeb5 | 89 | //Motor_left = 1000 + 5*abs(angle[1]); |
maetugr | 0:0c4fafa398b4 | 90 | } |
maetugr | 0:0c4fafa398b4 | 91 | } |