Nikhilesh Behera / Mbed 2 deprecated Adafruit9-DOf

Port of Adafruit Arduino code

Dependencies:   mbed

Fork of Adafruit9-DOf by Bruno Manganelli

Source/Adafruit_L3GD20_U.cpp@0:772bf4786416, 2015-03-21 (annotated)

Committer:
bmanga95
Date:
Sat Mar 21 12:33:05 2015 +0000
Revision:
0:772bf4786416
First version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bmanga95 0:772bf4786416 1 /***************************************************
bmanga95 0:772bf4786416 2 This is a library for the L3GD20 GYROSCOPE
bmanga95 0:772bf4786416 3
bmanga95 0:772bf4786416 4 Designed specifically to work with the Adafruit L3GD20 Breakout
bmanga95 0:772bf4786416 5 ----> https://www.adafruit.com/products/1032
bmanga95 0:772bf4786416 6
bmanga95 0:772bf4786416 7 These sensors use I2C or SPI to communicate, 2 pins (I2C)
bmanga95 0:772bf4786416 8 or 4 pins (SPI) are required to interface.
bmanga95 0:772bf4786416 9
bmanga95 0:772bf4786416 10 Adafruit invests time and resources providing this open source code,
bmanga95 0:772bf4786416 11 please support Adafruit and open-source hardware by purchasing
bmanga95 0:772bf4786416 12 products from Adafruit!
bmanga95 0:772bf4786416 13
bmanga95 0:772bf4786416 14 Written by Kevin "KTOWN" Townsend for Adafruit Industries.
bmanga95 0:772bf4786416 15 BSD license, all text above must be included in any redistribution
bmanga95 0:772bf4786416 16 ****************************************************/
bmanga95 0:772bf4786416 17
bmanga95 0:772bf4786416 18 #include "Adafruit_L3GD20_U.h"
bmanga95 0:772bf4786416 19
bmanga95 0:772bf4786416 20 /***************************************************************************
bmanga95 0:772bf4786416 21 PRIVATE FUNCTIONS
bmanga95 0:772bf4786416 22 ***************************************************************************/
bmanga95 0:772bf4786416 23
bmanga95 0:772bf4786416 24 /**************************************************************************/
bmanga95 0:772bf4786416 25 /*!
bmanga95 0:772bf4786416 26 @brief Abstract away platform differences in Arduino wire library
bmanga95 0:772bf4786416 27 */
bmanga95 0:772bf4786416 28 /**************************************************************************/
bmanga95 0:772bf4786416 29 void Adafruit_L3GD20_Unified::write8(byte reg, byte value)
bmanga95 0:772bf4786416 30 {
bmanga95 0:772bf4786416 31 byte data[2] = {reg,value};
bmanga95 0:772bf4786416 32 i2c->write(L3GD20_ADDRESS, data, 2);
bmanga95 0:772bf4786416 33 }
bmanga95 0:772bf4786416 34
bmanga95 0:772bf4786416 35 /**************************************************************************/
bmanga95 0:772bf4786416 36 /*!
bmanga95 0:772bf4786416 37 @brief Abstract away platform differences in Arduino wire library
bmanga95 0:772bf4786416 38 */
bmanga95 0:772bf4786416 39 /**************************************************************************/
bmanga95 0:772bf4786416 40 byte Adafruit_L3GD20_Unified::read8(byte reg)
bmanga95 0:772bf4786416 41 {
bmanga95 0:772bf4786416 42 byte value = reg;
bmanga95 0:772bf4786416 43 i2c->write(L3GD20_ADDRESS, &value, 1);
bmanga95 0:772bf4786416 44 i2c->read (L3GD20_ADDRESS, &value, 1);
bmanga95 0:772bf4786416 45
bmanga95 0:772bf4786416 46 return value;
bmanga95 0:772bf4786416 47 }
bmanga95 0:772bf4786416 48
bmanga95 0:772bf4786416 49 /***************************************************************************
bmanga95 0:772bf4786416 50 CONSTRUCTOR
bmanga95 0:772bf4786416 51 ***************************************************************************/
bmanga95 0:772bf4786416 52
bmanga95 0:772bf4786416 53 /**************************************************************************/
bmanga95 0:772bf4786416 54 /*!
bmanga95 0:772bf4786416 55 @brief Instantiates a new Adafruit_L3GD20_Unified class
bmanga95 0:772bf4786416 56 */
bmanga95 0:772bf4786416 57 /**************************************************************************/
bmanga95 0:772bf4786416 58 Adafruit_L3GD20_Unified::Adafruit_L3GD20_Unified(int32_t sensorID) {
bmanga95 0:772bf4786416 59 _sensorID = sensorID;
bmanga95 0:772bf4786416 60 _autoRangeEnabled = false;
bmanga95 0:772bf4786416 61 }
bmanga95 0:772bf4786416 62
bmanga95 0:772bf4786416 63 /***************************************************************************
bmanga95 0:772bf4786416 64 PUBLIC FUNCTIONS
bmanga95 0:772bf4786416 65 ***************************************************************************/
bmanga95 0:772bf4786416 66
bmanga95 0:772bf4786416 67 /**************************************************************************/
bmanga95 0:772bf4786416 68 /*!
bmanga95 0:772bf4786416 69 @brief Setups the HW
bmanga95 0:772bf4786416 70 */
bmanga95 0:772bf4786416 71 /**************************************************************************/
bmanga95 0:772bf4786416 72 bool Adafruit_L3GD20_Unified::begin(gyroRange_t rng)
bmanga95 0:772bf4786416 73 {
bmanga95 0:772bf4786416 74
bmanga95 0:772bf4786416 75
bmanga95 0:772bf4786416 76 /* Set the range the an appropriate value */
bmanga95 0:772bf4786416 77 _range = rng;
bmanga95 0:772bf4786416 78
bmanga95 0:772bf4786416 79 /* Make sure we have the correct chip ID since this checks
bmanga95 0:772bf4786416 80 for correct address and that the IC is properly connected */
bmanga95 0:772bf4786416 81 uint8_t id = read8(GYRO_REGISTER_WHO_AM_I);
bmanga95 0:772bf4786416 82 //Serial.println(id, HEX);
bmanga95 0:772bf4786416 83 if ((id != L3GD20_ID) && (id != L3GD20H_ID))
bmanga95 0:772bf4786416 84 {
bmanga95 0:772bf4786416 85 return false;
bmanga95 0:772bf4786416 86 }
bmanga95 0:772bf4786416 87
bmanga95 0:772bf4786416 88 /* Set CTRL_REG1 (0x20)
bmanga95 0:772bf4786416 89 ====================================================================
bmanga95 0:772bf4786416 90 BIT Symbol Description Default
bmanga95 0:772bf4786416 91 --- ------ --------------------------------------------- -------
bmanga95 0:772bf4786416 92 7-6 DR1/0 Output data rate 00
bmanga95 0:772bf4786416 93 5-4 BW1/0 Bandwidth selection 00
bmanga95 0:772bf4786416 94 3 PD 0 = Power-down mode, 1 = normal/sleep mode 0
bmanga95 0:772bf4786416 95 2 ZEN Z-axis enable (0 = disabled, 1 = enabled) 1
bmanga95 0:772bf4786416 96 1 YEN Y-axis enable (0 = disabled, 1 = enabled) 1
bmanga95 0:772bf4786416 97 0 XEN X-axis enable (0 = disabled, 1 = enabled) 1 */
bmanga95 0:772bf4786416 98
bmanga95 0:772bf4786416 99 /* Reset then switch to normal mode and enable all three channels */
bmanga95 0:772bf4786416 100 write8(GYRO_REGISTER_CTRL_REG1, 0x00);
bmanga95 0:772bf4786416 101 write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
bmanga95 0:772bf4786416 102 /* ------------------------------------------------------------------ */
bmanga95 0:772bf4786416 103
bmanga95 0:772bf4786416 104 /* Set CTRL_REG2 (0x21)
bmanga95 0:772bf4786416 105 ====================================================================
bmanga95 0:772bf4786416 106 BIT Symbol Description Default
bmanga95 0:772bf4786416 107 --- ------ --------------------------------------------- -------
bmanga95 0:772bf4786416 108 5-4 HPM1/0 High-pass filter mode selection 00
bmanga95 0:772bf4786416 109 3-0 HPCF3..0 High-pass filter cutoff frequency selection 0000 */
bmanga95 0:772bf4786416 110
bmanga95 0:772bf4786416 111 /* Nothing to do ... keep default values */
bmanga95 0:772bf4786416 112 /* ------------------------------------------------------------------ */
bmanga95 0:772bf4786416 113
bmanga95 0:772bf4786416 114 /* Set CTRL_REG3 (0x22)
bmanga95 0:772bf4786416 115 ====================================================================
bmanga95 0:772bf4786416 116 BIT Symbol Description Default
bmanga95 0:772bf4786416 117 --- ------ --------------------------------------------- -------
bmanga95 0:772bf4786416 118 7 I1_Int1 Interrupt enable on INT1 (0=disable,1=enable) 0
bmanga95 0:772bf4786416 119 6 I1_Boot Boot status on INT1 (0=disable,1=enable) 0
bmanga95 0:772bf4786416 120 5 H-Lactive Interrupt active config on INT1 (0=high,1=low) 0
bmanga95 0:772bf4786416 121 4 PP_OD Push-Pull/Open-Drain (0=PP, 1=OD) 0
bmanga95 0:772bf4786416 122 3 I2_DRDY Data ready on DRDY/INT2 (0=disable,1=enable) 0
bmanga95 0:772bf4786416 123 2 I2_WTM FIFO wtrmrk int on DRDY/INT2 (0=dsbl,1=enbl) 0
bmanga95 0:772bf4786416 124 1 I2_ORun FIFO overrun int on DRDY/INT2 (0=dsbl,1=enbl) 0
bmanga95 0:772bf4786416 125 0 I2_Empty FIFI empty int on DRDY/INT2 (0=dsbl,1=enbl) 0 */
bmanga95 0:772bf4786416 126
bmanga95 0:772bf4786416 127 /* Nothing to do ... keep default values */
bmanga95 0:772bf4786416 128 /* ------------------------------------------------------------------ */
bmanga95 0:772bf4786416 129
bmanga95 0:772bf4786416 130 /* Set CTRL_REG4 (0x23)
bmanga95 0:772bf4786416 131 ====================================================================
bmanga95 0:772bf4786416 132 BIT Symbol Description Default
bmanga95 0:772bf4786416 133 --- ------ --------------------------------------------- -------
bmanga95 0:772bf4786416 134 7 BDU Block Data Update (0=continuous, 1=LSB/MSB) 0
bmanga95 0:772bf4786416 135 6 BLE Big/Little-Endian (0=Data LSB, 1=Data MSB) 0
bmanga95 0:772bf4786416 136 5-4 FS1/0 Full scale selection 00
bmanga95 0:772bf4786416 137 00 = 250 dps
bmanga95 0:772bf4786416 138 01 = 500 dps
bmanga95 0:772bf4786416 139 10 = 2000 dps
bmanga95 0:772bf4786416 140 11 = 2000 dps
bmanga95 0:772bf4786416 141 0 SIM SPI Mode (0=4-wire, 1=3-wire) 0 */
bmanga95 0:772bf4786416 142
bmanga95 0:772bf4786416 143 /* Adjust resolution if requested */
bmanga95 0:772bf4786416 144 switch(_range)
bmanga95 0:772bf4786416 145 {
bmanga95 0:772bf4786416 146 case GYRO_RANGE_250DPS:
bmanga95 0:772bf4786416 147 write8(GYRO_REGISTER_CTRL_REG4, 0x00);
bmanga95 0:772bf4786416 148 break;
bmanga95 0:772bf4786416 149 case GYRO_RANGE_500DPS:
bmanga95 0:772bf4786416 150 write8(GYRO_REGISTER_CTRL_REG4, 0x10);
bmanga95 0:772bf4786416 151 break;
bmanga95 0:772bf4786416 152 case GYRO_RANGE_2000DPS:
bmanga95 0:772bf4786416 153 write8(GYRO_REGISTER_CTRL_REG4, 0x20);
bmanga95 0:772bf4786416 154 break;
bmanga95 0:772bf4786416 155 }
bmanga95 0:772bf4786416 156 /* ------------------------------------------------------------------ */
bmanga95 0:772bf4786416 157
bmanga95 0:772bf4786416 158 /* Set CTRL_REG5 (0x24)
bmanga95 0:772bf4786416 159 ====================================================================
bmanga95 0:772bf4786416 160 BIT Symbol Description Default
bmanga95 0:772bf4786416 161 --- ------ --------------------------------------------- -------
bmanga95 0:772bf4786416 162 7 BOOT Reboot memory content (0=normal, 1=reboot) 0
bmanga95 0:772bf4786416 163 6 FIFO_EN FIFO enable (0=FIFO disable, 1=enable) 0
bmanga95 0:772bf4786416 164 4 HPen High-pass filter enable (0=disable,1=enable) 0
bmanga95 0:772bf4786416 165 3-2 INT1_SEL INT1 Selection config 00
bmanga95 0:772bf4786416 166 1-0 OUT_SEL Out selection config 00 */
bmanga95 0:772bf4786416 167
bmanga95 0:772bf4786416 168 /* Nothing to do ... keep default values */
bmanga95 0:772bf4786416 169 /* ------------------------------------------------------------------ */
bmanga95 0:772bf4786416 170
bmanga95 0:772bf4786416 171 return true;
bmanga95 0:772bf4786416 172 }
bmanga95 0:772bf4786416 173
bmanga95 0:772bf4786416 174 /**************************************************************************/
bmanga95 0:772bf4786416 175 /*!
bmanga95 0:772bf4786416 176 @brief Enables or disables auto-ranging
bmanga95 0:772bf4786416 177 */
bmanga95 0:772bf4786416 178 /**************************************************************************/
bmanga95 0:772bf4786416 179 void Adafruit_L3GD20_Unified::enableAutoRange(bool enabled)
bmanga95 0:772bf4786416 180 {
bmanga95 0:772bf4786416 181 _autoRangeEnabled = enabled;
bmanga95 0:772bf4786416 182 }
bmanga95 0:772bf4786416 183
bmanga95 0:772bf4786416 184 /**************************************************************************/
bmanga95 0:772bf4786416 185 /*!
bmanga95 0:772bf4786416 186 @brief Gets the most recent sensor event
bmanga95 0:772bf4786416 187 */
bmanga95 0:772bf4786416 188 /**************************************************************************/
bmanga95 0:772bf4786416 189 void Adafruit_L3GD20_Unified::getEvent(sensors_event_t* event)
bmanga95 0:772bf4786416 190 {
bmanga95 0:772bf4786416 191 bool readingValid = false;
bmanga95 0:772bf4786416 192
bmanga95 0:772bf4786416 193 /* Clear the event */
bmanga95 0:772bf4786416 194 memset(event, 0, sizeof(sensors_event_t));
bmanga95 0:772bf4786416 195
bmanga95 0:772bf4786416 196 event->version = sizeof(sensors_event_t);
bmanga95 0:772bf4786416 197 event->sensor_id = _sensorID;
bmanga95 0:772bf4786416 198 event->type = SENSOR_TYPE_GYROSCOPE;
bmanga95 0:772bf4786416 199
bmanga95 0:772bf4786416 200 while(!readingValid)
bmanga95 0:772bf4786416 201 {
bmanga95 0:772bf4786416 202 event->timestamp = millis();
bmanga95 0:772bf4786416 203
bmanga95 0:772bf4786416 204
bmanga95 0:772bf4786416 205 i2c->writeByte(L3GD20_ADDRESS, GYRO_REGISTER_OUT_X_L | 0x80);
bmanga95 0:772bf4786416 206
bmanga95 0:772bf4786416 207 byte data[6];
bmanga95 0:772bf4786416 208
bmanga95 0:772bf4786416 209 i2c->read(L3GD20_ADDRESS, data, 6);
bmanga95 0:772bf4786416 210
bmanga95 0:772bf4786416 211 /* Shift values to create properly formed integer (low byte first) */
bmanga95 0:772bf4786416 212 event->gyro.x = (int16_t)(data[0] | (data[1] << 8));
bmanga95 0:772bf4786416 213 event->gyro.y = (int16_t)(data[2] | (data[3] << 8));
bmanga95 0:772bf4786416 214 event->gyro.z = (int16_t)(data[4] | (data[5] << 8));
bmanga95 0:772bf4786416 215
bmanga95 0:772bf4786416 216 /* Make sure the sensor isn't saturating if auto-ranging is enabled */
bmanga95 0:772bf4786416 217 if (!_autoRangeEnabled)
bmanga95 0:772bf4786416 218 {
bmanga95 0:772bf4786416 219 readingValid = true;
bmanga95 0:772bf4786416 220 }
bmanga95 0:772bf4786416 221 else
bmanga95 0:772bf4786416 222 {
bmanga95 0:772bf4786416 223 /* Check if the sensor is saturating or not */
bmanga95 0:772bf4786416 224 if ( (event->gyro.x >= 32760) | (event->gyro.x <= -32760) |
bmanga95 0:772bf4786416 225 (event->gyro.y >= 32760) | (event->gyro.y <= -32760) |
bmanga95 0:772bf4786416 226 (event->gyro.z >= 32760) | (event->gyro.z <= -32760) )
bmanga95 0:772bf4786416 227 {
bmanga95 0:772bf4786416 228 /* Saturating .... increase the range if we can */
bmanga95 0:772bf4786416 229 switch(_range)
bmanga95 0:772bf4786416 230 {
bmanga95 0:772bf4786416 231 case GYRO_RANGE_500DPS:
bmanga95 0:772bf4786416 232 /* Push the range up to 2000dps */
bmanga95 0:772bf4786416 233 _range = GYRO_RANGE_2000DPS;
bmanga95 0:772bf4786416 234 write8(GYRO_REGISTER_CTRL_REG1, 0x00);
bmanga95 0:772bf4786416 235 write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
bmanga95 0:772bf4786416 236 write8(GYRO_REGISTER_CTRL_REG4, 0x20);
bmanga95 0:772bf4786416 237 write8(GYRO_REGISTER_CTRL_REG5, 0x80);
bmanga95 0:772bf4786416 238 readingValid = false;
bmanga95 0:772bf4786416 239 // Serial.println("Changing range to 2000DPS");
bmanga95 0:772bf4786416 240 break;
bmanga95 0:772bf4786416 241 case GYRO_RANGE_250DPS:
bmanga95 0:772bf4786416 242 /* Push the range up to 500dps */
bmanga95 0:772bf4786416 243 _range = GYRO_RANGE_500DPS;
bmanga95 0:772bf4786416 244 write8(GYRO_REGISTER_CTRL_REG1, 0x00);
bmanga95 0:772bf4786416 245 write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
bmanga95 0:772bf4786416 246 write8(GYRO_REGISTER_CTRL_REG4, 0x10);
bmanga95 0:772bf4786416 247 write8(GYRO_REGISTER_CTRL_REG5, 0x80);
bmanga95 0:772bf4786416 248 readingValid = false;
bmanga95 0:772bf4786416 249 // Serial.println("Changing range to 500DPS");
bmanga95 0:772bf4786416 250 break;
bmanga95 0:772bf4786416 251 default:
bmanga95 0:772bf4786416 252 readingValid = true;
bmanga95 0:772bf4786416 253 break;
bmanga95 0:772bf4786416 254 }
bmanga95 0:772bf4786416 255 }
bmanga95 0:772bf4786416 256 else
bmanga95 0:772bf4786416 257 {
bmanga95 0:772bf4786416 258 /* All values are withing range */
bmanga95 0:772bf4786416 259 readingValid = true;
bmanga95 0:772bf4786416 260 }
bmanga95 0:772bf4786416 261 }
bmanga95 0:772bf4786416 262 }
bmanga95 0:772bf4786416 263
bmanga95 0:772bf4786416 264 /* Compensate values depending on the resolution */
bmanga95 0:772bf4786416 265 switch(_range)
bmanga95 0:772bf4786416 266 {
bmanga95 0:772bf4786416 267 case GYRO_RANGE_250DPS:
bmanga95 0:772bf4786416 268 event->gyro.x *= GYRO_SENSITIVITY_250DPS;
bmanga95 0:772bf4786416 269 event->gyro.y *= GYRO_SENSITIVITY_250DPS;
bmanga95 0:772bf4786416 270 event->gyro.z *= GYRO_SENSITIVITY_250DPS;
bmanga95 0:772bf4786416 271 break;
bmanga95 0:772bf4786416 272 case GYRO_RANGE_500DPS:
bmanga95 0:772bf4786416 273 event->gyro.x *= GYRO_SENSITIVITY_500DPS;
bmanga95 0:772bf4786416 274 event->gyro.y *= GYRO_SENSITIVITY_500DPS;
bmanga95 0:772bf4786416 275 event->gyro.z *= GYRO_SENSITIVITY_500DPS;
bmanga95 0:772bf4786416 276 break;
bmanga95 0:772bf4786416 277 case GYRO_RANGE_2000DPS:
bmanga95 0:772bf4786416 278 event->gyro.x *= GYRO_SENSITIVITY_2000DPS;
bmanga95 0:772bf4786416 279 event->gyro.y *= GYRO_SENSITIVITY_2000DPS;
bmanga95 0:772bf4786416 280 event->gyro.z *= GYRO_SENSITIVITY_2000DPS;
bmanga95 0:772bf4786416 281 break;
bmanga95 0:772bf4786416 282 }
bmanga95 0:772bf4786416 283
bmanga95 0:772bf4786416 284 /* Convert values to rad/s */
bmanga95 0:772bf4786416 285 event->gyro.x *= SENSORS_DPS_TO_RADS;
bmanga95 0:772bf4786416 286 event->gyro.y *= SENSORS_DPS_TO_RADS;
bmanga95 0:772bf4786416 287 event->gyro.z *= SENSORS_DPS_TO_RADS;
bmanga95 0:772bf4786416 288 }
bmanga95 0:772bf4786416 289
bmanga95 0:772bf4786416 290 /**************************************************************************/
bmanga95 0:772bf4786416 291 /*!
bmanga95 0:772bf4786416 292 @brief Gets the sensor_t data
bmanga95 0:772bf4786416 293 */
bmanga95 0:772bf4786416 294 /**************************************************************************/
bmanga95 0:772bf4786416 295 void Adafruit_L3GD20_Unified::getSensor(sensor_t* sensor)
bmanga95 0:772bf4786416 296 {
bmanga95 0:772bf4786416 297 /* Clear the sensor_t object */
bmanga95 0:772bf4786416 298 memset(sensor, 0, sizeof(sensor_t));
bmanga95 0:772bf4786416 299
bmanga95 0:772bf4786416 300 /* Insert the sensor name in the fixed length char array */
bmanga95 0:772bf4786416 301 strncpy (sensor->name, "L3GD20", sizeof(sensor->name) - 1);
bmanga95 0:772bf4786416 302 sensor->name[sizeof(sensor->name)- 1] = 0;
bmanga95 0:772bf4786416 303 sensor->version = 1;
bmanga95 0:772bf4786416 304 sensor->sensor_id = _sensorID;
bmanga95 0:772bf4786416 305 sensor->type = SENSOR_TYPE_GYROSCOPE;
bmanga95 0:772bf4786416 306 sensor->min_delay = 0;
bmanga95 0:772bf4786416 307 sensor->max_value = (float)this->_range * SENSORS_DPS_TO_RADS;
bmanga95 0:772bf4786416 308 sensor->min_value = (this->_range * -1.0) * SENSORS_DPS_TO_RADS;
bmanga95 0:772bf4786416 309 sensor->resolution = 0.0F; // TBD
bmanga95 0:772bf4786416 310 }