Diff: HMC58X3.cpp

Revision:

0:d84e1c7e4e86

Child:

1:72ecf7399250

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HMC58X3.cpp	Mon Jun 24 16:18:52 2013 +0000
@@ -0,0 +1,368 @@
+/*
+HMC58X3.cpp - Interface a Honeywell HMC58X3 or HMC5883L magnetometer to an mbed via i2c
+Copyright (C) 2011 Fabio Varesano <fvaresano@yahoo.it>
+ported for mbed by Aloïs Wolff (wolffalois@gmail.com)
+
+Based on:
+http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1274748346
+ Modification/extension of the following by E.J.Muller
+http://eclecti.cc/hardware/hmc5843-magnetometer-library-for-arduino
+ Copyright (c) 2009 Nirav Patel,
+
+The above were based on:
+http://www.sparkfun.com/datasheets/Sensors/Magneto/HMC58X3-v11.c
+http://www.atmel.com/dyn/resources/prod_documents/doc2545.pdf
+
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the version 3 GNU General Public License as
+published by the Free Software Foundation.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+*/
+
+//#define DEBUG (1)
+
+#include "mbed.h"
+#include "HMC58X3.h"
+#include <new>
+//#include <DebugUtils.h>
+#define DEBUG_PRINT
+
+
+
+
+/*!
+    Counts/milli-gauss per gain for the self test bias current.
+*/
+#if defined(ISHMC5843)
+const int counts_per_milligauss[8]= {
+    1620,
+    1300,
+    970,
+    780,
+    530,
+    460,
+    390,
+    280
+};
+#else // HMC5883L
+const int counts_per_milligauss[8]= {
+    1370,
+    1090,
+    820,
+    660,
+    440,
+    390,
+    330,
+    230
+};
+#endif
+
+
+
+/* PUBLIC METHODS */
+
+HMC58X3::HMC58X3(PinName sda, PinName scl): i2c(sda, scl)
+{
+    
+    x_scale=1.0F;
+    y_scale=1.0F;
+    z_scale=1.0F;
+
+}
+
+
+void HMC58X3::init(bool setmode)
+{
+    // note that we don't initialize Wire here.
+    // You'll have to do that in setup() in your Arduino program
+    wait_ms(5); // you need to wait at least 5ms after power on to initialize
+    if (setmode) {
+        setMode(0);
+    }
+
+
+
+    writeReg(HMC58X3_R_CONFA, 0x70); // 8 samples averaged, 75Hz frequency, no artificial bias.
+    writeReg(HMC58X3_R_CONFB, 0xA0);
+    writeReg(HMC58X3_R_MODE, 0x00);
+}
+
+
+void HMC58X3::setMode(unsigned char mode)
+{
+    if (mode > 2) {
+        return;
+    }
+
+    writeReg(HMC58X3_R_MODE, mode);
+    wait_ms(100);
+}
+
+/*
+    Calibrate which has a few weaknesses.
+    1. Uses wrong gain for first reading.
+    2. Uses max instead of max of average when normalizing the axis to one another.
+    3. Doesn't use neg bias. (possible improvement in measurement).
+*/
+void HMC58X3::calibrate(unsigned char gain)
+{
+    x_scale=1; // get actual values
+    y_scale=1;
+    z_scale=1;
+    writeReg(HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to pos bias
+    setGain(gain);
+    float x, y, z, mx=0, my=0, mz=0, t=10;
+
+    for (int i=0; i<(int)t; i++) {
+        setMode(1);
+        getValues(&x,&y,&z);
+        if (x>mx) mx=x;
+        if (y>my) my=y;
+        if (z>mz) mz=z;
+    }
+
+    float max=0;
+    if (mx>max) max=mx;
+    if (my>max) max=my;
+    if (mz>max) max=mz;
+    x_max=mx;
+    y_max=my;
+    z_max=mz;
+    x_scale=max/mx; // calc scales
+    y_scale=max/my;
+    z_scale=max/mz;
+
+    writeReg(HMC58X3_R_CONFA, 0x010); // set RegA/DOR back to default
+}   // calibrate().
+
+/*!
+    \brief Calibrate using the self test operation.
+
+    Average the values using bias mode to obtain the scale factors.
+
+    \param gain [in] Gain setting for the sensor. See data sheet.
+    \param n_samples [in] Number of samples to average together while applying the positive and negative bias.
+    \return Returns false if any of the following occurs:
+        # Invalid input parameters. (gain>7 or n_samples=0).
+        # Id registers are wrong for the compiled device. Unfortunately, we can't distinguish between HMC5843 and HMC5883L.
+        # Calibration saturates during the positive or negative bias on any of the readings.
+        # Readings are outside of the expected range for bias current.
+*/
+bool HMC58X3::calibrate(unsigned char gain,unsigned int n_samples)
+{
+    int xyz[3];                     // 16 bit integer values for each axis.
+    long int xyz_total[3]= {0,0,0}; // 32 bit totals so they won't overflow.
+    bool bret=true;                 // Function return value.  Will return false if the wrong identifier is returned, saturation is detected or response is out of range to self test bias.
+    char id[3];                     // Three identification registers should return 'H43'.
+    long int low_limit, high_limit;
+    /*
+        Make sure we are talking to the correct device.
+        Hard to believe Honeywell didn't change the identifier.
+    */
+    if ((8>gain) && (0<n_samples)) { // Notice this allows gain setting of 7 which the data sheet warns against.
+        getID(id);
+        if (('H' == id[0]) && ('4' == id[1]) && ('3' == id[2])) {
+            /*
+                Use the positive bias current to impose a known field on each axis.
+                This field depends on the device and the axis.
+            */
+            writeReg(HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to pos bias
+            /*
+                Note that the  very first measurement after a gain change maintains the same gain as the previous setting.
+                The new gain setting is effective from the second measurement and on.
+            */
+            setGain(gain);
+            setMode(1);                         // Change to single measurement mode.
+            getRaw(&xyz[0],&xyz[1],&xyz[2]);    // Get the raw values and ignore since this reading may use previous gain.
+
+            for (unsigned int i=0; i<n_samples; i++) {
+                setMode(1);
+                getRaw(&xyz[0],&xyz[1],&xyz[2]);   // Get the raw values in case the scales have already been changed.
+                /*
+                    Since the measurements are noisy, they should be averaged rather than taking the max.
+                */
+                xyz_total[0]+=xyz[0];
+                xyz_total[1]+=xyz[1];
+                xyz_total[2]+=xyz[2];
+                /*
+                    Detect saturation.
+                */
+                if (-(1<<12) >= min(xyz[0],min(xyz[1],xyz[2]))) {
+                    DEBUG_PRINT("HMC58x3 Self test saturated. Increase range.");
+                    bret=false;
+                    break;  // Breaks out of the for loop.  No sense in continuing if we saturated.
+                }
+            }
+            /*
+                Apply the negative bias. (Same gain)
+            */
+            writeReg(HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to negative bias.
+            for (unsigned int i=0; i<n_samples; i++) {
+                setMode(1);
+                getRaw(&xyz[0],&xyz[1],&xyz[2]);   // Get the raw values in case the scales have already been changed.
+                /*
+                    Since the measurements are noisy, they should be averaged.
+                */
+                xyz_total[0]-=xyz[0];
+                xyz_total[1]-=xyz[1];
+                xyz_total[2]-=xyz[2];
+                /*
+                    Detect saturation.
+                */
+                if (-(1<<12) >= min(xyz[0],min(xyz[1],xyz[2]))) {
+                    DEBUG_PRINT("HMC58x3 Self test saturated. Increase range.");
+                    bret=false;
+                    break;  // Breaks out of the for loop.  No sense in continuing if we saturated.
+                }
+            }
+            /*
+                Compare the values against the expected self test bias gauss.
+                Notice, the same limits are applied to all axis.
+            */
+            low_limit =SELF_TEST_LOW_LIMIT *counts_per_milligauss[gain]*2*n_samples;
+            high_limit=SELF_TEST_HIGH_LIMIT*counts_per_milligauss[gain]*2*n_samples;
+
+            if ((true==bret) &&
+                    (low_limit <= xyz_total[0]) && (high_limit >= xyz_total[0]) &&
+                    (low_limit <= xyz_total[1]) && (high_limit >= xyz_total[1]) &&
+                    (low_limit <= xyz_total[2]) && (high_limit >= xyz_total[2]) ) {
+                /*
+                    Successful calibration.
+                    Normalize the scale factors so all axis return the same range of values for the bias field.
+                    Factor of 2 is from summation of total of n_samples from both positive and negative bias.
+                */
+                x_scale=(counts_per_milligauss[gain]*(HMC58X3_X_SELF_TEST_GAUSS*2))/(xyz_total[0]/n_samples);
+                y_scale=(counts_per_milligauss[gain]*(HMC58X3_Y_SELF_TEST_GAUSS*2))/(xyz_total[1]/n_samples);
+                z_scale=(counts_per_milligauss[gain]*(HMC58X3_Z_SELF_TEST_GAUSS*2))/(xyz_total[2]/n_samples);
+            } else {
+                DEBUG_PRINT("HMC58x3 Self test out of range.");
+                bret=false;
+            }
+            writeReg(HMC58X3_R_CONFA, 0x010); // set RegA/DOR back to default.
+        } else {
+#if defined(ISHMC5843)
+            DEBUG_PRINT("HMC5843 failed id check.");
+#else
+            DEBUG_PRINT("HMC5883L failed id check.");
+#endif
+            bret=false;
+        }
+    } else {
+        /*
+            Bad input parameters.
+        */
+        DEBUG_PRINT("HMC58x3 Bad parameters.");
+        bret=false;
+    }
+    return(bret);
+}   //  calibrate().
+
+// set data output rate
+// 0-6, 4 default, normal operation assumed
+void HMC58X3::setDOR(unsigned char DOR)
+{
+    if (DOR>6) return;
+    writeReg(HMC58X3_R_CONFA,DOR<<2);
+}
+
+
+void HMC58X3::setGain(unsigned char gain)
+{
+    // 0-7, 1 default
+    if (gain > 7) return;
+    writeReg(HMC58X3_R_CONFB, gain << 5);
+}
+
+
+void HMC58X3::writeReg(unsigned char reg, unsigned char val)
+{
+    i2c.start();
+    i2c.write(reg);        // send register address
+    i2c.write(val);        // send value to write
+    i2c.stop(); //end transmission
+}
+
+
+void HMC58X3::getValues(int *x,int *y,int *z)
+{
+    float fx,fy,fz;
+    getValues(&fx,&fy,&fz);
+    *x= (int) (fx + 0.5);
+    *y= (int) (fy + 0.5);
+    *z= (int) (fz + 0.5);
+}
+
+
+void HMC58X3::getValues(float *x,float *y,float *z)
+{
+    int xr,yr,zr;
+
+    getRaw(&xr, &yr, &zr);
+    *x= ((float) xr) / x_scale;
+    *y = ((float) yr) / y_scale;
+    *z = ((float) zr) / z_scale;
+}
+
+
+void HMC58X3::getRaw(int *x,int *y,int *z)
+{
+
+    char cmd[2];
+    char data[6];
+    cmd[0] = 0x03;
+
+    i2c.write(I2C_ADDRESS, cmd, 1, true); // set the pointer to the start of x
+    i2c.read(I2C_ADDRESS, data, 6, false);
+
+    // read out the 3 values, 2 bytes each.
+    *x = int16_t(((unsigned char)data[0] << 8) | (unsigned char)data[1]);
+#ifdef ISHMC5843
+    *y = int16_t(((unsigned char)data[1*2] << 8) | (unsigned char)data[1*2+1]);
+    *z = int16_t(((unsigned char)data[2*2] << 8) | (unsigned char)data[2*2+1]);
+#else // the Z registers comes before the Y registers in the HMC5883L
+    *z = int16_t(((unsigned char)data[1*2] << 8) | (unsigned char)data[1*2+1]);
+    *y = int16_t(((unsigned char)data[2*2] << 8) | (unsigned char)data[2*2+1]);
+#endif
+    // the HMC58X3 will automatically wrap around on the next request
+
+
+}
+
+
+void HMC58X3::getValues(float *xyz)
+{
+    getValues(&xyz[0], &xyz[1], &xyz[2]);
+}
+
+/*!
+    \brief Retrieve the value of the three ID registers.
+
+    Note:  Both the HMC5843 and HMC5883L have the same 'H43' identification register values. (Looks like someone at Honeywell screwed up.)
+
+    \param id [out] Returns the three id register values.
+*/
+void HMC58X3::getID(char id[3])
+{
+    i2c.start();
+    i2c.write(HMC58X3_R_IDA);             // Will start reading registers starting from Identification Register A.
+
+    id[0] = i2c.read(0);
+    id[1] = i2c.read(0);
+    id[2] = i2c.read(0);
+
+    i2c.stop();
+}   // getID().
+
+int HMC58X3::min (int a, int b)
+{
+    return !(b<a)?a:b;     // or: return !comp(b,a)?a:b; for version (2)
+}
\ No newline at end of file