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 "HMC58X3.h"
00034 #include "mbed.h"
00035 #include "MODI2C.h"
00036 #include "rtos.h"
00037 #include <new>
00038 //#include <DebugUtils.h>
00039 #define DEBUG_PRINT
00040 
00041 /*!
00042     Counts/milli-gauss per gain for the self test bias current.
00043 */
00044 
00045 const int counts_per_milligauss[8]= {
00046     1370,
00047     1090,
00048     820,
00049     660,
00050     440,
00051     390,
00052     330,
00053     230
00054 };
00055 
00056 
00057 
00058 
00059 /* PUBLIC METHODS */
00060 
00061 
00062 //HMC58X3::HMC58X3(PinName sda, PinName scl): i2c(sda, scl)
00063 HMC58X3::HMC58X3():i2c(I2C_SDA,I2C_SCL),_thread(osPriorityAboveNormal),sem(0)
00064 {
00065     //this->i2c= i2c_;
00066     x_scale=1.0F;
00067     y_scale=1.0F;
00068     z_scale=1.0F;
00069 
00070     HMC58X3_R_IDA = 10;
00071     
00072     _thread.start(callback(&HMC58X3::samplingthread_stub, (void *) this));
00073 }
00074 
00075 void HMC58X3::samplingthread_stub(void const *p) {
00076   HMC58X3 *instance = (HMC58X3*)p;
00077   instance->samplingthread();
00078 }
00079 
00080 void HMC58X3::init(bool setmode)
00081 {
00082     // note that we don't initialize Wire here.
00083     // You'll have to do that in setup() in your Arduino program
00084     Thread::wait(10); // you need to wait at least 5ms after power on to initialize
00085 
00086     if (setmode) {
00087         setMode(0);
00088     }
00089 
00090     writeReg(HMC58X3_R_CONFA, 0x18); // 4 samples averaged, 75Hz frequency, no artificial bias.
00091     
00092     writeReg(HMC58X3_R_CONFB, 0xA0);
00093     
00094     writeReg(HMC58X3_R_MODE, 0x00);
00095 }
00096 
00097 
00098 void HMC58X3::setMode(unsigned char mode)
00099 {
00100     if (mode > 2) {
00101         return;
00102     }
00103 
00104     writeReg(HMC58X3_R_MODE, mode);
00105     Thread::wait(100);
00106 }
00107 
00108 /*
00109     Calibrate which has a few weaknesses.
00110     1. Uses wrong gain for first reading.
00111     2. Uses max instead of max of average when normalizing the axis to one another.
00112     3. Doesn't use neg bias. (possible improvement in measurement).
00113 */
00114 void HMC58X3::calibrate(unsigned char gain)
00115 {
00116     x_scale=1; // get actual values
00117     y_scale=1;
00118     z_scale=1;
00119     writeReg(HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to pos bias
00120     setGain(gain);
00121     float x, y, z, mx=0, my=0, mz=0, t=10;
00122 
00123     for (int i=0; i<(int)t; i++) {
00124         setMode(1);
00125         getValues(&x,&y,&z);
00126         if (x>mx) mx=x;
00127         if (y>my) my=y;
00128         if (z>mz) mz=z;
00129     }
00130 
00131     float max=0;
00132     if (mx>max) max=mx;
00133     if (my>max) max=my;
00134     if (mz>max) max=mz;
00135     x_max=mx;
00136     y_max=my;
00137     z_max=mz;
00138     x_scale=max/mx; // calc scales
00139     y_scale=max/my;
00140     z_scale=max/mz;
00141 
00142     writeReg(HMC58X3_R_CONFA, 0x010); // set RegA/DOR back to default
00143 }   // calibrate().
00144 
00145 /*!
00146     \brief Calibrate using the self test operation.
00147 
00148     Average the values using bias mode to obtain the scale factors.
00149 
00150     \param gain [in] Gain setting for the sensor. See data sheet.
00151     \param n_samples [in] Number of samples to average together while applying the positive and negative bias.
00152     \return Returns false if any of the following occurs:
00153         # Invalid input parameters. (gain>7 or n_samples=0).
00154         # Id registers are wrong for the compiled device. Unfortunately, we can't distinguish between HMC5843 and HMC5883L.
00155         # Calibration saturates during the positive or negative bias on any of the readings.
00156         # Readings are outside of the expected range for bias current.
00157 */
00158 bool HMC58X3::calibrate(unsigned char gain,unsigned int n_samples)
00159 {
00160     int16_t xyz[3];                     // 16 bit integer values for each axis.
00161     long xyz_total[3]= {0,0,0}; // 32 bit totals so they won't overflow.
00162     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.
00163     char id[3];                     // Three identification registers should return 'H43'.
00164     long low_limit, high_limit;
00165     /*
00166         Make sure we are talking to the correct device.
00167         Hard to believe Honeywell didn't change the identifier.
00168     */
00169     if ((8>gain) && (0<n_samples)) { // Notice this allows gain setting of 7 which the data sheet warns against.
00170         getID(id);
00171         if (('H' == id[0]) && ('4' == id[1]) && ('3' == id[2])) {
00172             /*
00173                 Use the positive bias current to impose a known field on each axis.
00174                 This field depends on the device and the axis.
00175             */
00176             writeReg(HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to pos bias
00177             /*
00178                 Note that the  very first measurement after a gain change maintains the same gain as the previous setting.
00179                 The new gain setting is effective from the second measurement and on.
00180             */
00181             setGain(gain);
00182             setMode(1);                         // Change to single measurement mode.
00183             getRaw(&xyz[0],&xyz[1],&xyz[2]);    // Get the raw values and ignore since this reading may use previous gain.
00184 
00185             for (unsigned int i=0; i<n_samples; i++) {
00186                 setMode(1);
00187                 getRaw(&xyz[0],&xyz[1],&xyz[2]);   // Get the raw values in case the scales have already been changed.
00188                 /*
00189                     Since the measurements are noisy, they should be averaged rather than taking the max.
00190                 */
00191                 xyz_total[0]+=xyz[0];
00192                 xyz_total[1]+=xyz[1];
00193                 xyz_total[2]+=xyz[2];
00194                 /*
00195                     Detect saturation.
00196                 */
00197                 if (-(1<<12) >= min(xyz[0],min(xyz[1],xyz[2]))) {
00198                     DEBUG_PRINT("HMC58x3 Self test saturated. Increase range.");
00199                     bret=false;
00200                     break;  // Breaks out of the for loop.  No sense in continuing if we saturated.
00201                 }
00202             }
00203             /*
00204                 Apply the negative bias. (Same gain)
00205             */
00206             writeReg(HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR=0x010 + MS1,MS0 set to negative bias.
00207             for (unsigned int i=0; i<n_samples; i++) {
00208                 setMode(1);
00209                 getRaw(&xyz[0],&xyz[1],&xyz[2]);   // Get the raw values in case the scales have already been changed.
00210                 /*
00211                     Since the measurements are noisy, they should be averaged.
00212                 */
00213                 xyz_total[0]-=xyz[0];
00214                 xyz_total[1]-=xyz[1];
00215                 xyz_total[2]-=xyz[2];
00216                 /*
00217                     Detect saturation.
00218                 */
00219                 if (-(1<<12) >= min(xyz[0],min(xyz[1],xyz[2]))) {
00220                     DEBUG_PRINT("HMC58x3 Self test saturated. Increase range.");
00221                     bret=false;
00222                     break;  // Breaks out of the for loop.  No sense in continuing if we saturated.
00223                 }
00224             }
00225             /*
00226                 Compare the values against the expected self test bias gauss.
00227                 Notice, the same limits are applied to all axis.
00228             */
00229             low_limit =SELF_TEST_LOW_LIMIT *counts_per_milligauss[gain]*2*n_samples;
00230             high_limit=SELF_TEST_HIGH_LIMIT*counts_per_milligauss[gain]*2*n_samples;
00231 
00232             if ((true==bret) &&
00233                     (low_limit <= xyz_total[0]) && (high_limit >= xyz_total[0]) &&
00234                     (low_limit <= xyz_total[1]) && (high_limit >= xyz_total[1]) &&
00235                     (low_limit <= xyz_total[2]) && (high_limit >= xyz_total[2]) ) {
00236                 /*
00237                     Successful calibration.
00238                     Normalize the scale factors so all axis return the same range of values for the bias field.
00239                     Factor of 2 is from summation of total of n_samples from both positive and negative bias.
00240                 */
00241                 x_scale=(counts_per_milligauss[gain]*(HMC58X3_X_SELF_TEST_GAUSS*2))/(xyz_total[0]/n_samples);
00242                 y_scale=(counts_per_milligauss[gain]*(HMC58X3_Y_SELF_TEST_GAUSS*2))/(xyz_total[1]/n_samples);
00243                 z_scale=(counts_per_milligauss[gain]*(HMC58X3_Z_SELF_TEST_GAUSS*2))/(xyz_total[2]/n_samples);
00244             } else {
00245                 DEBUG_PRINT("HMC58x3 Self test out of range.");
00246                 bret=false;
00247             }
00248             writeReg(HMC58X3_R_CONFA, 0x010); // set RegA/DOR back to default.
00249         } else {
00250 #if defined(ISHMC5843)
00251             DEBUG_PRINT("HMC5843 failed id check.");
00252 #else
00253             DEBUG_PRINT("HMC5883L failed id check.");
00254 #endif
00255             bret=false;
00256         }
00257     } else {
00258         /*
00259             Bad input parameters.
00260         */
00261         DEBUG_PRINT("HMC58x3 Bad parameters.");
00262         bret=false;
00263     }
00264     return(bret);
00265 }   //  calibrate().
00266 
00267 // set data output rate
00268 // 0-6, 4 default, normal operation assumed
00269 void HMC58X3::setDOR(unsigned char DOR)
00270 {
00271     if (DOR>6) return;
00272     writeReg(HMC58X3_R_CONFA,DOR<<2);
00273 }
00274 
00275 
00276 void HMC58X3::setGain(unsigned char gain)
00277 {
00278     // 0-7, 1 default
00279     if (gain > 7) return;
00280     writeReg(HMC58X3_R_CONFB, gain << 5);
00281 }
00282 
00283 MemoryPool<short, 16> mpool;
00284 
00285 uint32_t writeregfin(uint32_t in)
00286 {
00287     short *tmp = (short *)in;
00288     mpool.free(tmp);
00289     return 0;
00290 }
00291 
00292 void HMC58X3::writeReg(unsigned char reg, unsigned char val)
00293 {
00294     unsigned char *tmp = (unsigned char *)mpool.alloc();
00295     tmp[0]=reg;
00296     tmp[1]=val;
00297     i2c.write(I2C_ADDRESS, (char*)tmp, 2, &writeregfin, (void*)tmp);
00298 }
00299 
00300 
00301 void HMC58X3::getValues(int16_t *x,int16_t *y,int16_t *z)
00302 {
00303     float fx,fy,fz;
00304     getValues(&fx,&fy,&fz);
00305     *x= (int16_t) (fx + 0.5);
00306     *y= (int16_t) (fy + 0.5);
00307     *z= (int16_t) (fz + 0.5);
00308 }
00309 
00310 
00311 void HMC58X3::getValues(float *x,float *y,float *z)
00312 {
00313     int16_t xr,yr,zr;
00314 
00315     getRaw(&xr, &yr, &zr);
00316     *x= ((float) xr) / x_scale;
00317     *y = ((float) yr) / y_scale;
00318     *z = ((float) zr) / z_scale;
00319 }
00320 
00321 uint32_t magn_readfin(uint32_t param){
00322     HMC58X3* ins = (HMC58X3*)param;
00323     ins->sem.release();
00324     return 0;
00325 }
00326 
00327 void HMC58X3::start_sampling(){
00328     _thread.signal_set(0x1);
00329 }
00330 
00331 bool magn_valid = false;
00332 
00333 void HMC58X3::samplingthread()
00334 {
00335     char tmp[2];
00336     tmp[0]=HMC58X3_R_MODE;
00337     tmp[1]=1;
00338     char magn_data[6];
00339     Timer magntimer;
00340     
00341     char cmd[2];
00342     cmd[0] = 0x03;
00343 
00344     Thread::signal_wait(0x1);
00345     magntimer.start();
00346     
00347     for (;;) {
00348         i2c.write(0x3D, (char*)tmp, 2);
00349         
00350         i2c.write(0x3D, cmd, 1, true); // set the pointer to the start of x
00351         
00352         magntimer.reset();
00353         
00354         i2c.read_nb(0x3D, (char*)magn_data, 6, &magn_readfin, this, false);
00355         
00356         sem.wait();
00357         
00358         // read out the 3 values, 2 bytes each.
00359         cache_x = int16_t(((unsigned char)magn_data[0] << 8) | (unsigned char)magn_data[1]);
00360 #ifdef ISHMC5843
00361         cache_y = int16_t(((unsigned char)magn_data[1*2] << 8) | (unsigned char)magn_data[1*2+1]);
00362         cache_z = int16_t(((unsigned char)magn_data[2*2] << 8) | (unsigned char)magn_data[2*2+1]);
00363 #else // the Z registers comes before the Y registers in the HMC5883L
00364         cache_z = int16_t(((unsigned char)magn_data[1*2] << 8) | (unsigned char)magn_data[1*2+1]);
00365         cache_y = int16_t(((unsigned char)magn_data[2*2] << 8) | (unsigned char)magn_data[2*2+1]);
00366 #endif
00367         // the HMC58X3 will automatically wrap around on the next request.
00368 
00369         magn_valid = true;
00370 
00371         int time = ((6800-magntimer.read_us())/1000);
00372         if (time >= 0)
00373             Thread::wait(time);
00374     }
00375 
00376 }
00377 
00378 void HMC58X3::getRaw(int16_t *x,int16_t *y,int16_t *z)
00379 {
00380     *x = cache_x;
00381     *y = cache_y;
00382     *z = cache_z;
00383 }
00384 
00385 
00386 void HMC58X3::getValues(float *xyz)
00387 {
00388     getValues(&xyz[0], &xyz[1], &xyz[2]);
00389 }
00390 
00391 /*!
00392     \brief Retrieve the value of the three ID registers.
00393 
00394     Note:  Both the HMC5843 and HMC5883L have the same 'H43' identification register values. (Looks like someone at Honeywell screwed up.)
00395 
00396     \param id [out] Returns the three id register values.
00397 */
00398 void HMC58X3::getID(char id[3])
00399 {
00400     i2c.write(I2C_ADDRESS, (char*)&HMC58X3_R_IDA, 1, true);
00401     i2c.read(I2C_ADDRESS, id, 3);
00402 
00403 }   // getID().
00404 
00405 int HMC58X3::min (int a, int b)
00406 {
00407     return !(b<a)?a:b;     // or: return !comp(b,a)?a:b; for version (2)
00408 }