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.
Diff: main.cpp
- Revision:
- 5:818c0668fd2d
- Parent:
- 4:e96b16ad986d
- Child:
- 6:179752756e9f
--- a/main.cpp Fri Oct 05 14:05:10 2012 +0000 +++ b/main.cpp Sat Oct 13 08:59:03 2012 +0000 @@ -1,84 +1,103 @@ #include "mbed.h" // Standar Library #include "LCD.h" // Display #include "LED.h" // LEDs -#include "L3G4200D.h" // Gyro -#include "ADXL345.h" // Acc +#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 - -// initialisation -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); -//Servo Motor(p12); +#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"); + LCD.cls(); // Display l�schen + LCD.printf("Flybed v0.2"); LEDs.roll(2); - LEDs = 15; + + pc.baud(57600); - float Gyro_data[3]; - int Acc_data[3]; - //int Gyro_angle[3] = {0,0,0}; - unsigned long dt = 0; - unsigned long time_loop = 0; + // 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); - float angle[3] = {0,0,0}; - float Acc_angle[2] = {0,0}; + //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(); - // Acc data angle - //float Acc_abs = sqrt(pow((float)Acc_data[0],2) + pow((float)Acc_data[1],2) + pow((float)Acc_data[2],2)); - //float Acc_angle = Rad2Deg * acos((float)Acc_data[2]/Acc_abs); + // 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] = -Rad2Deg * atan2((float)Acc_data[0], (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]); - //dt berechnen - dt = GlobalTimer.read_us() - time_loop; - time_loop = GlobalTimer.read_us(); + // 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] += /*(Acc_angle[1] - angle[1])/50 +*/ Gyro_data[2] *dt/15000000.0; - //Gyro_angle[0] += (Gyro_data[0]) * 0.01; - LCD.locate(0,0); - //LCD.printf("%2.1f %2.1f %2.1f", Gyro_data[0],Gyro_data[1],Gyro_data[2]); - //LCD.printf("%d %d |%2.1f ",Acc_data[1],Acc_data[2] ,Acc_angle); //roll(x) pitch(y) yaw(z) - LCD.printf(" |%2.1f ",Acc_data[2]/20.0); + angle[2] += (Comp_angle - angle[2])/50 - Gyro_data[2] *dt/15000000.0; + tempangle -= Gyro_data[2] *dt/15000000.0; - LCD.locate(1,0); - //LCD.printf("%d %d %d %2.1f ", Acc_data[0],Acc_data[1],Acc_data[2]); - //LCD.printf("%2.1f %2.1f %2.2f %2.1f", Acc_angle,Acc_angle,dt/10000.0, angle); - LCD.printf("%2.1f %2.1f %2.1f ", angle[0], angle[1], angle[2]); + 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]); // Motorwert anpassen + //Motor = 1000 + abs(Acc_data[1]); // set new motor speed //LED hin und her int ledset = 0; - if (Acc_angle < 0) - ledset += 1; - else - ledset += 8; - if (angle < 0) - ledset += 2; - else - ledset += 4; - //wait(0.1); + 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); } }