LIS2MDL.cpp

00001 /* 
00002   Created by Naresh Krishnamoorthy
00003 
00004   The LIS2MDL is a low power magnetometer, here used as 3 DoF solution.
00005   Library may be used freely and without limit with attribution.
00006 
00007 */
00008 
00009 #include "LIS2MDL.h"
00010 
00011 LIS2MDL::LIS2MDL (I2C& p_i2c, uint8_t addr)
00012     : _i2c(p_i2c)
00013 {
00014     _i2c.frequency(400000);
00015 }
00016 
00017 void LIS2MDL::reset()
00018 {
00019     // reset device
00020     uint8_t temp = readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A);
00021     writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x20); // Set bit 5 to 1 to reset LIS2MDL
00022     wait(0.1);
00023     writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, temp | 0x40); // Set bit 6 to 1 to boot LIS2MDL
00024     wait(0.1); // Wait for all registers to reset
00025 }
00026 
00027 void LIS2MDL::offsetBias(float * dest1, float * dest2)
00028 {
00029     int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};
00030     int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};
00031     float _mRes = 0.0015f;
00032 
00033     wait(4);
00034 
00035     for (int ii = 0; ii < 4000; ii++) {
00036         readData(mag_temp);
00037         for (int jj = 0; jj < 3; jj++) {
00038             if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
00039             if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
00040         }
00041         wait(0.12);
00042     }
00043 
00044     _mRes = 0.0015f; // fixed sensitivity
00045     // Get hard iron correction
00046     mag_bias[0]  = (mag_max[0] + mag_min[0])/2;  // get average x mag bias in counts
00047     mag_bias[1]  = (mag_max[1] + mag_min[1])/2;  // get average y mag bias in counts
00048     mag_bias[2]  = (mag_max[2] + mag_min[2])/2;  // get average z mag bias in counts
00049 
00050     dest1[0] = (float) mag_bias[0] * _mRes;  // save mag biases in G for main program
00051     dest1[1] = (float) mag_bias[1] * _mRes;
00052     dest1[2] = (float) mag_bias[2] * _mRes;
00053 
00054     // Get soft iron correction estimate
00055     mag_scale[0]  = (mag_max[0] - mag_min[0])/2;  // get average x axis max chord length in counts
00056     mag_scale[1]  = (mag_max[1] - mag_min[1])/2;  // get average y axis max chord length in counts
00057     mag_scale[2]  = (mag_max[2] - mag_min[2])/2;  // get average z axis max chord length in counts
00058 
00059     float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
00060     avg_rad /= 3.0f;
00061 
00062     dest2[0] = avg_rad/((float)mag_scale[0]);
00063     dest2[1] = avg_rad/((float)mag_scale[1]);
00064     dest2[2] = avg_rad/((float)mag_scale[2]);
00065 
00066 }
00067 
00068 
00069 void LIS2MDL::init(uint8_t MODR)
00070 {
00071 
00072 // enable temperature compensation (bit 7 == 1), continuous mode (bits 0:1 == 00)
00073     writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_A, 0x80 | MODR<<2);
00074 
00075 // enable low pass filter (bit 0 == 1), set to ODR/4
00076     writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_B, 0x01);
00077 
00078 // enable data ready on interrupt pin (bit 0 == 1), enable block data read (bit 4 == 1)
00079     writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, 0x01 | 0x10);
00080 
00081 }
00082 
00083 uint8_t LIS2MDL::getChipID()
00084 {
00085     uint8_t c = readByte(LIS2MDL_ADDRESS, LIS2MDL_WHO_AM_I);
00086     return c;
00087 }
00088 
00089 uint8_t LIS2MDL::status()
00090 {
00091     // Read the status register of the altimeter
00092     uint8_t temp = readByte(LIS2MDL_ADDRESS, LIS2MDL_STATUS_REG);
00093     return temp;
00094 }
00095 
00096 void LIS2MDL::readData(int16_t * destination)
00097 {
00098     char rawData[6];  // x/y/z mag register data stored here
00099     readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_OUTX_L_REG), 8, &rawData[0]);  // Read the 6 raw data registers into data array
00100 
00101     destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ;     // Turn the MSB and LSB into a signed 16-bit value
00102     destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ;
00103     destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
00104 
00105 }
00106 
00107 int16_t LIS2MDL::readTemperature()
00108 {
00109     char rawData[2];  // x/y/z mag register data stored here
00110     readBytes(LIS2MDL_ADDRESS, (0x80 | LIS2MDL_TEMP_OUT_L_REG), 2, &rawData[0]);  // Read the 8 raw data registers into data array
00111 
00112     int16_t temp = ((int16_t)rawData[1] << 8) | rawData[0] ;       // Turn the MSB and LSB into a signed 16-bit value
00113     return temp;
00114 }
00115 
00116 void LIS2MDL::lis2mdlSelfCheck()
00117 {
00118   int16_t temp[3] = {0, 0, 0};
00119   float magTest[3] = {0., 0., 0.};
00120   float magNom[3] = {0., 0., 0.};
00121   int32_t sum[3] = {0, 0, 0};
00122   float _mRes = 0.0015f;
00123     
00124   // first, get average response with self test disabled
00125   for (int ii = 0; ii < 50; ii++)
00126   {
00127     readData(temp);
00128     sum[0] += temp[0];
00129     sum[1] += temp[1];
00130     sum[2] += temp[2];
00131     wait(0.1);
00132   }
00133   
00134   magNom[0] = (float) sum[0] / 50.0f;
00135   magNom[1] = (float) sum[1] / 50.0f;
00136   magNom[2] = (float) sum[2] / 50.0f;
00137   
00138   uint8_t c = readByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C);
00139   writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c | 0x02); // enable self test
00140   wait(0.1); // let mag respond
00141   
00142   sum[0] = 0;
00143   sum[1] = 0;
00144   sum[2] = 0;
00145   for (int ii = 0; ii < 50; ii++)
00146   {
00147     readData(temp);
00148     sum[0] += temp[0];
00149     sum[1] += temp[1];
00150     sum[2] += temp[2];
00151     wait(0.1);
00152   }
00153   
00154   magTest[0] = (float) sum[0] / 50.0f;
00155   magTest[1] = (float) sum[1] / 50.0f;
00156   magTest[2] = (float) sum[2] / 50.0f;
00157   
00158   writeByte(LIS2MDL_ADDRESS, LIS2MDL_CFG_REG_C, c); // return to previous settings/normal mode
00159   wait(0.1); // let mag respond
00160 
00161   //replace with serial print statements for seeing the readings.
00162   /*SEGGER_RTT.printf(0, "Mag Self Test: \n");
00163   SEGGER_RTT.printf(0, "Mx results:"); 
00164   SEGGER_RTT.printf(0, " %f ", (magTest[0] - magNom[0]) * _mRes * 1000.0); 
00165   SEGGER_RTT.printf(0, " mG \n");
00166   SEGGER_RTT.printf(0, "My results:"); 
00167   SEGGER_RTT.printf(0, " %f \n", (magTest[0] - magNom[0]) * _mRes * 1000.0);
00168   SEGGER_RTT.printf(0, "Mz results:"); 
00169   SEGGER_RTT.printf(0, " %f \n", (magTest[1] - magNom[1]) * _mRes * 1000.0);
00170   SEGGER_RTT.printf(0, "Should be between 15 and 500 mG \n");*/
00171   wait(2.0);  // give some time to read the screen
00172 }
00173 
00174 
00175 
00176 
00177 //*******************************************
00178 // I2C read/write functions for the LIS2MDL
00179 //*******************************************
00180 
00181 uint8_t LIS2MDL::readByte(uint8_t address, char subAddress)
00182 {
00183     char temp[1];
00184     int ack = 0;
00185     _i2c.start();
00186     ack = _i2c.write(0x3C);
00187     ack = _i2c.write(subAddress);
00188     _i2c.start();
00189     ack = _i2c.write(0x3D);
00190     temp[0] = _i2c.read(0);
00191     _i2c.stop();
00192     return temp[0];
00193 }
00194 
00195 
00196 void LIS2MDL::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, char * dest)
00197 {
00198     int ack = 0;
00199     _i2c.start();
00200     ack = _i2c.write(0x3C);
00201     ack = _i2c.write(subAddress);
00202     ack = _i2c.read(address, &dest[0], count);
00203     _i2c.stop();
00204 }
00205 
00206 void LIS2MDL::writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
00207 {
00208     _i2c.start();
00209     _i2c.write(0x3C);
00210     _i2c.write(subAddress);
00211     _i2c.write(data);
00212     _i2c.stop();
00213 }