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);
     }
 }