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@8:d25ecdcdbeb5, 2012-10-15 (annotated)

Committer:
maetugr
Date:
Mon Oct 15 19:03:17 2012 +0000
Revision:
8:d25ecdcdbeb5
Parent:
7:9d4313510646
Child:
9:4e0c3936c756
mit Ticker (aufger?umt)

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 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 8:d25ecdcdbeb5 60 angle[2] -= Gyro_data[2] *dt/15000000.0;
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 5:818c0668fd2d 75 //Kompass kalibrieren --> Problem fremde Magnetfelder!
maetugr 5:818c0668fd2d 76 //Comp.AutoCalibration = 1;
maetugr 5:818c0668fd2d 77 short MagRawMin[3]= {-400, -400, -400}; //Gespeicherte Werte verwenden
maetugr 5:818c0668fd2d 78 short MagRawMax[3]= {400, 400, 400};
maetugr 5:818c0668fd2d 79 Comp.Calibrate(MagRawMin, MagRawMax);
maetugr 5:818c0668fd2d 80 //Comp.Calibrate(20);
maetugr 2:93f703d2c4d7 81
maetugr 8:d25ecdcdbeb5 82 Alt.oss = 0; //Oversampling des Barometers setzen
maetugr 4:e96b16ad986d 83
maetugr 2:93f703d2c4d7 84 GlobalTimer.start();
maetugr 8:d25ecdcdbeb5 85 Datagetter.attach(&get_Data, 0.02); // start to get data all 10ms
maetugr 0:0c4fafa398b4 86 while(1) {
maetugr 8:d25ecdcdbeb5 87 // PC output
maetugr 7:9d4313510646 88 pc.locate(10,5);
maetugr 8:d25ecdcdbeb5 89 pc.printf("dt:%dms %6.1fm ", dt/1000, Alt.CalcAltitude(Alt.Pressure));
maetugr 7:9d4313510646 90 pc.locate(10,8);
maetugr 6:179752756e9f 91 pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]);
maetugr 7:9d4313510646 92 pc.locate(10,10);
maetugr 8:d25ecdcdbeb5 93 pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp_angle, tempangle);
maetugr 7:9d4313510646 94 pc.locate(10,12);
maetugr 7:9d4313510646 95 pc.printf("Comp_Raw: %6.1f %6.1f %6.1f ", Comp.RawMag[0], Comp.RawMag[1], Comp.RawMag[2]);
maetugr 7:9d4313510646 96 pc.locate(10,13);
maetugr 7:9d4313510646 97 pc.printf("Comp_Mag: %6.1f %6.1f %6.1f ", Comp.Mag[0], Comp.Mag[1], Comp.Mag[2]);
maetugr 8:d25ecdcdbeb5 98 //Motor_left = 1000 + 5*abs(angle[1]);
maetugr 0:0c4fafa398b4 99 }
maetugr 0:0c4fafa398b4 100 }