Nikhilesh Behera
Port of Adafruit Arduino code
Fork of
Adafruit9-DOf
by 
Bruno Manganelli
Diff: Source/Adafruit_L3GD20_U.cpp
- Revision:
- 0:772bf4786416
diff -r 000000000000 -r 772bf4786416 Source/Adafruit_L3GD20_U.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Source/Adafruit_L3GD20_U.cpp Sat Mar 21 12:33:05 2015 +0000
@@ -0,0 +1,310 @@
+/***************************************************
+ This is a library for the L3GD20 GYROSCOPE
+
+ Designed specifically to work with the Adafruit L3GD20 Breakout
+ ----> https://www.adafruit.com/products/1032
+
+ These sensors use I2C or SPI to communicate, 2 pins (I2C)
+ or 4 pins (SPI) are required to interface.
+
+ Adafruit invests time and resources providing this open source code,
+ please support Adafruit and open-source hardware by purchasing
+ products from Adafruit!
+
+ Written by Kevin "KTOWN" Townsend for Adafruit Industries.
+ BSD license, all text above must be included in any redistribution
+ ****************************************************/
+
+#include "Adafruit_L3GD20_U.h"
+
+/***************************************************************************
+ PRIVATE FUNCTIONS
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Abstract away platform differences in Arduino wire library
+*/
+/**************************************************************************/
+void Adafruit_L3GD20_Unified::write8(byte reg, byte value)
+{
+ byte data[2] = {reg,value};
+ i2c->write(L3GD20_ADDRESS, data, 2);
+}
+
+/**************************************************************************/
+/*!
+ @brief Abstract away platform differences in Arduino wire library
+*/
+/**************************************************************************/
+byte Adafruit_L3GD20_Unified::read8(byte reg)
+{
+ byte value = reg;
+ i2c->write(L3GD20_ADDRESS, &value, 1);
+ i2c->read (L3GD20_ADDRESS, &value, 1);
+
+ return value;
+}
+
+/***************************************************************************
+ CONSTRUCTOR
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Instantiates a new Adafruit_L3GD20_Unified class
+*/
+/**************************************************************************/
+Adafruit_L3GD20_Unified::Adafruit_L3GD20_Unified(int32_t sensorID) {
+ _sensorID = sensorID;
+ _autoRangeEnabled = false;
+}
+
+/***************************************************************************
+ PUBLIC FUNCTIONS
+ ***************************************************************************/
+
+/**************************************************************************/
+/*!
+ @brief Setups the HW
+*/
+/**************************************************************************/
+bool Adafruit_L3GD20_Unified::begin(gyroRange_t rng)
+{
+
+
+ /* Set the range the an appropriate value */
+ _range = rng;
+
+ /* Make sure we have the correct chip ID since this checks
+ for correct address and that the IC is properly connected */
+ uint8_t id = read8(GYRO_REGISTER_WHO_AM_I);
+ //Serial.println(id, HEX);
+ if ((id != L3GD20_ID) && (id != L3GD20H_ID))
+ {
+ return false;
+ }
+
+ /* Set CTRL_REG1 (0x20)
+ ====================================================================
+ BIT Symbol Description Default
+ --- ------ --------------------------------------------- -------
+ 7-6 DR1/0 Output data rate 00
+ 5-4 BW1/0 Bandwidth selection 00
+ 3 PD 0 = Power-down mode, 1 = normal/sleep mode 0
+ 2 ZEN Z-axis enable (0 = disabled, 1 = enabled) 1
+ 1 YEN Y-axis enable (0 = disabled, 1 = enabled) 1
+ 0 XEN X-axis enable (0 = disabled, 1 = enabled) 1 */
+
+ /* Reset then switch to normal mode and enable all three channels */
+ write8(GYRO_REGISTER_CTRL_REG1, 0x00);
+ write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
+ /* ------------------------------------------------------------------ */
+
+ /* Set CTRL_REG2 (0x21)
+ ====================================================================
+ BIT Symbol Description Default
+ --- ------ --------------------------------------------- -------
+ 5-4 HPM1/0 High-pass filter mode selection 00
+ 3-0 HPCF3..0 High-pass filter cutoff frequency selection 0000 */
+
+ /* Nothing to do ... keep default values */
+ /* ------------------------------------------------------------------ */
+
+ /* Set CTRL_REG3 (0x22)
+ ====================================================================
+ BIT Symbol Description Default
+ --- ------ --------------------------------------------- -------
+ 7 I1_Int1 Interrupt enable on INT1 (0=disable,1=enable) 0
+ 6 I1_Boot Boot status on INT1 (0=disable,1=enable) 0
+ 5 H-Lactive Interrupt active config on INT1 (0=high,1=low) 0
+ 4 PP_OD Push-Pull/Open-Drain (0=PP, 1=OD) 0
+ 3 I2_DRDY Data ready on DRDY/INT2 (0=disable,1=enable) 0
+ 2 I2_WTM FIFO wtrmrk int on DRDY/INT2 (0=dsbl,1=enbl) 0
+ 1 I2_ORun FIFO overrun int on DRDY/INT2 (0=dsbl,1=enbl) 0
+ 0 I2_Empty FIFI empty int on DRDY/INT2 (0=dsbl,1=enbl) 0 */
+
+ /* Nothing to do ... keep default values */
+ /* ------------------------------------------------------------------ */
+
+ /* Set CTRL_REG4 (0x23)
+ ====================================================================
+ BIT Symbol Description Default
+ --- ------ --------------------------------------------- -------
+ 7 BDU Block Data Update (0=continuous, 1=LSB/MSB) 0
+ 6 BLE Big/Little-Endian (0=Data LSB, 1=Data MSB) 0
+ 5-4 FS1/0 Full scale selection 00
+ 00 = 250 dps
+ 01 = 500 dps
+ 10 = 2000 dps
+ 11 = 2000 dps
+ 0 SIM SPI Mode (0=4-wire, 1=3-wire) 0 */
+
+ /* Adjust resolution if requested */
+ switch(_range)
+ {
+ case GYRO_RANGE_250DPS:
+ write8(GYRO_REGISTER_CTRL_REG4, 0x00);
+ break;
+ case GYRO_RANGE_500DPS:
+ write8(GYRO_REGISTER_CTRL_REG4, 0x10);
+ break;
+ case GYRO_RANGE_2000DPS:
+ write8(GYRO_REGISTER_CTRL_REG4, 0x20);
+ break;
+ }
+ /* ------------------------------------------------------------------ */
+
+ /* Set CTRL_REG5 (0x24)
+ ====================================================================
+ BIT Symbol Description Default
+ --- ------ --------------------------------------------- -------
+ 7 BOOT Reboot memory content (0=normal, 1=reboot) 0
+ 6 FIFO_EN FIFO enable (0=FIFO disable, 1=enable) 0
+ 4 HPen High-pass filter enable (0=disable,1=enable) 0
+ 3-2 INT1_SEL INT1 Selection config 00
+ 1-0 OUT_SEL Out selection config 00 */
+
+ /* Nothing to do ... keep default values */
+ /* ------------------------------------------------------------------ */
+
+ return true;
+}
+
+/**************************************************************************/
+/*!
+ @brief Enables or disables auto-ranging
+*/
+/**************************************************************************/
+void Adafruit_L3GD20_Unified::enableAutoRange(bool enabled)
+{
+ _autoRangeEnabled = enabled;
+}
+
+/**************************************************************************/
+/*!
+ @brief Gets the most recent sensor event
+*/
+/**************************************************************************/
+void Adafruit_L3GD20_Unified::getEvent(sensors_event_t* event)
+{
+ bool readingValid = false;
+
+ /* Clear the event */
+ memset(event, 0, sizeof(sensors_event_t));
+
+ event->version = sizeof(sensors_event_t);
+ event->sensor_id = _sensorID;
+ event->type = SENSOR_TYPE_GYROSCOPE;
+
+ while(!readingValid)
+ {
+ event->timestamp = millis();
+
+
+ i2c->writeByte(L3GD20_ADDRESS, GYRO_REGISTER_OUT_X_L | 0x80);
+
+ byte data[6];
+
+ i2c->read(L3GD20_ADDRESS, data, 6);
+
+ /* Shift values to create properly formed integer (low byte first) */
+ event->gyro.x = (int16_t)(data[0] | (data[1] << 8));
+ event->gyro.y = (int16_t)(data[2] | (data[3] << 8));
+ event->gyro.z = (int16_t)(data[4] | (data[5] << 8));
+
+ /* Make sure the sensor isn't saturating if auto-ranging is enabled */
+ if (!_autoRangeEnabled)
+ {
+ readingValid = true;
+ }
+ else
+ {
+ /* Check if the sensor is saturating or not */
+ if ( (event->gyro.x >= 32760) | (event->gyro.x <= -32760) |
+ (event->gyro.y >= 32760) | (event->gyro.y <= -32760) |
+ (event->gyro.z >= 32760) | (event->gyro.z <= -32760) )
+ {
+ /* Saturating .... increase the range if we can */
+ switch(_range)
+ {
+ case GYRO_RANGE_500DPS:
+ /* Push the range up to 2000dps */
+ _range = GYRO_RANGE_2000DPS;
+ write8(GYRO_REGISTER_CTRL_REG1, 0x00);
+ write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
+ write8(GYRO_REGISTER_CTRL_REG4, 0x20);
+ write8(GYRO_REGISTER_CTRL_REG5, 0x80);
+ readingValid = false;
+ // Serial.println("Changing range to 2000DPS");
+ break;
+ case GYRO_RANGE_250DPS:
+ /* Push the range up to 500dps */
+ _range = GYRO_RANGE_500DPS;
+ write8(GYRO_REGISTER_CTRL_REG1, 0x00);
+ write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
+ write8(GYRO_REGISTER_CTRL_REG4, 0x10);
+ write8(GYRO_REGISTER_CTRL_REG5, 0x80);
+ readingValid = false;
+ // Serial.println("Changing range to 500DPS");
+ break;
+ default:
+ readingValid = true;
+ break;
+ }
+ }
+ else
+ {
+ /* All values are withing range */
+ readingValid = true;
+ }
+ }
+ }
+
+ /* Compensate values depending on the resolution */
+ switch(_range)
+ {
+ case GYRO_RANGE_250DPS:
+ event->gyro.x *= GYRO_SENSITIVITY_250DPS;
+ event->gyro.y *= GYRO_SENSITIVITY_250DPS;
+ event->gyro.z *= GYRO_SENSITIVITY_250DPS;
+ break;
+ case GYRO_RANGE_500DPS:
+ event->gyro.x *= GYRO_SENSITIVITY_500DPS;
+ event->gyro.y *= GYRO_SENSITIVITY_500DPS;
+ event->gyro.z *= GYRO_SENSITIVITY_500DPS;
+ break;
+ case GYRO_RANGE_2000DPS:
+ event->gyro.x *= GYRO_SENSITIVITY_2000DPS;
+ event->gyro.y *= GYRO_SENSITIVITY_2000DPS;
+ event->gyro.z *= GYRO_SENSITIVITY_2000DPS;
+ break;
+ }
+
+ /* Convert values to rad/s */
+ event->gyro.x *= SENSORS_DPS_TO_RADS;
+ event->gyro.y *= SENSORS_DPS_TO_RADS;
+ event->gyro.z *= SENSORS_DPS_TO_RADS;
+}
+
+/**************************************************************************/
+/*!
+ @brief Gets the sensor_t data
+*/
+/**************************************************************************/
+void Adafruit_L3GD20_Unified::getSensor(sensor_t* sensor)
+{
+ /* Clear the sensor_t object */
+ memset(sensor, 0, sizeof(sensor_t));
+
+ /* Insert the sensor name in the fixed length char array */
+ strncpy (sensor->name, "L3GD20", sizeof(sensor->name) - 1);
+ sensor->name[sizeof(sensor->name)- 1] = 0;
+ sensor->version = 1;
+ sensor->sensor_id = _sensorID;
+ sensor->type = SENSOR_TYPE_GYROSCOPE;
+ sensor->min_delay = 0;
+ sensor->max_value = (float)this->_range * SENSORS_DPS_TO_RADS;
+ sensor->min_value = (this->_range * -1.0) * SENSORS_DPS_TO_RADS;
+ sensor->resolution = 0.0F; // TBD
+}
