Adafruit_L3GD20_U.cpp

00001 /***************************************************
00002   This is a library for the L3GD20 GYROSCOPE
00003 
00004   Designed specifically to work with the Adafruit L3GD20 Breakout 
00005   ----> https://www.adafruit.com/products/1032
00006 
00007   These sensors use I2C or SPI to communicate, 2 pins (I2C) 
00008   or 4 pins (SPI) are required to interface.
00009 
00010   Adafruit invests time and resources providing this open source code,
00011   please support Adafruit and open-source hardware by purchasing
00012   products from Adafruit!
00013 
00014   Written by Kevin "KTOWN" Townsend for Adafruit Industries.
00015   BSD license, all text above must be included in any redistribution
00016  ****************************************************/
00017 
00018 #include "Adafruit_L3GD20_U.h"
00019 
00020 /***************************************************************************
00021  PRIVATE FUNCTIONS
00022  ***************************************************************************/
00023 
00024 /**************************************************************************/
00025 /*!
00026     @brief  Abstract away platform differences in Arduino wire library
00027 */
00028 /**************************************************************************/
00029 void Adafruit_L3GD20_Unified::write8(byte reg, byte value)
00030 {
00031   byte data[2] = {reg,value};
00032   i2c->write(L3GD20_ADDRESS, data, 2);
00033 }
00034 
00035 /**************************************************************************/
00036 /*!
00037     @brief  Abstract away platform differences in Arduino wire library
00038 */
00039 /**************************************************************************/
00040 byte Adafruit_L3GD20_Unified::read8(byte reg)
00041 {
00042   byte value = reg;
00043   i2c->write(L3GD20_ADDRESS, &value, 1);
00044   i2c->read (L3GD20_ADDRESS, &value, 1);
00045   
00046   return value;
00047 }
00048 
00049 /***************************************************************************
00050  CONSTRUCTOR
00051  ***************************************************************************/
00052  
00053 /**************************************************************************/
00054 /*!
00055     @brief  Instantiates a new Adafruit_L3GD20_Unified class
00056 */
00057 /**************************************************************************/
00058 Adafruit_L3GD20_Unified::Adafruit_L3GD20_Unified(int32_t sensorID) {
00059   _sensorID = sensorID;
00060   _autoRangeEnabled = false;
00061 }
00062 
00063 /***************************************************************************
00064  PUBLIC FUNCTIONS
00065  ***************************************************************************/
00066  
00067 /**************************************************************************/
00068 /*!
00069     @brief  Setups the HW
00070 */
00071 /**************************************************************************/
00072 bool Adafruit_L3GD20_Unified::begin(gyroRange_t rng)
00073 {
00074 
00075 
00076   /* Set the range the an appropriate value */  
00077   _range = rng;
00078 
00079   /* Make sure we have the correct chip ID since this checks
00080      for correct address and that the IC is properly connected */
00081   uint8_t id = read8(GYRO_REGISTER_WHO_AM_I);
00082   //Serial.println(id, HEX);
00083   if ((id != L3GD20_ID) && (id != L3GD20H_ID))
00084   {
00085     return false;
00086   }
00087 
00088   /* Set CTRL_REG1 (0x20)
00089    ====================================================================
00090    BIT  Symbol    Description                                   Default
00091    ---  ------    --------------------------------------------- -------
00092    7-6  DR1/0     Output data rate                                   00
00093    5-4  BW1/0     Bandwidth selection                                00
00094      3  PD        0 = Power-down mode, 1 = normal/sleep mode          0
00095      2  ZEN       Z-axis enable (0 = disabled, 1 = enabled)           1
00096      1  YEN       Y-axis enable (0 = disabled, 1 = enabled)           1
00097      0  XEN       X-axis enable (0 = disabled, 1 = enabled)           1 */
00098 
00099   /* Reset then switch to normal mode and enable all three channels */
00100   write8(GYRO_REGISTER_CTRL_REG1, 0x00);
00101   write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
00102   /* ------------------------------------------------------------------ */
00103 
00104   /* Set CTRL_REG2 (0x21)
00105    ====================================================================
00106    BIT  Symbol    Description                                   Default
00107    ---  ------    --------------------------------------------- -------
00108    5-4  HPM1/0    High-pass filter mode selection                    00
00109    3-0  HPCF3..0  High-pass filter cutoff frequency selection      0000 */
00110 
00111   /* Nothing to do ... keep default values */
00112   /* ------------------------------------------------------------------ */
00113 
00114   /* Set CTRL_REG3 (0x22)
00115    ====================================================================
00116    BIT  Symbol    Description                                   Default
00117    ---  ------    --------------------------------------------- -------
00118      7  I1_Int1   Interrupt enable on INT1 (0=disable,1=enable)       0
00119      6  I1_Boot   Boot status on INT1 (0=disable,1=enable)            0
00120      5  H-Lactive Interrupt active config on INT1 (0=high,1=low)      0
00121      4  PP_OD     Push-Pull/Open-Drain (0=PP, 1=OD)                   0
00122      3  I2_DRDY   Data ready on DRDY/INT2 (0=disable,1=enable)        0
00123      2  I2_WTM    FIFO wtrmrk int on DRDY/INT2 (0=dsbl,1=enbl)        0
00124      1  I2_ORun   FIFO overrun int on DRDY/INT2 (0=dsbl,1=enbl)       0
00125      0  I2_Empty  FIFI empty int on DRDY/INT2 (0=dsbl,1=enbl)         0 */
00126 
00127   /* Nothing to do ... keep default values */
00128   /* ------------------------------------------------------------------ */
00129 
00130   /* Set CTRL_REG4 (0x23)
00131    ====================================================================
00132    BIT  Symbol    Description                                   Default
00133    ---  ------    --------------------------------------------- -------
00134      7  BDU       Block Data Update (0=continuous, 1=LSB/MSB)         0
00135      6  BLE       Big/Little-Endian (0=Data LSB, 1=Data MSB)          0
00136    5-4  FS1/0     Full scale selection                               00
00137                                   00 = 250 dps
00138                                   01 = 500 dps
00139                                   10 = 2000 dps
00140                                   11 = 2000 dps
00141      0  SIM       SPI Mode (0=4-wire, 1=3-wire)                       0 */
00142 
00143   /* Adjust resolution if requested */
00144   switch(_range)
00145   {
00146     case GYRO_RANGE_250DPS:
00147       write8(GYRO_REGISTER_CTRL_REG4, 0x00);
00148       break;
00149     case GYRO_RANGE_500DPS:
00150       write8(GYRO_REGISTER_CTRL_REG4, 0x10);
00151       break;
00152     case GYRO_RANGE_2000DPS:
00153       write8(GYRO_REGISTER_CTRL_REG4, 0x20);
00154       break;
00155   }
00156   /* ------------------------------------------------------------------ */
00157 
00158   /* Set CTRL_REG5 (0x24)
00159    ====================================================================
00160    BIT  Symbol    Description                                   Default
00161    ---  ------    --------------------------------------------- -------
00162      7  BOOT      Reboot memory content (0=normal, 1=reboot)          0
00163      6  FIFO_EN   FIFO enable (0=FIFO disable, 1=enable)              0
00164      4  HPen      High-pass filter enable (0=disable,1=enable)        0
00165    3-2  INT1_SEL  INT1 Selection config                              00
00166    1-0  OUT_SEL   Out selection config                               00 */
00167 
00168   /* Nothing to do ... keep default values */
00169   /* ------------------------------------------------------------------ */
00170 
00171   return true;
00172 }
00173 
00174 /**************************************************************************/
00175 /*! 
00176     @brief  Enables or disables auto-ranging
00177 */
00178 /**************************************************************************/
00179 void Adafruit_L3GD20_Unified::enableAutoRange(bool enabled)
00180 {
00181   _autoRangeEnabled = enabled;
00182 }
00183 
00184 /**************************************************************************/
00185 /*! 
00186     @brief  Gets the most recent sensor event
00187 */
00188 /**************************************************************************/
00189 void Adafruit_L3GD20_Unified::getEvent(sensors_event_t* event)
00190 {
00191   bool readingValid = false;
00192   
00193   /* Clear the event */
00194   memset(event, 0, sizeof(sensors_event_t));
00195 
00196   event->version   = sizeof(sensors_event_t);
00197   event->sensor_id = _sensorID;
00198   event->type      = SENSOR_TYPE_GYROSCOPE;
00199   
00200   while(!readingValid)
00201   {
00202     event->timestamp = millis();
00203  
00204     
00205     i2c->writeByte(L3GD20_ADDRESS, GYRO_REGISTER_OUT_X_L | 0x80);
00206     
00207     byte data[6];
00208     
00209     i2c->read(L3GD20_ADDRESS, data, 6);
00210  
00211     /* Shift values to create properly formed integer (low byte first) */
00212     event->gyro.x = (int16_t)(data[0] | (data[1] << 8));
00213     event->gyro.y = (int16_t)(data[2] | (data[3] << 8));
00214     event->gyro.z = (int16_t)(data[4] | (data[5] << 8));
00215     
00216     /* Make sure the sensor isn't saturating if auto-ranging is enabled */
00217     if (!_autoRangeEnabled)
00218     {
00219       readingValid = true;
00220     }
00221     else
00222     {
00223       /* Check if the sensor is saturating or not */
00224       if ( (event->gyro.x >= 32760) | (event->gyro.x <= -32760) | 
00225            (event->gyro.y >= 32760) | (event->gyro.y <= -32760) | 
00226            (event->gyro.z >= 32760) | (event->gyro.z <= -32760) )
00227       {
00228         /* Saturating .... increase the range if we can */
00229         switch(_range)
00230         {
00231           case GYRO_RANGE_500DPS:
00232             /* Push the range up to 2000dps */
00233             _range = GYRO_RANGE_2000DPS;
00234             write8(GYRO_REGISTER_CTRL_REG1, 0x00);
00235             write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
00236             write8(GYRO_REGISTER_CTRL_REG4, 0x20);
00237             write8(GYRO_REGISTER_CTRL_REG5, 0x80);
00238             readingValid = false;
00239             // Serial.println("Changing range to 2000DPS");
00240             break;
00241           case GYRO_RANGE_250DPS:
00242             /* Push the range up to 500dps */
00243             _range = GYRO_RANGE_500DPS;
00244             write8(GYRO_REGISTER_CTRL_REG1, 0x00);
00245             write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
00246             write8(GYRO_REGISTER_CTRL_REG4, 0x10);
00247             write8(GYRO_REGISTER_CTRL_REG5, 0x80);
00248             readingValid = false;
00249             // Serial.println("Changing range to 500DPS");
00250             break;
00251           default:
00252             readingValid = true;
00253             break;  
00254         }
00255       }
00256       else
00257       {
00258         /* All values are withing range */
00259         readingValid = true;
00260       }
00261     }
00262   }
00263   
00264   /* Compensate values depending on the resolution */
00265   switch(_range)
00266   {
00267     case GYRO_RANGE_250DPS:
00268       event->gyro.x *= GYRO_SENSITIVITY_250DPS;
00269       event->gyro.y *= GYRO_SENSITIVITY_250DPS;
00270       event->gyro.z *= GYRO_SENSITIVITY_250DPS;
00271       break;
00272     case GYRO_RANGE_500DPS:
00273       event->gyro.x *= GYRO_SENSITIVITY_500DPS;
00274       event->gyro.y *= GYRO_SENSITIVITY_500DPS;
00275       event->gyro.z *= GYRO_SENSITIVITY_500DPS;
00276       break;
00277     case GYRO_RANGE_2000DPS:
00278       event->gyro.x *= GYRO_SENSITIVITY_2000DPS;
00279       event->gyro.y *= GYRO_SENSITIVITY_2000DPS;
00280       event->gyro.z *= GYRO_SENSITIVITY_2000DPS;
00281       break;
00282   }
00283   
00284   /* Convert values to rad/s */
00285   event->gyro.x *= SENSORS_DPS_TO_RADS;
00286   event->gyro.y *= SENSORS_DPS_TO_RADS;
00287   event->gyro.z *= SENSORS_DPS_TO_RADS;
00288 }
00289 
00290 /**************************************************************************/
00291 /*! 
00292     @brief  Gets the sensor_t data
00293 */
00294 /**************************************************************************/
00295 void  Adafruit_L3GD20_Unified::getSensor(sensor_t* sensor)
00296 {  
00297   /* Clear the sensor_t object */
00298   memset(sensor, 0, sizeof(sensor_t));
00299 
00300   /* Insert the sensor name in the fixed length char array */
00301   strncpy (sensor->name, "L3GD20", sizeof(sensor->name) - 1);
00302   sensor->name[sizeof(sensor->name)- 1] = 0;
00303   sensor->version     = 1;
00304   sensor->sensor_id   = _sensorID;
00305   sensor->type        = SENSOR_TYPE_GYROSCOPE;
00306   sensor->min_delay   = 0;
00307   sensor->max_value   = (float)this->_range * SENSORS_DPS_TO_RADS;
00308   sensor->min_value   = (this->_range * -1.0) * SENSORS_DPS_TO_RADS;
00309   sensor->resolution  = 0.0F; // TBD
00310 }