![](/media/cache/profiles/4b602c3f2f6b86b43ad406fd438e09a8.jpg.50x50_q85.jpg)
RaheeNew
Fork of Adafruit9-DOf by
Diff: Source/Adafruit_L3GD20_U.cpp
- Revision:
- 0:772bf4786416
--- /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 +}