Revision 5:818c0668fd2d, committed 2012-10-13

Comitter:

maetugr

Date:

Sat Oct 13 08:59:03 2012 +0000

Parent:

4:e96b16ad986d

Child:

6:179752756e9f

Commit message:

?bergang von LCD auf PC-Terminal

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP085/BMP085.cpp	Sat Oct 13 08:59:03 2012 +0000
@@ -0,0 +1,147 @@
+
+#include "mbed.h"
+#include "BMP085.h"
+
+//I2C Adresse
+#define BMP085_ADRESS 0xEE
+
+#define xpow(x, y) ((long)1 << y)
+
+
+// Constructor
+// -----------------------------------------------
+BMP085::BMP085(PinName sda, PinName scl) : i2c_(sda, scl)
+    {
+    Init();
+    }
+    
+
+// Temperatur und Druck auslesen und berechnen
+// -----------------------------------------------
+void BMP085::Update () 
+    {
+    long P, UTemp, UPressure, X1, X2, X3, B3, B5, B6;
+    unsigned long B4, B7;
+
+    twi_writechar(BMP085_ADRESS, 0xf4, 0x2e);
+    // Wait at least 4.5ms
+    wait(0.005);
+    UTemp = twi_readshort(BMP085_ADRESS, 0xf6);
+    
+    X1 = ((UTemp - AC6) * AC5) >> 15;
+    X2 = (MC << 11) / (X1 + MD);
+    B5 = X1 + X2;
+    Temperature = (float)((B5 + 8) >> 4)/10.0;
+
+    twi_writechar(BMP085_ADRESS, 0xf4, 0x34 + (oss << 6));
+    // Wait at least 4.5ms
+    wait(0.005);
+    UPressure = twi_readlong(BMP085_ADRESS, 0xf6) >> (8 - oss);
+
+    B6 = B5 - 4000;
+    X1 = (B2 * (B6 * B6) >> 12) >> 11;
+    X2 = (AC2 * B6) >> 11;
+    X3 = X1 + X2;
+    B3 = ((AC1 * 4 + X3) << oss) >> 2;    
+
+    X1 = (AC3 * B6) >> 13;
+    X2 = (B1 * (B6 * B6) >> 12) >> 16;
+    X3 = ((X1 + X2) + 2) >> 2;
+    B4 = AC4 * (X3 + 32768) >> 15;
+    
+    B7 = (unsigned long)(UPressure - B3) * (50000 >> oss);
+    
+    if (B7 < 0x80000000) 
+        {
+        P = (2 * B7) / B4;
+        }
+    else 
+        {
+        P = 2* (B7 / B4);
+        }
+    X1 = (P >> 8) * (P >> 8);
+    X1 = (X1 * 3038) >> 16;
+    X2 = (-7357 * P) >> 16;    
+    P = P + ((X1 + X2 + 3791) >> 4);
+    Pressure = (float)P / 100.0;
+    }
+
+
+// Hoehe u.M. berechnen  (Druck in hPa)
+// -----------------------------------------------
+float BMP085::CalcAltitude(float Press)
+    {
+    float A = Press/1013.25;
+    float B = 1/5.25588;
+    float C = pow(A,B);
+    
+    C = 1 - C;
+    C = C / 22.5577e-6;
+    return C;
+    }
+    
+        
+// Drucksensor initialisieren  
+// -----------------------------------------------
+void BMP085::Init () 
+    {
+    AC1 = twi_readshort(BMP085_ADRESS, 0xaa);
+    AC2 = twi_readshort(BMP085_ADRESS, 0xac);
+    AC3 = twi_readshort(BMP085_ADRESS, 0xae);
+    AC4 = twi_readshort(BMP085_ADRESS, 0xb0);
+    AC5 = twi_readshort(BMP085_ADRESS, 0xb2);
+    AC6 = twi_readshort(BMP085_ADRESS, 0xb4);
+    B1 = twi_readshort(BMP085_ADRESS, 0xb6);
+    B2 = twi_readshort(BMP085_ADRESS, 0xb8);
+    MB = twi_readshort(BMP085_ADRESS, 0xba);
+    MC = twi_readshort(BMP085_ADRESS, 0xbc);
+    MD = twi_readshort(BMP085_ADRESS, 0xbe);
+    }
+    
+    
+// -----------------------------------------------
+unsigned short BMP085::twi_readshort (int id, int addr) {
+    unsigned short i;
+
+    i2c_.start();
+    i2c_.write(id);
+    i2c_.write(addr);
+
+    i2c_.start();
+    i2c_.write(id | 1);
+    i = i2c_.read(1) << 8;
+    i |= i2c_.read(0);
+    i2c_.stop();
+
+    return i;
+}
+
+
+// -----------------------------------------------
+unsigned long BMP085::twi_readlong (int id, int addr) {
+    unsigned long i;
+
+    i2c_.start();
+    i2c_.write(id);
+    i2c_.write(addr);
+
+    i2c_.start();
+    i2c_.write(id | 1);
+    i = i2c_.read(1) << 16;
+    i |= i2c_.read(1) << 8;
+    i |= i2c_.read(0);
+    i2c_.stop();
+
+    return i;
+}
+
+
+// -----------------------------------------------
+void BMP085::twi_writechar (int id, int addr, int dat) {
+
+    i2c_.start();
+    i2c_.write(id);
+    i2c_.write(addr);
+    i2c_.write(dat);
+    i2c_.stop();
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP085/BMP085.h	Sat Oct 13 08:59:03 2012 +0000
@@ -0,0 +1,34 @@
+ 
+#ifndef BMP085_I2C_H
+#define BMP085_I2C_H
+
+#include "mbed.h"
+
+class BMP085
+    {
+    private:
+        I2C i2c_;
+        
+    public:
+        BMP085(PinName sda, PinName scl);
+    
+        void    Update();
+        float   Temperature;
+        float   Pressure;
+        float   CalcAltitude(float Press);
+        short   oss;
+    
+    protected:
+        void Init();
+        unsigned short twi_readshort (int, int);
+        unsigned long twi_readlong (int, int);
+        void twi_writechar (int, int, int);
+    
+    
+    private:
+    
+        short AC1, AC2, AC3, B1, B2, MB, MC, MD;
+        unsigned short AC4, AC5, AC6;
+    };
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883/HMC5883.cpp	Sat Oct 13 08:59:03 2012 +0000
@@ -0,0 +1,166 @@
+#include "mbed.h"
+#include "HMC5883.h"
+
+#define I2CADR_W(ADR)           (ADR<<1&0xFE)
+#define I2CADR_R(ADR)           (ADR<<1|0x01)
+
+//Initialisieren
+HMC5883::HMC5883(PinName sda, PinName scl, Timer & GlobalTime_) :  i2c_(sda, scl),  GlobalTime(GlobalTime_)
+    {
+    Init();
+    }
+
+void HMC5883::Init()
+{
+    //Nullsetzen
+    MeasurementError= 0;
+    AutoCalibration= 0;
+    #pragma unroll
+    for(int i= 0; i < 3; i++)
+    {
+        RawMin[i]= -400;
+        RawMax[i]= 400;
+        Offset[i]= 0;
+        Scale[i]= 1.0;
+        RawMag[i]= 0;
+        Mag[i]= 0;
+    }
+    
+       
+    //HMC5883 initialisieren
+    char tx[4];
+    
+    //1 Sample pro Messung
+    //75Hz Output Rate
+    //Range: +- 1.3 Gauss
+    //Continuous-Measurement Mode
+    tx[0]= 0x00;
+    tx[1]= 0x78;    //Configuration Register A
+    tx[2]= 0x20;    //Configuration Register B
+    tx[3]= 0x00;    //Mode Register
+    i2c_.write(I2CADR_W(HMC5883_ADRESS), tx, 4);
+    
+    
+    Update();
+}
+
+//Rohdaten lesen
+void HMC5883::ReadRawData()
+{
+    //Drehrate aller Axen abholen    
+    char tx[1];
+    char rx[6];
+     
+    tx[0]= 0x03;
+    i2c_.write(I2CADR_W(HMC5883_ADRESS), tx, 1);
+    i2c_.read (I2CADR_R(HMC5883_ADRESS), rx, 6); 
+    
+    //Aus den einzelnen Bytes den 16-Bit-Wert zusammenbauen
+    short r[3];
+    r[0]= rx[0]<<8|rx[1];
+    r[1]= rx[4]<<8|rx[5];
+    r[2]= rx[2]<<8|rx[3];
+    
+    //Grober Messfehler?
+    if(r[0] == -4096
+    || r[1] == -4096
+    || r[2] == -4096)
+        MeasurementError= 1;
+    else
+    {
+        MeasurementError= 0;
+        RawMag[0]= r[0];
+        RawMag[1]= r[1];
+        RawMag[2]= r[2];
+    }
+}
+
+
+//Update-Methode
+void HMC5883::Update()
+{
+    //Rohdaten lesen
+    ReadRawData();
+    
+    if(AutoCalibration)
+    {
+        #pragma unroll
+        for(int i= 0; i < 3; i++)
+        {
+            //Neuer Min Max Wert?
+            RawMin[i]= RawMin[i] < RawMag[i] ? RawMin[i] : RawMag[i];
+            RawMax[i]= RawMax[i] > RawMag[i] ? RawMax[i] : RawMag[i];
+            
+            //Scale und Offset aus gesammelten Min Max Werten berechnen
+            //Die neue Untere und obere Grenze bilden -1 und +1
+            Scale[i]= 2.0 / (float)(RawMax[i]-RawMin[i]);
+            Offset[i]= 1.0 - (float)(RawMax[i]) * Scale[i];
+        }
+    }
+    
+    //Feldstaerke berechnen
+    Mag[0]= Scale[0] * (float)(RawMag[0]) + Offset[0];
+    Mag[1]= Scale[1] * (float)(RawMag[1]) + Offset[1];
+    Mag[2]= Scale[2] * (float)(RawMag[2]) + Offset[2];
+}
+
+
+//Kalibrieren
+void HMC5883::Calibrate(const short * pRawMin, const short * pRawMax)
+{
+    #pragma unroll
+    for(int i= 0; i < 3; i++)
+    {        
+        RawMin[i]= pRawMin[i];
+        RawMax[i]= pRawMax[i];
+    }
+    char AutoCalibrationBak= AutoCalibration;
+    AutoCalibration= 1;
+    Update();
+    AutoCalibration= AutoCalibrationBak;
+}
+
+void HMC5883::Calibrate(int s)
+{
+    char AutoCalibrationBak= AutoCalibration;
+    AutoCalibration= 1;
+    
+    //Ende der Kalibrierung in ms Millisekunden berechnen
+    int CalibEnd= GlobalTime.read_ms() + s*1000;
+    
+    while(GlobalTime.read_ms() < CalibEnd)
+    {
+        //Update erledigt alles
+        Update();
+    }
+    
+    AutoCalibration= AutoCalibrationBak;
+}
+
+// Winkel berechnen
+//---------------------------------------------------------------------------------------------------------------------------------------
+float HMC5883::getAngle(float x, float y)
+    {
+    #define Rad2Deg       57.295779513082320876798154814105
+    #define PI             3.1415926535897932384626433832795
+
+    float Heading;
+    float DecAngle; 
+    
+    DecAngle = 1.367 / Rad2Deg;         //Missweisung = Winkel zwischen geographischer und magnetischer Nordrichtung
+                                        //Bern ca. 1.367 Grad Ost
+                                        //http://www.swisstopo.admin.ch/internet/swisstopo/de/home/apps/calc/declination.html
+    Heading = atan2((float)y,(float)x);
+    
+    Heading += DecAngle;                //bei Ost-Deklination  += DecAngle, bei West-Deklination -= DecAngle
+ 
+    if(Heading < 0)  
+        Heading += 2*PI;                //korrigieren bei negativem Vorzeichen
+        
+    if(Heading > 2*PI)   
+        Heading -= 2*PI;                 //auf 2Pi begrenzen
+        
+    return  (Heading * 180/PI);         //Radianten in Grad konvertieren
+    }
+    
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC5883/HMC5883.h	Sat Oct 13 08:59:03 2012 +0000
@@ -0,0 +1,51 @@
+#ifndef HMC5883_H
+#define HMC5883_H
+
+//I2C Adresse
+#define HMC5883_ADRESS 0x1E
+
+
+class HMC5883
+{
+private:
+    I2C i2c_;
+    Timer & GlobalTime;
+        
+public:
+    //Calibration
+    char AutoCalibration;       //automatische Kalibrierung der Sensordaten
+    short RawMin[3], RawMax[3]; //gespeicherte Daten fuer die Auto-Kalibrierung
+    float Scale[3];             //jede Achse einzeln skalieren
+    float Offset[3];            //zum Schluss noch das Offset dazu
+    
+    //Feldstaerke auf allen drei Achsen
+    short RawMag[3];            //Rohdaten
+    float Mag[3];               //kalibrierte Rohdaten (nicht normalisiert)
+    
+    //Bei zu hoher Feldstaerke wird -4096 gelesen
+    //Wenn dies eine Achse betreffen sollte, ist diese Variable 1
+    short MeasurementError;
+    
+    float   getAngle(float x, float y);
+       
+    //Initialisieren
+    HMC5883(PinName sda, PinName scl, Timer & HMC5883_Time_);
+    void Init();
+
+private:    
+    //Rohdaten lesen
+    void ReadRawData();
+    
+public:
+    //Update-Methode
+    void Update();
+    
+    //Kalibrieren
+    //Fertige Daten benutzen. Scale[0] muss 1.0 sein!
+    void Calibrate(const short * pRawMin, const short * pRawMax);
+    
+    //Selbst auswerten, dauert s Sekunden lang
+    void Calibrate(int s);
+};
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Terminal-Emulation/terminal.cpp	Sat Oct 13 08:59:03 2012 +0000
@@ -0,0 +1,19 @@
+
+#include "terminal.h"
+#include "mbed.h"
+
+terminal::terminal(PinName tx, PinName rx) : Serial(tx, rx) 
+    {
+    }
+
+
+void terminal::cls() 
+    {
+    printf("\x1B[2J");
+    }
+
+
+void terminal::locate(int Spalte, int Zeile) 
+    {
+    printf("\x1B[%d;%dH", Zeile + 1, Spalte + 1);
+    }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Terminal-Emulation/terminal.h	Sat Oct 13 08:59:03 2012 +0000
@@ -0,0 +1,14 @@
+
+#include "mbed.h"
+
+#ifndef MBED_TERMINAL_H
+#define MBED_TERMINAL_H
+
+class terminal : public Serial 
+    {
+    public:
+        terminal(PinName tx, PinName rx);
+        void cls();
+        void locate(int column, int row);
+    };
+#endif

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