Diff: Sensors/Comp/HMC5883.cpp

Revision:

12:67a06c9b69d5

Parent:

11:9bf69bc6df45

Child:

13:4737ee9ebfee

--- a/Sensors/Comp/HMC5883.cpp	Thu Oct 18 20:04:16 2012 +0000
+++ b/Sensors/Comp/HMC5883.cpp	Sat Oct 20 17:28:28 2012 +0000
@@ -1,14 +1,23 @@
 #include "mbed.h"
 #include "HMC5883.h"
 
-HMC5883::HMC5883(PinName sda, PinName scl) :  i2c(sda, scl)
-{       
+HMC5883::HMC5883(PinName sda, PinName scl) :  local("local"), i2c(sda, scl)
+{
+    // load calibration values
+    FILE *fp = fopen("/local/compass.txt", "r");
+    for(int i = 0; i < 3; i++)
+        fscanf(fp, "%f", &scale[i]);
+    for(int i = 0; i < 3; i++)
+        fscanf(fp, "%f", &offset[i]);
+    fclose(fp);
+    
     // 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
+    /*
+    // Scaling with testmode (not important, just from data sheet)
     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;
     
@@ -22,6 +31,7 @@
     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
@@ -29,23 +39,49 @@
 
 void HMC5883::read()
 {
+    readraw();
+    for(int i = 0; i < 3; i++)
+        data[i] = scale[i] * (float)(raw[i]) + offset[i];
+}
+
+void HMC5883::calibrate(int s)
+{
+    Timer calibrate_timer;
+    calibrate_timer.start();
+    
+    while(calibrate_timer.read() < s)
+    {
+        readraw();
+        for(int i = 0; i < 3; i++) {
+            Min[i]= Min[i] < raw[i] ? Min[i] : raw[i];
+            Max[i]= Max[i] > raw[i] ? Max[i] : raw[i];
+    
+            //Scale und Offset aus gesammelten Min Max Werten berechnen
+            //Die neue Untere und obere Grenze bilden -1 und +1
+            scale[i]= 2000 / (float)(Max[i]-Min[i]);
+            offset[i]= 1000 - (float)(Max[i]) * scale[i];
+        }
+    }
+    
+    // save values
+    FILE *fp = fopen("/local/compass.txt", "w");
+    for(int i = 0; i < 3; i++)
+        fprintf(fp, "%f\r\n", scale[i]);
+    for(int i = 0; i < 3; i++)
+        fprintf(fp, "%f\r\n", offset[i]);
+    fclose(fp);
+}
+
+void HMC5883::readraw()
+{
     char buffer[6];
-    int dataint[3];
     
-    readMultiReg(HMC5883_DATA_OUT_X_MSB, buffer, 6);
+    readMultiReg(HMC5883_DATA_OUT_X_MSB, buffer, 6); // read the raw data from I2C
     
     // 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]);
+    raw[0] = (short) (buffer[0] << 8 | buffer[1]);
+    raw[1] = (short) (buffer[4] << 8 | buffer[5]);
+    raw[2] = (short) (buffer[2] << 8 | buffer[3]);
 }
 
 void HMC5883::writeReg(char address, char data){ 
@@ -59,46 +95,27 @@
     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)
+
+float HMC5883::get_angle()
 {
-    
-    //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; 
+    
+    Heading = Rad2Deg * atan2(data[0],data[1]);
     
-    DecAngle = 1.367 / Rad2Deg;         //Missweisung = Winkel zwischen geographischer und magnetischer Nordrichtung
+    Heading += 1.367;                   //bei Ost-Deklination  += DecAngle, bei West-Deklination -= DecAngle
+                                        //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
+        Heading += 360;                 // minimum 0 degree
         
-    if(Heading > 2*PI)   
-        Heading -= 2*PI;                 //auf 2Pi begrenzen
+    if(Heading > 360)   
+        Heading -= 360;                 // maximum 360 degree
         
-    return  (Heading * 180/PI);         //Radianten in Grad konvertieren
-    }
+    return Heading;
+}