hmc5883l: changed to standard heading and added declination

Signed-off-by: Mihai Tudor Panu <mihai.t.panu@intel.com>

examples/hmc5883l.cxx

@@ -1,5 +1,6 @@
 /*
  * Author: Brendan Le Foll <brendan.le.foll@intel.com>
+ * Contributions: Mihai Tudor Panu <mihai.t.panu@intel.com>
  * Copyright (c) 2014 Intel Corporation.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
@@ -22,6 +23,7 @@
  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
  */
 
+#include <unistd.h>
 #include "hmc5883l.h"
 
 int
@@ -29,7 +31,17 @@ main(int argc, char **argv)
 {
 //! [Interesting]
     upm::Hmc5883l* compass = new upm::Hmc5883l(0);
-    fprintf(stdout, "heading: %f\n", compass->heading());
+    int *pos;
+
+    compass->set_declination(0.2749); // Set your declination from true north in radians
+
+    while(true){
+        compass->update(); // Update the coordinates
+        pos = compass->coordinates();
+        fprintf(stdout, "coor: %5d %5d %5d ", pos[0], pos[1], pos[2]);
+        fprintf(stdout, "heading: %5.2f direction: %3.2f\n", compass->heading(), compass->direction());
+        sleep(1);
+    }
 //! [Interesting]
 
     return 0;

src/hmc5883l/hmc5883l.cxx

@@ -1,5 +1,6 @@
 /*
  * Author: Brendan Le Foll <brendan.le.foll@intel.com>
+ * Contributions: Mihai Tudor Panu <mihai.t.panu@intel.com>
  * Copyright (c) 2014 Intel Corporation.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
@@ -94,7 +95,7 @@ Hmc5883l::Hmc5883l(int bus)
     Hmc5883l::update();
 }
 
-int
+mraa_result_t
 Hmc5883l::update(void)
 {
     mraa_i2c_address(m_i2c, HMC5883L_I2C_ADDR);
@@ -116,13 +117,17 @@ Hmc5883l::update(void)
 float
 Hmc5883l::direction(void)
 {
-    return atan2(m_coor[1] * SCALE_0_92_MG, m_coor[0] * SCALE_0_92_MG);
+    return atan2(m_coor[1] * SCALE_0_92_MG, m_coor[0] * SCALE_0_92_MG) + m_declination;
 }
 
 float
 Hmc5883l::heading(void)
 {
-    return Hmc5883l::direction() * 180/M_PI;
+    float dir = Hmc5883l::direction() * 180/M_PI;
+    if(dir < 0){
+        dir += 360.0;
+    }
+    return dir;
 }
 
 int*
@@ -130,3 +135,15 @@ Hmc5883l::coordinates(void)
 {
     return &m_coor[0];
 }
+
+void
+Hmc5883l::set_declination(float dec)
+{
+    m_declination = dec;
+}
+
+float
+Hmc5883l::get_declination()
+{
+    return m_declination;
+}

src/hmc5883l/hmc5883l.h

@@ -1,5 +1,6 @@
 /*
  * Author: Brendan Le Foll <brendan.le.foll@intel.com>
+ * Contributions: Mihai Tudor Panu <mihai.t.panu@intel.com>
  * Copyright (c) 2014 Intel Corporation.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
@@ -78,9 +79,22 @@ public:
      *
      * @return 0 for success
      */
-    int update();
+    mraa_result_t update();
+
+    /**
+     * Sets the magnetic declination for better calibration
+     */
+    void set_declination(float dec);
+
+    /**
+     * Gets the current magnetic declination value
+     *
+     * @return magnetic declination as a float
+     */
+    float get_declination();
 private:
     int m_coor[3];
+    float m_declination;
     uint8_t m_rx_tx_buf[MAX_BUFFER_LENGTH];
     mraa_i2c_context m_i2c;
 };