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