Adafruit_LSM303_U.cpp

00001 /***************************************************************************
00002   This is a library for the LSM303 Accelerometer and magnentometer/compass
00003 
00004   Designed specifically to work with the Adafruit LSM303DLHC Breakout
00005 
00006   These displays use I2C to communicate, 2 pins are required to interface.
00007 
00008   Adafruit invests time and resources providing this open source code,
00009   please support Adafruit andopen-source hardware by purchasing products
00010   from Adafruit!
00011 
00012   Written by Kevin Townsend for Adafruit Industries.  
00013   BSD license, all text above must be included in any redistribution
00014  ***************************************************************************/
00015 
00016 
00017 #include "Adafruit_LSM303_U.h"
00018 #include "Serial_base.h"
00019 
00020 static float _lsm303Accel_MG_LSB     = 0.001F;   // 1, 2, 4 or 12 mg per lsb
00021 static float _lsm303Mag_Gauss_LSB_XY = 1100.0F;  // Varies with gain
00022 static float _lsm303Mag_Gauss_LSB_Z  = 980.0F;   // Varies with gain
00023 
00024 /***************************************************************************
00025  ACCELEROMETER
00026  ***************************************************************************/
00027 /***************************************************************************
00028  PRIVATE FUNCTIONS
00029  ***************************************************************************/
00030 
00031 /**************************************************************************/
00032 /*!
00033     @brief  Abstract away platform differences in Arduino wire library
00034 */
00035 /**************************************************************************/
00036 void Adafruit_LSM303_Accel_Unified::write8(byte address, byte reg, byte value)
00037 {
00038   byte data[2] = {reg, value};
00039   i2c->write(address,data,2);
00040 }
00041 
00042 /**************************************************************************/
00043 /*!
00044     @brief  Abstract away platform differences in Arduino wire library
00045 */
00046 /**************************************************************************/
00047 byte Adafruit_LSM303_Accel_Unified::read8(byte address, byte reg)
00048 {
00049   byte value;
00050 
00051   i2c->writeByte(address, reg);
00052   i2c->read(address, &value, 1);
00053  
00054   return value;
00055 }
00056 
00057 /**************************************************************************/
00058 /*!
00059     @brief  Reads the raw data from the sensor
00060 */
00061 /**************************************************************************/
00062 void Adafruit_LSM303_Accel_Unified::read()
00063 {
00064   // Read the accelerometer
00065   i2c->writeByte(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_OUT_X_L_A | 0x80);
00066   
00067   byte data[6];
00068   i2c->read(LSM303_ADDRESS_ACCEL, data, 6);
00069 
00070   // Shift values to create properly formed integer (low byte first)
00071   _accelData.x = (int16_t)(data[0] | (data[1] << 8)) >> 4;
00072   _accelData.y = (int16_t)(data[2] | (data[3] << 8)) >> 4;
00073   _accelData.z = (int16_t)(data[4] | (data[5] << 8)) >> 4;
00074 }
00075 
00076 /***************************************************************************
00077  CONSTRUCTOR
00078  ***************************************************************************/
00079  
00080 /**************************************************************************/
00081 /*!
00082     @brief  Instantiates a new Adafruit_LSM303 class
00083 */
00084 /**************************************************************************/
00085 Adafruit_LSM303_Accel_Unified::Adafruit_LSM303_Accel_Unified(int32_t sensorID) {
00086   _sensorID = sensorID;
00087 }
00088 
00089 /***************************************************************************
00090  PUBLIC FUNCTIONS
00091  ***************************************************************************/
00092  
00093 /**************************************************************************/
00094 /*!
00095     @brief  Setups the HW
00096 */
00097 /**************************************************************************/
00098 bool Adafruit_LSM303_Accel_Unified::begin()
00099 {
00100   // Enable I2C
00101   // Enable the accelerometer (100Hz)
00102   write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x57);
00103   
00104   // LSM303DLHC has no WHOAMI register so read CTRL_REG1_A back to check
00105   // if we are connected or not
00106   uint8_t reg1_a = read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A);
00107   if (reg1_a != 0x57)
00108   {
00109     return false;
00110   }  
00111   
00112   return true;
00113 }
00114 
00115 /**************************************************************************/
00116 /*! 
00117     @brief  Gets the most recent sensor event
00118 */
00119 /**************************************************************************/
00120 void Adafruit_LSM303_Accel_Unified::getEvent(sensors_event_t *event) {
00121   /* Clear the event */
00122   memset(event, 0, sizeof(sensors_event_t));
00123   
00124   /* Read new data */
00125   read();
00126 
00127   event->version   = sizeof(sensors_event_t);
00128   event->sensor_id = _sensorID;
00129   event->type      = SENSOR_TYPE_ACCELEROMETER;
00130   event->timestamp = millis();
00131   event->acceleration.x = _accelData.x * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
00132   event->acceleration.y = _accelData.y * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
00133   event->acceleration.z = _accelData.z * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
00134 }
00135 
00136 /**************************************************************************/
00137 /*! 
00138     @brief  Gets the sensor_t data
00139 */
00140 /**************************************************************************/
00141 void Adafruit_LSM303_Accel_Unified::getSensor(sensor_t *sensor) {
00142   /* Clear the sensor_t object */
00143   memset(sensor, 0, sizeof(sensor_t));
00144 
00145   /* Insert the sensor name in the fixed length char array */
00146   strncpy (sensor->name, "LSM303", sizeof(sensor->name) - 1);
00147   sensor->name[sizeof(sensor->name)- 1] = 0;
00148   sensor->version     = 1;
00149   sensor->sensor_id   = _sensorID;
00150   sensor->type        = SENSOR_TYPE_ACCELEROMETER;
00151   sensor->min_delay   = 0;
00152   sensor->max_value   = 0.0F; // TBD
00153   sensor->min_value   = 0.0F; // TBD
00154   sensor->resolution  = 0.0F; // TBD
00155 }
00156 
00157 /***************************************************************************
00158  MAGNETOMETER
00159  ***************************************************************************/
00160 /***************************************************************************
00161  PRIVATE FUNCTIONS
00162  ***************************************************************************/
00163 
00164 /**************************************************************************/
00165 /*!
00166     @brief  Abstract away platform differences in Arduino wire library
00167 */
00168 /**************************************************************************/
00169 void Adafruit_LSM303_Mag_Unified::write8(byte address, byte reg, byte value)
00170 {
00171   byte data[2] = {reg, value};
00172   i2c->write(address,data,2);
00173 }
00174 
00175 /**************************************************************************/
00176 /*!
00177     @brief  Abstract away platform differences in Arduino wire library
00178 */
00179 /**************************************************************************/
00180 byte Adafruit_LSM303_Mag_Unified::read8(byte address, byte reg)
00181 {
00182   byte value;
00183   i2c->writeByte(address, reg);
00184   i2c->read(address, &value, 1);
00185 
00186   return value;
00187 }
00188 
00189 /**************************************************************************/
00190 /*!
00191     @brief  Reads the raw data from the sensor
00192 */
00193 /**************************************************************************/
00194 void Adafruit_LSM303_Mag_Unified::read()
00195 {
00196   // Read the magnetometer
00197   i2c->writeByte(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_OUT_X_H_M);
00198   
00199   byte data[6];
00200   i2c->read(LSM303_ADDRESS_MAG, data, 6);
00201   
00202   
00203   // Shift values to create properly formed integer (low byte first)
00204   _magData.x = (int16_t)(data[0] | ((int16_t)data[1] << 8));
00205   _magData.y = (int16_t)(data[2] | ((int16_t)data[3] << 8));
00206   _magData.z = (int16_t)(data[4] | ((int16_t)data[5] << 8));
00207   
00208   // ToDo: Calculate orientation
00209   _magData.orientation = 0.0;
00210 }
00211 
00212 /***************************************************************************
00213  CONSTRUCTOR
00214  ***************************************************************************/
00215  
00216 /**************************************************************************/
00217 /*!
00218     @brief  Instantiates a new Adafruit_LSM303 class
00219 */
00220 /**************************************************************************/
00221 Adafruit_LSM303_Mag_Unified::Adafruit_LSM303_Mag_Unified(int32_t sensorID) {
00222   _sensorID = sensorID;
00223   _autoRangeEnabled = false;
00224 }
00225 
00226 /***************************************************************************
00227  PUBLIC FUNCTIONS
00228  ***************************************************************************/
00229  
00230 /**************************************************************************/
00231 /*!
00232     @brief  Setups the HW
00233 */
00234 /**************************************************************************/
00235 bool Adafruit_LSM303_Mag_Unified::begin()
00236 {
00237   // Enable I2C
00238 
00239   // Enable the magnetometer
00240   write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, 0x00);
00241 
00242   // LSM303DLHC has no WHOAMI register so read CRA_REG_M to check
00243   // the default value (0b00010000/0x10)
00244   uint8_t reg1_a = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M);
00245   if (reg1_a != 0x10)
00246   {
00247     return false;
00248   }
00249   
00250   // Set the gain to a known level
00251   setMagGain(LSM303_MAGGAIN_1_3);
00252 
00253   return true;
00254 }
00255 
00256 /**************************************************************************/
00257 /*! 
00258     @brief  Enables or disables auto-ranging
00259 */
00260 /**************************************************************************/
00261 void Adafruit_LSM303_Mag_Unified::enableAutoRange(bool enabled)
00262 {
00263   _autoRangeEnabled = enabled;
00264 }
00265 
00266 /**************************************************************************/
00267 /*!
00268     @brief  Sets the magnetometer's gain
00269 */
00270 /**************************************************************************/
00271 void Adafruit_LSM303_Mag_Unified::setMagGain(lsm303MagGain gain)
00272 {
00273   write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, (byte)gain);
00274   
00275   _magGain = gain;
00276  
00277   switch(gain)
00278   {
00279     case LSM303_MAGGAIN_1_3:
00280       _lsm303Mag_Gauss_LSB_XY = 1100;
00281       _lsm303Mag_Gauss_LSB_Z  = 980;
00282       break;
00283     case LSM303_MAGGAIN_1_9:
00284       _lsm303Mag_Gauss_LSB_XY = 855;
00285       _lsm303Mag_Gauss_LSB_Z  = 760;
00286       break;
00287     case LSM303_MAGGAIN_2_5:
00288       _lsm303Mag_Gauss_LSB_XY = 670;
00289       _lsm303Mag_Gauss_LSB_Z  = 600;
00290       break;
00291     case LSM303_MAGGAIN_4_0:
00292       _lsm303Mag_Gauss_LSB_XY = 450;
00293       _lsm303Mag_Gauss_LSB_Z  = 400;
00294       break;
00295     case LSM303_MAGGAIN_4_7:
00296       _lsm303Mag_Gauss_LSB_XY = 400;
00297       _lsm303Mag_Gauss_LSB_Z  = 355;
00298       break;
00299     case LSM303_MAGGAIN_5_6:
00300       _lsm303Mag_Gauss_LSB_XY = 330;
00301       _lsm303Mag_Gauss_LSB_Z  = 295;
00302       break;
00303     case LSM303_MAGGAIN_8_1:
00304       _lsm303Mag_Gauss_LSB_XY = 230;
00305       _lsm303Mag_Gauss_LSB_Z  = 205;
00306       break;
00307   } 
00308 }
00309 
00310 /**************************************************************************/
00311 /*! 
00312     @brief  Gets the most recent sensor event
00313 */
00314 /**************************************************************************/
00315 void Adafruit_LSM303_Mag_Unified::getEvent(sensors_event_t *event) {
00316   bool readingValid = false;
00317   
00318   /* Clear the event */
00319   memset(event, 0, sizeof(sensors_event_t));
00320   
00321   while(!readingValid)
00322   {
00323     /* Read new data */
00324     read();
00325     
00326     /* Make sure the sensor isn't saturating if auto-ranging is enabled */
00327     if (!_autoRangeEnabled)
00328     {
00329       readingValid = true;
00330     }
00331     else
00332     {
00333       s_com->print(_magData.x); s_com->print(" ");
00334       s_com->print(_magData.y); s_com->print(" ");
00335       s_com->print(_magData.z); s_com->println(" ");
00336       /* Check if the sensor is saturating or not */
00337       if ( (_magData.x >= 2040) | (_magData.x <= -2040) | 
00338            (_magData.y >= 2040) | (_magData.y <= -2040) | 
00339            (_magData.z >= 2040) | (_magData.z <= -2040) )
00340       {
00341         /* Saturating .... increase the range if we can */
00342         switch(_magGain)
00343         {
00344           case LSM303_MAGGAIN_5_6:
00345             setMagGain(LSM303_MAGGAIN_8_1);
00346             readingValid = false;
00347             s_com->println("Changing range to +/- 8.1");
00348             break;
00349           case LSM303_MAGGAIN_4_7:
00350             setMagGain(LSM303_MAGGAIN_5_6);
00351             readingValid = false;
00352             s_com->println("Changing range to +/- 5.6");
00353             break;
00354           case LSM303_MAGGAIN_4_0:
00355             setMagGain(LSM303_MAGGAIN_4_7);
00356             readingValid = false;
00357             s_com->println("Changing range to +/- 4.7");
00358             break;
00359           case LSM303_MAGGAIN_2_5:
00360             setMagGain(LSM303_MAGGAIN_4_0);
00361             readingValid = false;
00362             s_com->println("Changing range to +/- 4.0");
00363             break;
00364           case LSM303_MAGGAIN_1_9:
00365             setMagGain(LSM303_MAGGAIN_2_5);
00366             readingValid = false;
00367             s_com->println("Changing range to +/- 2.5");
00368             break;
00369           case LSM303_MAGGAIN_1_3:
00370             setMagGain(LSM303_MAGGAIN_1_9);
00371             readingValid = false;
00372             s_com->println("Changing range to +/- 1.9");
00373             break;
00374           default:
00375             readingValid = true;
00376             break;  
00377         }
00378       }
00379       else
00380       {
00381         /* All values are withing range */
00382         readingValid = true;
00383       }
00384     }
00385   }
00386   
00387   event->version   = sizeof(sensors_event_t);
00388   event->sensor_id = _sensorID;
00389   event->type      = SENSOR_TYPE_MAGNETIC_FIELD;
00390   event->timestamp = millis();
00391   event->magnetic.x = _magData.x / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
00392   event->magnetic.y = _magData.y / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
00393   event->magnetic.z = _magData.z / _lsm303Mag_Gauss_LSB_Z * SENSORS_GAUSS_TO_MICROTESLA;
00394 }
00395 
00396 /**************************************************************************/
00397 /*! 
00398     @brief  Gets the sensor_t data
00399 */
00400 /**************************************************************************/
00401 void Adafruit_LSM303_Mag_Unified::getSensor(sensor_t *sensor) {
00402   /* Clear the sensor_t object */
00403   memset(sensor, 0, sizeof(sensor_t));
00404 
00405   /* Insert the sensor name in the fixed length char array */
00406   strncpy (sensor->name, "LSM303", sizeof(sensor->name) - 1);
00407   sensor->name[sizeof(sensor->name)- 1] = 0;
00408   sensor->version     = 1;
00409   sensor->sensor_id   = _sensorID;
00410   sensor->type        = SENSOR_TYPE_MAGNETIC_FIELD;
00411   sensor->min_delay   = 0;
00412   sensor->max_value   = 0.0F; // TBD
00413   sensor->min_value   = 0.0F; // TBD
00414   sensor->resolution  = 0.0F; // TBD
00415 }