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
- Committer:
- maetugr
- Date:
- 2012-10-13
- Revision:
- 5:818c0668fd2d
- Parent:
- 4:e96b16ad986d
- Child:
- 6:179752756e9f
File content as of revision 5:818c0668fd2d:
#include "mbed.h" // Standar Library #include "LCD.h" // Display #include "LED.h" // LEDs #include "L3G4200D.h" // Gyro (Gyroscope) #include "ADXL345.h" // Acc (Accelerometer) #include "HMC5883.h" // Comp (Compass) #include "BMP085.h" // Alt (Altitude sensor) #include "Servo.h" // Motor #include "terminal.h" Timer GlobalTimer; // initialisation of hardware LED LEDs; TextLCD LCD(p5, p6, p7, p8, p9, p10, p11, TextLCD::LCD16x2); // RS, RW, E, D0, D1, D2, D3, Typ L3G4200D Gyro(p28, p27); ADXL345 Acc(p28, p27); HMC5883 Comp(p28, p27, GlobalTimer); BMP085 Alt(p28, p27); //Servo Motor(p12); struct terminal pc(USBTX, USBRX); //Achtung: Treiber auf PC fuer Serial-mbed installieren. //hier herunterladen https://mbed.org/media/downloads/drivers/mbedWinSerial_16466.exe #define PI 3.1415926535897932384626433832795 #define Rad2Deg 57.295779513082320876798154814105 int main() { // LCD/LED init LCD.cls(); // Display l�schen LCD.printf("Flybed v0.2"); LEDs.roll(2); pc.baud(57600); // variables for loop float Gyro_data[3]; int Acc_data[3]; unsigned long dt = 0; unsigned long time_loop = 0; float angle[3] = {0,0,0}; float Acc_angle[2] = {0,0}; float Comp_angle = 0; float tempangle = 0; //Motor.initialize(); //Kompass kalibrieren --> Problem fremde Magnetfelder! //Comp.AutoCalibration = 1; short MagRawMin[3]= {-400, -400, -400}; //Gespeicherte Werte verwenden short MagRawMax[3]= {400, 400, 400}; Comp.Calibrate(MagRawMin, MagRawMax); //Comp.Calibrate(20); //Oversampling des Barometers setzen Alt.oss = 0; GlobalTimer.start(); while(1) { // read data from sensors Gyro.read(Gyro_data); Acc.read(Acc_data); Comp.Update(); Alt.Update(); // calculate the angles for roll and pitch from accelerometer Acc_angle[0] = Rad2Deg * atan2((float)Acc_data[1], (float)Acc_data[2]); Acc_angle[1] = 3 -Rad2Deg * atan2((float)Acc_data[0], (float)Acc_data[2]); // TODO Offset accelerometer einstellen //calculate angle for yaw from compass Comp_angle = Comp.getAngle(Comp.Mag[0], Comp.Mag[1]); // meassure dt dt = GlobalTimer.read_us() - time_loop; // Zeit in us seit letzter loop time_loop = GlobalTimer.read_us(); // setze aktuelle zeit f�r n�chste messung // calculate angles for roll, pitch an yaw angle[0] += (Acc_angle[0] - angle[0])/50 + Gyro_data[0] *dt/15000000.0; angle[1] += (Acc_angle[1] - angle[1])/50 + Gyro_data[1] *dt/15000000.0; angle[2] += (Comp_angle - angle[2])/50 - Gyro_data[2] *dt/15000000.0; tempangle -= Gyro_data[2] *dt/15000000.0; int i =345; // LCD output pc.locate(10,5); // first line pc.printf("Y:%2.1f %2.1fm ", angle[2], Alt.CalcAltitude(Alt.Pressure)); //LCD.printf("%2.1f %2.1f %2.1f ", Comp.RawMag[0], Comp.RawMag[1], Comp.RawMag[2]); pc.locate(10,8); // second line pc.printf("R:%2.1f P:%2.1f ", angle[0], angle[1]); //LCD.printf("R:%2.1f P:%2.1f ", Comp_angle, tempangle); //LCD.printf("%2.1f %2.1f %2.1f ", Comp.Mag[0], Comp.Mag[1], Comp.Mag[2]); //Motor = 1000 + abs(Acc_data[1]); // set new motor speed //LED hin und her int ledset = 0; if (angle[0] < 0) ledset += 1; else ledset += 8; if (angle[1] < 0) ledset += 2; else ledset += 4; LEDs = ledset; //LEDs.rollnext(); //wait(0.1); } }