HMC58X3.cpp

00001 /*
00002 HMC58X3.cpp - Interface a Honeywell HMC58X3 or HMC5883L magnetometer to an mbed via i2c
00003 Copyright (C) 2011 Fabio Varesano <fvaresano@yahoo.it>
00004 ported for mbed by Aloïs Wolff (wolffalois@gmail.com)
00005 
00006 Based on:
00007 http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1274748346
00008  Modification/extension of the following by E.J.Muller
00009 http://eclecti.cc/hardware/hmc5843-magnetometer-library-for-arduino
00010  Copyright (c) 2009 Nirav Patel,
00011 
00012 The above were based on:
00013 http://www.sparkfun.com/datasheets/Sensors/Magneto/HMC58X3-v11.c
00014 http://www.atmel.com/dyn/resources/prod_documents/doc2545.pdf
00015 
00016 
00017 This program is free software: you can redistribute it and/or modify
00018 it under the terms of the version 3 GNU General Public License as
00019 published by the Free Software Foundation.
00020 
00021 This program is distributed in the hope that it will be useful,
00022 but WITHOUT ANY WARRANTY; without even the implied warranty of
00023 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00024 GNU General Public License for more details.
00025 
00026 You should have received a copy of the GNU General Public License
00027 along with this program.  If not, see <http://www.gnu.org/licenses/>.
00028 
00029 */
00030 
00031 //#define DEBUG (1)
00032 
00033 #include "mbed.h"
00034 #include "HMC58X3.h"
00035 #include <new>
00036 //#include <DebugUtils.h>
00037 #define DEBUG_PRINT
00038 
00039 
00040 
00041 
00042 /*!
00043     Counts/milli-gauss per gain for the self test bias current.
00044 */
00045 #if defined(ISHMC5843)
00046 const int counts_per_milligauss[8]= {
00047     1620,
00048     1300,
00049     970,
00050     780,
00051     530,
00052     460,
00053     390,
00054     280
00055 };
00056 #else // HMC5883L
00057 const int counts_per_milligauss[8]= {
00058     1370,
00059     1090,
00060     820,
00061     660,
00062     440,
00063     390,
00064     330,
00065     230
00066 };
00067 #endif
00068 
00069 
00070 
00071 /* PUBLIC METHODS */
00072 
00073 //HMC58X3::HMC58X3(PinName sda, PinName scl): i2c(sda, scl)
00074 HMC58X3::HMC58X3(I2C i2c_):i2c(i2c_)
00075 {
00076     //this->i2c= i2c_;
00077     x_scale=1.0F;
00078     y_scale=1.0F;
00079     z_scale=1.0F;
00080 
00081 }
00082 
00083 
00084 void HMC58X3::init(bool setmode)
00085 {
00086     // note that we don't initialize Wire here.
00087     // You'll have to do that in setup() in your Arduino program
00088     wait_ms(5); // you need to wait at least 5ms after power on to initialize
00089     if (setmode) {
00090         setMode(0);
00091     }
00092 
00093 
00094 
00095     writeReg(HMC58X3_R_CONFA, 0x70); // 8 samples averaged, 75Hz frequency, no artificial bias.
00096     writeReg(HMC58X3_R_CONFB, 0xA0);
00097     writeReg(HMC58X3_R_MODE, 0x00);
00098 }
00099 
00100 
00101 void HMC58X3::setMode(unsigned char mode)
00102 {
00103     if (mode > 2) {
00104         return;
00105     }
00106 
00107     writeReg(HMC58X3_R_MODE, mode);
00108     wait_ms(100);
00109 }
00110 
00111 /*
00112     Calibrate which has a few weaknesses.
00113     1. Uses wrong gain for first reading.
00114     2. Uses max instead of max of average when normalizing the axis to one another.
00115     3. Doesn't use neg bias. (possible improvement in measurement).
00116 */
00117 void HMC58X3::calibrate(unsigned char gain)
00118 {
00119     x_scale=1; // get actual values
00120     y_scale=1;
00121     z_scale=1;
00122     writeReg(HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to pos bias
00123     setGain(gain);
00124     float x, y, z, mx=0, my=0, mz=0, t=10;
00125 
00126     for (int i=0; i<(int)t; i++) {
00127         setMode(1);
00128         getValues(&x,&y,&z);
00129         if (x>mx) mx=x;
00130         if (y>my) my=y;
00131         if (z>mz) mz=z;
00132     }
00133 
00134     float max=0;
00135     if (mx>max) max=mx;
00136     if (my>max) max=my;
00137     if (mz>max) max=mz;
00138     x_max=mx;
00139     y_max=my;
00140     z_max=mz;
00141     x_scale=max/mx; // calc scales
00142     y_scale=max/my;
00143     z_scale=max/mz;
00144 
00145     writeReg(HMC58X3_R_CONFA, 0x010); // set RegA/DOR back to default
00146 }   // calibrate().
00147 
00148 /*!
00149     \brief Calibrate using the self test operation.
00150 
00151     Average the values using bias mode to obtain the scale factors.
00152 
00153     \param gain [in] Gain setting for the sensor. See data sheet.
00154     \param n_samples [in] Number of samples to average together while applying the positive and negative bias.
00155     \return Returns false if any of the following occurs:
00156         # Invalid input parameters. (gain>7 or n_samples=0).
00157         # Id registers are wrong for the compiled device. Unfortunately, we can't distinguish between HMC5843 and HMC5883L.
00158         # Calibration saturates during the positive or negative bias on any of the readings.
00159         # Readings are outside of the expected range for bias current.
00160 */
00161 bool HMC58X3::calibrate(unsigned char gain,unsigned int n_samples)
00162 {
00163     int xyz[3];                     // 16 bit integer values for each axis.
00164     long int xyz_total[3]= {0,0,0}; // 32 bit totals so they won't overflow.
00165     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.
00166     char id[3];                     // Three identification registers should return 'H43'.
00167     long int low_limit, high_limit;
00168     /*
00169         Make sure we are talking to the correct device.
00170         Hard to believe Honeywell didn't change the identifier.
00171     */
00172     if ((8>gain) && (0<n_samples)) { // Notice this allows gain setting of 7 which the data sheet warns against.
00173         getID(id);
00174         if (('H' == id[0]) && ('4' == id[1]) && ('3' == id[2])) {
00175             /*
00176                 Use the positive bias current to impose a known field on each axis.
00177                 This field depends on the device and the axis.
00178             */
00179             writeReg(HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to pos bias
00180             /*
00181                 Note that the  very first measurement after a gain change maintains the same gain as the previous setting.
00182                 The new gain setting is effective from the second measurement and on.
00183             */
00184             setGain(gain);
00185             setMode(1);                         // Change to single measurement mode.
00186             getRaw(&xyz[0],&xyz[1],&xyz[2]);    // Get the raw values and ignore since this reading may use previous gain.
00187 
00188             for (unsigned int i=0; i<n_samples; i++) {
00189                 setMode(1);
00190                 getRaw(&xyz[0],&xyz[1],&xyz[2]);   // Get the raw values in case the scales have already been changed.
00191                 /*
00192                     Since the measurements are noisy, they should be averaged rather than taking the max.
00193                 */
00194                 xyz_total[0]+=xyz[0];
00195                 xyz_total[1]+=xyz[1];
00196                 xyz_total[2]+=xyz[2];
00197                 /*
00198                     Detect saturation.
00199                 */
00200                 if (-(1<<12) >= min(xyz[0],min(xyz[1],xyz[2]))) {
00201                     DEBUG_PRINT("HMC58x3 Self test saturated. Increase range.");
00202                     bret=false;
00203                     break;  // Breaks out of the for loop.  No sense in continuing if we saturated.
00204                 }
00205             }
00206             /*
00207                 Apply the negative bias. (Same gain)
00208             */
00209             writeReg(HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to negative bias.
00210             for (unsigned int i=0; i<n_samples; i++) {
00211                 setMode(1);
00212                 getRaw(&xyz[0],&xyz[1],&xyz[2]);   // Get the raw values in case the scales have already been changed.
00213                 /*
00214                     Since the measurements are noisy, they should be averaged.
00215                 */
00216                 xyz_total[0]-=xyz[0];
00217                 xyz_total[1]-=xyz[1];
00218                 xyz_total[2]-=xyz[2];
00219                 /*
00220                     Detect saturation.
00221                 */
00222                 if (-(1<<12) >= min(xyz[0],min(xyz[1],xyz[2]))) {
00223                     DEBUG_PRINT("HMC58x3 Self test saturated. Increase range.");
00224                     bret=false;
00225                     break;  // Breaks out of the for loop.  No sense in continuing if we saturated.
00226                 }
00227             }
00228             /*
00229                 Compare the values against the expected self test bias gauss.
00230                 Notice, the same limits are applied to all axis.
00231             */
00232             low_limit =SELF_TEST_LOW_LIMIT *counts_per_milligauss[gain]*2*n_samples;
00233             high_limit=SELF_TEST_HIGH_LIMIT*counts_per_milligauss[gain]*2*n_samples;
00234 
00235             if ((true==bret) &&
00236                     (low_limit <= xyz_total[0]) && (high_limit >= xyz_total[0]) &&
00237                     (low_limit <= xyz_total[1]) && (high_limit >= xyz_total[1]) &&
00238                     (low_limit <= xyz_total[2]) && (high_limit >= xyz_total[2]) ) {
00239                 /*
00240                     Successful calibration.
00241                     Normalize the scale factors so all axis return the same range of values for the bias field.
00242                     Factor of 2 is from summation of total of n_samples from both positive and negative bias.
00243                 */
00244                 x_scale=(counts_per_milligauss[gain]*(HMC58X3_X_SELF_TEST_GAUSS*2))/(xyz_total[0]/n_samples);
00245                 y_scale=(counts_per_milligauss[gain]*(HMC58X3_Y_SELF_TEST_GAUSS*2))/(xyz_total[1]/n_samples);
00246                 z_scale=(counts_per_milligauss[gain]*(HMC58X3_Z_SELF_TEST_GAUSS*2))/(xyz_total[2]/n_samples);
00247             } else {
00248                 DEBUG_PRINT("HMC58x3 Self test out of range.");
00249                 bret=false;
00250             }
00251             writeReg(HMC58X3_R_CONFA, 0x010); // set RegA/DOR back to default.
00252         } else {
00253 #if defined(ISHMC5843)
00254             DEBUG_PRINT("HMC5843 failed id check.");
00255 #else
00256             DEBUG_PRINT("HMC5883L failed id check.");
00257 #endif
00258             bret=false;
00259         }
00260     } else {
00261         /*
00262             Bad input parameters.
00263         */
00264         DEBUG_PRINT("HMC58x3 Bad parameters.");
00265         bret=false;
00266     }
00267     return(bret);
00268 }   //  calibrate().
00269 
00270 // set data output rate
00271 // 0-6, 4 default, normal operation assumed
00272 void HMC58X3::setDOR(unsigned char DOR)
00273 {
00274     if (DOR>6) return;
00275     writeReg(HMC58X3_R_CONFA,DOR<<2);
00276 }
00277 
00278 
00279 void HMC58X3::setGain(unsigned char gain)
00280 {
00281     // 0-7, 1 default
00282     if (gain > 7) return;
00283     writeReg(HMC58X3_R_CONFB, gain << 5);
00284 }
00285 
00286 
00287 void HMC58X3::writeReg(unsigned char reg, unsigned char val)
00288 {
00289     i2c.start();
00290     i2c.write(reg);        // send register address
00291     i2c.write(val);        // send value to write
00292     i2c.stop(); //end transmission
00293 }
00294 
00295 
00296 void HMC58X3::getValues(int *x,int *y,int *z)
00297 {
00298     float fx,fy,fz;
00299     getValues(&fx,&fy,&fz);
00300     *x= (int) (fx + 0.5);
00301     *y= (int) (fy + 0.5);
00302     *z= (int) (fz + 0.5);
00303 }
00304 
00305 
00306 void HMC58X3::getValues(float *x,float *y,float *z)
00307 {
00308     int xr,yr,zr;
00309 
00310     getRaw(&xr, &yr, &zr);
00311     *x= ((float) xr) / x_scale;
00312     *y = ((float) yr) / y_scale;
00313     *z = ((float) zr) / z_scale;
00314 }
00315 
00316 
00317 void HMC58X3::getRaw(int *x,int *y,int *z)
00318 {
00319 
00320     char cmd[2];
00321     char data[6];
00322     cmd[0] = 0x03;
00323 
00324     i2c.write(I2C_ADDRESS, cmd, 1, true); // set the pointer to the start of x
00325     i2c.read(I2C_ADDRESS, data, 6, false);
00326 
00327     // read out the 3 values, 2 bytes each.
00328     *x = int16_t(((unsigned char)data[0] << 8) | (unsigned char)data[1]);
00329 #ifdef ISHMC5843
00330     *y = int16_t(((unsigned char)data[1*2] << 8) | (unsigned char)data[1*2+1]);
00331     *z = int16_t(((unsigned char)data[2*2] << 8) | (unsigned char)data[2*2+1]);
00332 #else // the Z registers comes before the Y registers in the HMC5883L
00333     *z = int16_t(((unsigned char)data[1*2] << 8) | (unsigned char)data[1*2+1]);
00334     *y = int16_t(((unsigned char)data[2*2] << 8) | (unsigned char)data[2*2+1]);
00335 #endif
00336     // the HMC58X3 will automatically wrap around on the next request
00337 
00338 
00339 }
00340 
00341 
00342 void HMC58X3::getValues(float *xyz)
00343 {
00344     getValues(&xyz[0], &xyz[1], &xyz[2]);
00345 }
00346 
00347 /*!
00348     \brief Retrieve the value of the three ID registers.
00349 
00350     Note:  Both the HMC5843 and HMC5883L have the same 'H43' identification register values. (Looks like someone at Honeywell screwed up.)
00351 
00352     \param id [out] Returns the three id register values.
00353 */
00354 void HMC58X3::getID(char id[3])
00355 {
00356     i2c.start();
00357     i2c.write(HMC58X3_R_IDA);             // Will start reading registers starting from Identification Register A.
00358 
00359     id[0] = i2c.read(0);
00360     id[1] = i2c.read(0);
00361     id[2] = i2c.read(0);
00362 
00363     i2c.stop();
00364 }   // getID().
00365 
00366 int HMC58X3::min (int a, int b)
00367 {
00368     return !(b<a)?a:b;     // or: return !comp(b,a)?a:b; for version (2)
00369 }