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 #include "Adafruit_LSM303_U.h"
00017 
00018 //extern I2C i2c(I2C_SDA, I2C_SCL);        // sda, scl
00019 
00020 /* enabling this #define will enable the debug print blocks
00021 #define LSM303_DEBUG
00022 */
00023 typedef uint8_t byte;
00024 
00025 static float _lsm303Accel_MG_LSB     = 0.001F;   // 1, 2, 4 or 12 mg per lsb
00026 static float _lsm303Mag_Gauss_LSB_XY = 1100.0F;  // Varies with gain
00027 static float _lsm303Mag_Gauss_LSB_Z  = 980.0F;   // Varies with gain
00028 
00029 /***************************************************************************
00030  ACCELEROMETER
00031  ***************************************************************************/
00032 /***************************************************************************
00033  PRIVATE FUNCTIONS
00034  ***************************************************************************/
00035 
00036 /**************************************************************************/
00037 /*!
00038     @brief  Abstract away platform differences in Arduino wire library
00039 */
00040 /**************************************************************************/
00041 void Adafruit_LSM303_Accel_Unified::write8(byte address, byte reg, byte value)
00042 {
00043     char data_write[2];
00044 
00045     data_write[0] = reg;
00046     data_write[1] = value;
00047     int status = i2c->write(address, data_write, 2, 0);
00048 
00049 }
00050 
00051 /**************************************************************************/
00052 /*!
00053     @brief  Abstract away platform differences in Arduino wire library
00054 */
00055 /**************************************************************************/
00056 byte Adafruit_LSM303_Accel_Unified::read8(byte address, byte reg)
00057 {
00058     byte value;
00059 
00060   
00061     char data_write[2];
00062     char data_read[2];
00063 
00064   
00065     // Read register
00066     data_write[0] = reg;
00067     i2c->write(address, data_write, 1, 1); // no stop
00068     i2c->read(address, data_read, 2, 0);
00069     value = data_read[0];
00070     
00071   return value;
00072 }
00073 
00074 /**************************************************************************/
00075 /*!
00076     @brief  Reads the raw data from the sensor
00077 */
00078 /**************************************************************************/
00079 void Adafruit_LSM303_Accel_Unified::read()
00080 {
00081     char data_write[2];
00082     char data_read[6];
00083     uint32_t status;
00084     uint8_t xlo;
00085     uint8_t xhi;
00086     uint8_t ylo;
00087     uint8_t yhi;
00088     uint8_t zlo;
00089     uint8_t zhi;
00090     
00091   data_write[0] = LSM303_REGISTER_ACCEL_OUT_X_L_A | 0x80 ;
00092 
00093   i2c->write(LSM303_ADDRESS_ACCEL, data_write, 1, 1);// non stop i2c
00094   
00095   // Read the accelerometer
00096   // Read 6 byte data
00097   status = i2c->read(LSM303_ADDRESS_ACCEL, data_read, 6, 0);
00098   // Read the accelerometer
00099 
00100   // Wait around until enough data is available
00101 
00102     if ( status == 0 )
00103     {
00104         xlo = data_read[0];
00105         xhi = data_read[1];
00106         ylo = data_read[2];
00107         yhi = data_read[3];
00108         zlo = data_read[4];
00109         zhi = data_read[5];
00110 
00111 
00112   // Shift values to create properly formed integer (low byte first)
00113     _accelData.x = (int16_t)(xlo | (xhi << 8)) >> 4;
00114     _accelData.y = (int16_t)(ylo | (yhi << 8)) >> 4;
00115     _accelData.z = (int16_t)(zlo | (zhi << 8)) >> 4;
00116   }
00117 
00118 }
00119 
00120 /***************************************************************************
00121  CONSTRUCTOR
00122  ***************************************************************************/
00123  
00124 /**************************************************************************/
00125 /*!
00126     @brief  Instantiates a new Adafruit_LSM303 class
00127 */
00128 /**************************************************************************/
00129 Adafruit_LSM303_Accel_Unified::Adafruit_LSM303_Accel_Unified(int32_t sensorID) {
00130   _sensorID = sensorID;
00131 }
00132 
00133 /***************************************************************************
00134  PUBLIC FUNCTIONS
00135  ***************************************************************************/
00136  
00137 /**************************************************************************/
00138 /*!
00139     @brief  Setups the HW
00140 */
00141 /**************************************************************************/
00142 bool Adafruit_LSM303_Accel_Unified::begin()
00143 {
00144  
00145   // Enable the accelerometer (100Hz)
00146   write8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A, 0x57);
00147   
00148   // LSM303DLHC has no WHOAMI register so read CTRL_REG1_A back to check
00149   // if we are connected or not
00150   uint8_t reg1_a = read8(LSM303_ADDRESS_ACCEL, LSM303_REGISTER_ACCEL_CTRL_REG1_A);
00151   if (reg1_a != 0x57)
00152   {
00153     return false;
00154   }  
00155   
00156   return true;
00157 }
00158 
00159 /**************************************************************************/
00160 /*! 
00161     @brief  Gets the most recent sensor event
00162 */
00163 /**************************************************************************/
00164 bool Adafruit_LSM303_Accel_Unified::getEvent(sensors_event_t *event) {
00165   /* Clear the event */
00166   memset(event, 0, sizeof(sensors_event_t));
00167   
00168   /* Read new data */
00169   read();
00170 
00171   event->version   = sizeof(sensors_event_t);
00172   event->sensor_id = _sensorID;
00173   event->type      = SENSOR_TYPE_ACCELEROMETER;
00174   event->timestamp = timer.read_ms();//millis();
00175   event->acceleration.x = _accelData.x * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
00176   event->acceleration.y = _accelData.y * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
00177   event->acceleration.z = _accelData.z * _lsm303Accel_MG_LSB * SENSORS_GRAVITY_STANDARD;
00178 
00179   return true;
00180 }
00181 
00182 /**************************************************************************/
00183 /*! 
00184     @brief  Gets the sensor_t data
00185 */
00186 /**************************************************************************/
00187 void Adafruit_LSM303_Accel_Unified::getSensor(sensor_t *sensor) {
00188   /* Clear the sensor_t object */
00189   memset(sensor, 0, sizeof(sensor_t));
00190 
00191   /* Insert the sensor name in the fixed length char array */
00192   strncpy (sensor->name, "LSM303", sizeof(sensor->name) - 1);
00193   sensor->name[sizeof(sensor->name)- 1] = 0;
00194   sensor->version     = 1;
00195   sensor->sensor_id   = _sensorID;
00196   sensor->type        = SENSOR_TYPE_ACCELEROMETER;
00197   sensor->min_delay   = 0;
00198   sensor->max_value   = 0.0F; // TBD
00199   sensor->min_value   = 0.0F; // TBD
00200   sensor->resolution  = 0.0F; // TBD
00201 }
00202 
00203 /***************************************************************************
00204  MAGNETOMETER
00205  ***************************************************************************/
00206 /***************************************************************************
00207  PRIVATE FUNCTIONS
00208  ***************************************************************************/
00209 
00210 /**************************************************************************/
00211 /*!
00212     @brief  Abstract away platform differences in Arduino wire library
00213 */
00214 /**************************************************************************/
00215 void Adafruit_LSM303_Mag_Unified::write8(byte address, byte reg, byte value)
00216 {
00217     char data_write[2];
00218 
00219     data_write[0] = reg;
00220     data_write[1] = value;
00221     int status = i2c->write(address, data_write, 2, 0);
00222 
00223 }
00224 
00225 /**************************************************************************/
00226 /*!
00227     @brief  Abstract away platform differences in Arduino wire library
00228 */
00229 /**************************************************************************/
00230 byte Adafruit_LSM303_Mag_Unified::read8(byte address, byte reg)
00231 {
00232   byte value;
00233 
00234     char data_write[2];
00235     char data_read[2];
00236 
00237     // Read register
00238     data_write[0] = reg;
00239     i2c->write(address, data_write, 1, 1); // no stop
00240     i2c->read(address, data_read, 2, 0);
00241     value = data_read[0];
00242     
00243   return value;
00244 }
00245 
00246 /**************************************************************************/
00247 /*!
00248     @brief  Reads the raw data from the sensor
00249 */
00250 /**************************************************************************/
00251 void Adafruit_LSM303_Mag_Unified::read()
00252 {
00253 
00254   char data_write[2];
00255   char data_read[6];
00256   uint32_t status;      
00257   uint8_t xhi;
00258   uint8_t xlo;
00259   uint8_t zhi;
00260   uint8_t zlo;
00261   uint8_t yhi;
00262   uint8_t ylo;
00263   // Read the magnetometer
00264   data_write[0] = LSM303_REGISTER_MAG_OUT_X_H_M ;
00265 
00266   i2c->write(LSM303_ADDRESS_MAG, data_write, 1, 1);// non stop i2c
00267   
00268   // Read the accelerometer
00269   // Read 6 byte data
00270   status = i2c->read(LSM303_ADDRESS_MAG, data_read, 6, 0);
00271   // Read the accelerometer
00272 
00273   // Wait around until enough data is available
00274 
00275     if ( status == 0 )
00276     {
00277         xlo = data_read[0];
00278         xhi = data_read[1];
00279         ylo = data_read[2];
00280         yhi = data_read[3];
00281         zlo = data_read[4];
00282         zhi = data_read[5];
00283 
00284 
00285   // Shift values to create properly formed integer (low byte first)
00286   _magData.x = (int16_t)(xlo | ((int16_t)xhi << 8));
00287   _magData.y = (int16_t)(ylo | ((int16_t)yhi << 8));
00288   _magData.z = (int16_t)(zlo | ((int16_t)zhi << 8));
00289   }  
00290   // ToDo: Calculate orientation
00291   // _magData.orientation = 0.0;
00292 }
00293 
00294 /***************************************************************************
00295  CONSTRUCTOR
00296  ***************************************************************************/
00297  
00298 /**************************************************************************/
00299 /*!
00300     @brief  Instantiates a new Adafruit_LSM303 class
00301 */
00302 /**************************************************************************/
00303 Adafruit_LSM303_Mag_Unified::Adafruit_LSM303_Mag_Unified(int32_t sensorID) {
00304   _sensorID = sensorID;
00305   _autoRangeEnabled = false;
00306 }
00307 
00308 /***************************************************************************
00309  PUBLIC FUNCTIONS
00310  ***************************************************************************/
00311  
00312 /**************************************************************************/
00313 /*!
00314     @brief  Setups the HW
00315 */
00316 /**************************************************************************/
00317 bool Adafruit_LSM303_Mag_Unified::begin()
00318 {
00319   
00320   // Enable the magnetometer
00321   write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_MR_REG_M, 0x00);
00322 
00323   // LSM303DLHC has no WHOAMI register so read CRA_REG_M to check
00324   // the default value (0b00010000/0x10)
00325   uint8_t reg1_a = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M);
00326   if (reg1_a != 0x10)
00327   {
00328     return false;
00329   }
00330 
00331   // Set the gain to a known level
00332   setMagGain(LSM303_MAGGAIN_1_3);
00333 
00334   return true;
00335 }
00336 
00337 /**************************************************************************/
00338 /*! 
00339     @brief  Enables or disables auto-ranging
00340 */
00341 /**************************************************************************/
00342 void Adafruit_LSM303_Mag_Unified::enableAutoRange(bool enabled)
00343 {
00344   _autoRangeEnabled = enabled;
00345 }
00346 
00347 /**************************************************************************/
00348 /*!
00349     @brief  Sets the magnetometer's gain
00350 */
00351 /**************************************************************************/
00352 void Adafruit_LSM303_Mag_Unified::setMagGain(lsm303MagGain gain)
00353 {
00354   write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRB_REG_M, (byte)gain);
00355   
00356   _magGain = gain;
00357  
00358   switch(gain)
00359   {
00360     case LSM303_MAGGAIN_1_3:
00361       _lsm303Mag_Gauss_LSB_XY = 1100;
00362       _lsm303Mag_Gauss_LSB_Z  = 980;
00363       break;
00364     case LSM303_MAGGAIN_1_9:
00365       _lsm303Mag_Gauss_LSB_XY = 855;
00366       _lsm303Mag_Gauss_LSB_Z  = 760;
00367       break;
00368     case LSM303_MAGGAIN_2_5:
00369       _lsm303Mag_Gauss_LSB_XY = 670;
00370       _lsm303Mag_Gauss_LSB_Z  = 600;
00371       break;
00372     case LSM303_MAGGAIN_4_0:
00373       _lsm303Mag_Gauss_LSB_XY = 450;
00374       _lsm303Mag_Gauss_LSB_Z  = 400;
00375       break;
00376     case LSM303_MAGGAIN_4_7:
00377       _lsm303Mag_Gauss_LSB_XY = 400;
00378       _lsm303Mag_Gauss_LSB_Z  = 355;
00379       break;
00380     case LSM303_MAGGAIN_5_6:
00381       _lsm303Mag_Gauss_LSB_XY = 330;
00382       _lsm303Mag_Gauss_LSB_Z  = 295;
00383       break;
00384     case LSM303_MAGGAIN_8_1:
00385       _lsm303Mag_Gauss_LSB_XY = 230;
00386       _lsm303Mag_Gauss_LSB_Z  = 205;
00387       break;
00388   } 
00389 }
00390 
00391 /**************************************************************************/
00392 /*!
00393     @brief  Sets the magnetometer's update rate
00394 */
00395 /**************************************************************************/
00396 void Adafruit_LSM303_Mag_Unified::setMagRate(lsm303MagRate rate)
00397 {
00398     byte reg_m = ((byte)rate & 0x07) << 2;
00399   write8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_CRA_REG_M, reg_m);
00400 }
00401 
00402 
00403 /**************************************************************************/
00404 /*! 
00405     @brief  Gets the most recent sensor event
00406 */
00407 /**************************************************************************/
00408 bool Adafruit_LSM303_Mag_Unified::getEvent(sensors_event_t *event) {
00409   bool readingValid = false;
00410   
00411   /* Clear the event */
00412   memset(event, 0, sizeof(sensors_event_t));
00413   
00414   while(!readingValid)
00415   {
00416 
00417     uint8_t reg_mg = read8(LSM303_ADDRESS_MAG, LSM303_REGISTER_MAG_SR_REG_Mg);
00418     if (!(reg_mg & 0x1)) {
00419             return false;
00420     }
00421   
00422     /* Read new data */
00423     read();
00424     
00425     /* Make sure the sensor isn't saturating if auto-ranging is enabled */
00426     if (!_autoRangeEnabled)
00427     {
00428       readingValid = true;
00429     }
00430     else
00431     {
00432 #ifdef LSM303_DEBUG
00433       Serial.print(_magData.x); Serial.print(" ");
00434       Serial.print(_magData.y); Serial.print(" ");
00435       Serial.print(_magData.z); Serial.println(" ");
00436 #endif    
00437       /* Check if the sensor is saturating or not */
00438       if ( (_magData.x >= 2040) | (_magData.x <= -2040) | 
00439            (_magData.y >= 2040) | (_magData.y <= -2040) | 
00440            (_magData.z >= 2040) | (_magData.z <= -2040) )
00441       {
00442         /* Saturating .... increase the range if we can */
00443         switch(_magGain)
00444         {
00445           case LSM303_MAGGAIN_5_6:
00446             setMagGain(LSM303_MAGGAIN_8_1);
00447             readingValid = false;
00448 #ifdef LSM303_DEBUG
00449             Serial.println("Changing range to +/- 8.1");
00450 #endif
00451             break;
00452           case LSM303_MAGGAIN_4_7:
00453             setMagGain(LSM303_MAGGAIN_5_6);
00454             readingValid = false;
00455 #ifdef LSM303_DEBUG
00456             Serial.println("Changing range to +/- 5.6");
00457 #endif
00458             break;
00459           case LSM303_MAGGAIN_4_0:
00460             setMagGain(LSM303_MAGGAIN_4_7);
00461             readingValid = false;
00462 #ifdef LSM303_DEBUG
00463             Serial.println("Changing range to +/- 4.7");
00464 #endif          
00465             break;
00466           case LSM303_MAGGAIN_2_5:
00467             setMagGain(LSM303_MAGGAIN_4_0);
00468             readingValid = false;
00469 #ifdef LSM303_DEBUG
00470             Serial.println("Changing range to +/- 4.0");
00471 #endif          
00472             break;
00473           case LSM303_MAGGAIN_1_9:
00474             setMagGain(LSM303_MAGGAIN_2_5);
00475             readingValid = false;
00476 #ifdef LSM303_DEBUG
00477             Serial.println("Changing range to +/- 2.5");
00478 #endif          
00479             break;
00480           case LSM303_MAGGAIN_1_3:
00481             setMagGain(LSM303_MAGGAIN_1_9);
00482             readingValid = false;
00483 #ifdef LSM303_DEBUG
00484             Serial.println("Changing range to +/- 1.9");
00485 #endif          
00486             break;
00487           default:
00488             readingValid = true;
00489             break;  
00490         }
00491       }
00492       else
00493       {
00494         /* All values are withing range */
00495         readingValid = true;
00496       }
00497     }
00498   }
00499   
00500   event->version   = sizeof(sensors_event_t);
00501   event->sensor_id = _sensorID;
00502   event->type      = SENSOR_TYPE_MAGNETIC_FIELD;
00503   event->timestamp = timer.read_ms();//millis();
00504   event->magnetic.x = _magData.x / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
00505   event->magnetic.y = _magData.y / _lsm303Mag_Gauss_LSB_XY * SENSORS_GAUSS_TO_MICROTESLA;
00506   event->magnetic.z = _magData.z / _lsm303Mag_Gauss_LSB_Z * SENSORS_GAUSS_TO_MICROTESLA;
00507         
00508     return true;
00509 }
00510 
00511 /**************************************************************************/
00512 /*! 
00513     @brief  Gets the sensor_t data
00514 */
00515 /**************************************************************************/
00516 void Adafruit_LSM303_Mag_Unified::getSensor(sensor_t *sensor) {
00517   /* Clear the sensor_t object */
00518   memset(sensor, 0, sizeof(sensor_t));
00519 
00520   /* Insert the sensor name in the fixed length char array */
00521   strncpy (sensor->name, "LSM303", sizeof(sensor->name) - 1);
00522   sensor->name[sizeof(sensor->name)- 1] = 0;
00523   sensor->version     = 1;
00524   sensor->sensor_id   = _sensorID;
00525   sensor->type        = SENSOR_TYPE_MAGNETIC_FIELD;
00526   sensor->min_delay   = 0;
00527   sensor->max_value   = 0.0F; // TBD
00528   sensor->min_value   = 0.0F; // TBD
00529   sensor->resolution  = 0.0F; // TBD
00530 }