Revision 11:9bf69bc6df45, committed 2012-10-18

Comitter:

maetugr

Date:

Thu Oct 18 20:04:16 2012 +0000

Parent:

10:953afcbcebfc

Child:

12:67a06c9b69d5

Commit message:

Kompass immernoch nicht gut, vor Kalibrierungsberechnung

Changed in this revision

--- a/HMC5883/HMC5883.cpp	Wed Oct 17 08:37:08 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,174 +0,0 @@
-#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();
-    // MYINIT ----------
-    //Kompass kalibrieren  --> Problem fremde Magnetfelder!
-    AutoCalibration = 1;
-    //short MagRawMin[3]= {-400, -400, -400};     //Gespeicherte Werte verwenden
-    //short MagRawMax[3]= {400, 400, 400};
-    //Calibrate(MagRawMin, MagRawMax);
-    //Calibrate(20);
-    // MYINIT ----------
-    }
-
-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
-    }
-    
-

--- a/HMC5883/HMC5883.h	Wed Oct 17 08:37:08 2012 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,51 +0,0 @@
-#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

--- a/Sensors/Acc/ADXL345.cpp	Wed Oct 17 08:37:08 2012 +0000
+++ b/Sensors/Acc/ADXL345.cpp	Thu Oct 18 20:04:16 2012 +0000
@@ -60,12 +60,11 @@
     angle[2] =   Rad2Deg * acos((float)data[2] / R);
 }
 
-int ADXL345::writeReg(char address, char data){ 
-   int ack = 0;
+void ADXL345::writeReg(char address, char data){ 
    char tx[2];
    tx[0] = address;
    tx[1] = data;
-   return   ack | i2c.write(ADXL345_WRITE, tx, 2);   
+   i2c.write(ADXL345_WRITE, tx, 2);
 }
 
 char ADXL345::readReg(char address){   
@@ -81,12 +80,12 @@
     i2c.read(ADXL345_READ , output, size); //tell it where to store the data read
 }
 
-int ADXL345::setDataRate(char rate) {
+void ADXL345::setDataRate(char rate) {
     //Get the current register contents, so we don't clobber the power bit.
     char registerContents = readReg(ADXL345_BW_RATE_REG);
 
     registerContents &= 0x10;
     registerContents |= rate;
 
-    return writeReg(ADXL345_BW_RATE_REG, registerContents);
+    writeReg(ADXL345_BW_RATE_REG, registerContents);
 }
\ No newline at end of file

--- a/Sensors/Acc/ADXL345.h	Wed Oct 17 08:37:08 2012 +0000
+++ b/Sensors/Acc/ADXL345.h	Thu Oct 18 20:04:16 2012 +0000
@@ -78,10 +78,10 @@
        
     private:
         I2C i2c; // i2c object to communicate
-        int writeReg(byte reg, byte value); // write one single register to sensor
+        void writeReg(byte reg, byte value); // write one single register to sensor
         byte readReg(byte reg); // read one single register from sensor
         void readMultiReg(char startAddress, char* ptr_output, int size); // read multiple regs
-        int setDataRate(char rate); // data rate configuration (not only a reg to write)
+        void setDataRate(char rate); // data rate configuration (not only a reg to write)
 };
 
 #endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Sensors/Comp/HMC5883.cpp	Thu Oct 18 20:04:16 2012 +0000
@@ -0,0 +1,104 @@
+#include "mbed.h"
+#include "HMC5883.h"
+
+HMC5883::HMC5883(PinName sda, PinName scl) :  i2c(sda, scl)
+{       
+    // initialize HMC5883 for scaling
+    writeReg(HMC5883_CONF_REG_A, 0x19); // 8 samples, 75Hz output, test mode for scaling!
+    writeReg(HMC5883_CONF_REG_B, 0x20); // Gain for +- 1.3 gauss (earth compass ~0.6 gauss)
+    writeReg(HMC5883_MODE_REG, 0x00); // continuous measurement-mode
+    
+    // Scaling
+    for(int j = 0; j < 3; j++) // set all scales to 1 first so the measurement for scaling is not already scaled
+        scale[j] = 1;
+    
+    int data50[3] = {0,0,0}; // to save the 50 measurements
+    for(int i = 0; i < 50; i++) // measure 50 times the testmode value to get an average
+    {
+        read();
+        for(int j = 0; j < 3; j++)
+            data50[j] += data[j];
+    }
+    scale[0] = (1.16 * 1090)/(data50[0]/50.0); // value that it should be with selftest of 1.1 Gauss * 1090 LSB/Gauss   /   the value it is
+    scale[1] = (1.16 * 1090)/(data50[1]/50.0);
+    scale[2] = (1.08 * 1090)/(data50[2]/50.0);
+    
+    // set normal mode
+    writeReg(HMC5883_CONF_REG_A, 0x78); // 8 samples, 75Hz output, normal mode
+}
+
+void HMC5883::read()
+{
+    char buffer[6];
+    int dataint[3];
+    
+    readMultiReg(HMC5883_DATA_OUT_X_MSB, buffer, 6);
+    
+    // join MSB and LSB of X, Z and Y (yes, order is so stupid, see datasheet)
+    dataint[0] = (short) (buffer[0] << 8 | buffer[1]);
+    dataint[1] = (short) (buffer[4] << 8 | buffer[5]);
+    dataint[2] = (short) (buffer[2] << 8 | buffer[3]);
+    
+    for(int j = 0; j < 3; j++) {
+        Min[j]= Min[j] < dataint[j] ? Min[j] : dataint[j];
+        Max[j]= Max[j] > dataint[j] ? Max[j] : dataint[j];
+        data[j] = dataint[j]/1.090; //* scale[j];
+    }
+        
+    heading = 57.295779513082320876798154814105*atan2(data[1], data[0]);
+}
+
+void HMC5883::writeReg(char address, char data){ 
+    char tx[2];
+    tx[0] = address;
+    tx[1] = data;
+    i2c.write(I2CADR_W(HMC5883_ADDRESS), tx, 2);
+}
+
+void HMC5883::readMultiReg(char address, char* output, int size) {
+    i2c.write(I2CADR_W(HMC5883_ADDRESS), &address, 1); //tell it where to read from
+    i2c.read(I2CADR_R(HMC5883_ADDRESS) , output, size); //tell it where to store the data read
+}
+/*
+void HMC5883::Calibrate(int s)
+{
+    
+    //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/Sensors/Comp/HMC5883.h	Thu Oct 18 20:04:16 2012 +0000
@@ -0,0 +1,40 @@
+#ifndef HMC5883_H
+#define HMC5883_H
+
+#define HMC5883_CONF_REG_A      0x00
+#define HMC5883_CONF_REG_B      0x01
+#define HMC5883_MODE_REG        0x02
+#define HMC5883_DATA_OUT_X_MSB  0x03
+
+// I2C addresses
+#define HMC5883_ADDRESS 0x1E
+#define I2CADR_W(ADR)           (ADR << 1&0xFE) // ADR & 1111 1110
+#define I2CADR_R(ADR)           (ADR << 1|0x01) // ADR | 0000 0001
+
+class HMC5883
+{           
+    public:
+        HMC5883(PinName sda, PinName scl);
+        
+        //my
+        float data[3];
+        
+        void read();
+        void writeReg(char address, char data);
+        void readMultiReg(char address, char* output, int size);
+        
+        float scale[3]; //privatisieren
+        
+        float heading;
+        
+        int Min[3];
+        int Max[3];
+        
+        float   getAngle(float x, float y); //von Götti
+        
+    private:
+        I2C i2c;
+        
+};
+
+#endif

--- a/Sensors/Gyro/L3G4200D.cpp	Wed Oct 17 08:37:08 2012 +0000
+++ b/Sensors/Gyro/L3G4200D.cpp	Thu Oct 18 20:04:16 2012 +0000
@@ -9,7 +9,6 @@
 {
     i2c.frequency(400000);
     // Turns on the L3G4200D's gyro and places it in normal mode.
-    // 0x0F = 0b00001111
     // Normal power mode, all axes enabled
     
     //writeReg(L3G4200D_CTRL_REG2, 0x05); // control filter
@@ -79,22 +78,12 @@
     // to do slave-transmit subaddress updating.
     buffer[0] = L3G4200D_OUT_X_L | (1 << 7);
     i2c.write(L3G4200D_I2C_ADDRESS, buffer, 1); 
-
-//    Wire.requestFrom(GYR_ADDRESS, 6);
-//    while (Wire.available() < 6);
     
     i2c.read(L3G4200D_I2C_ADDRESS, buffer, 6); 
 
-    uint8_t xla = buffer[0];
-    uint8_t xha = buffer[1];
-    uint8_t yla = buffer[2];
-    uint8_t yha = buffer[3];
-    uint8_t zla = buffer[4];
-    uint8_t zha = buffer[5];
-
-    data[0] = (short) (xha << 8 | xla);
-    data[1] = (short) (yha << 8 | yla);
-    data[2] = (short) (zha << 8 | zla);
+    data[0] = (short) (buffer[1] << 8 | buffer[0]);
+    data[1] = (short) (buffer[3] << 8 | buffer[2]);
+    data[2] = (short) (buffer[5] << 8 | buffer[4]);
     
     //with offset of calibration
     for (int j = 0; j < 3; j++)

--- a/main.cpp	Wed Oct 17 08:37:08 2012 +0000
+++ b/main.cpp	Thu Oct 18 20:04:16 2012 +0000
@@ -23,7 +23,7 @@
 PC          pc(USBTX, USBRX, 57600);
 L3G4200D    Gyro(p28, p27);
 ADXL345     Acc(p28, p27);
-HMC5883     Comp(p28, p27, GlobalTimer);
+HMC5883     Comp(p28, p27);
 BMP085      Alt(p28, p27);
 RC_Channel  RC[] = {(p10), (p11), (p12), (p13)}; // noooo p19/p20!!!!
 Servo       Motor[] = {(p15), (p16), (p17), (p18)};
@@ -31,7 +31,7 @@
 // variables for loop
 unsigned long   dt = 0;
 unsigned long   time_loop = 0;
-float           angle[3] = {0,0,0}; // angle of calculated situation
+float           angle[3] = {0,0,0}; // angle 0: x,roll / 1: y,pitch / 2: z,yaw
 float           Comp_angle = 0;
 float           tempangle = 0;
 int             Motorvalue[3];
@@ -43,11 +43,11 @@
     // read data from sensors
     Gyro.read();
     Acc.read();
-    Comp.Update();
+    Comp.read();
     Alt.Update();
 
     //calculate angle for yaw from compass
-    Comp_angle = Comp.getAngle(Comp.Mag[0], Comp.Mag[1]);
+    //Comp_angle = Comp.getAngle(Comp.Mag[0], Comp.Mag[1]);
     
     // meassure dt
     dt = GlobalTimer.read_us() - time_loop; // time in us since last loop
@@ -63,7 +63,7 @@
     controller.setProcessValue(RC[3 -1].read());
     for (int j = 0; j < 4; j++)
         Motorvalue[j] = controller.compute(); // throttle
-        
+    
     for (int j = 0; j < 4; j++)
         Motor[j] = 1000 + 5*abs(angle[1]);//Motorvalue[j]; // set new motorspeeds
     pid.setProcessValue(angle[0]);
@@ -99,11 +99,15 @@
         pc.locate(10,10);
         pc.printf("Debug_Yaw:  Comp:%6.1f tempangle:%6.1f  ", Comp_angle, tempangle);
         pc.locate(10,12);
-        pc.printf("Comp_Raw: %6.1f %6.1f %6.1f   ", Comp.RawMag[0], Comp.RawMag[1], Comp.RawMag[2]);
+        pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.heading);
         pc.locate(10,13);
-        pc.printf("Comp_Mag: %6.1f %6.1f %6.1f   ", Comp.Mag[0], Comp.Mag[1], Comp.Mag[2]);
+        pc.printf("Comp_scale: %6.4f %6.4f %6.4f   ", Comp.scale[0], Comp.scale[1], Comp.scale[2]);
         pc.locate(10,15);
         pc.printf("pidtester: %6.1f   RC: %d %d %d %d     ", pidtester, RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
+        pc.locate(10,18);
+        pc.printf("Max: %d %d %d     ", Comp.Max[0], Comp.Max[1], Comp.Max[2]);
+        pc.locate(10,19);
+        pc.printf("Min: %d %d %d     ", Comp.Min[0], Comp.Min[1], Comp.Min[2]);
         LEDs.rollnext();
     }
 }