Team_temp / VT1_domc

Dependencies:   mbed-dev

Fork of Adafruit9-DOf_AHRS_Regler_Discrete by RT-Labor IMS

Committer:
domanpie
Date:
Thu Oct 19 11:50:04 2017 +0000
Revision:
1:8c4f93e10af3
Parent:
0:772bf4786416
Child:
3:bd353b8184cc
V1.0; domc

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
domanpie 1:8c4f93e10af3 76 /* Set the range to 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 */
domanpie 1:8c4f93e10af3 81 uint8_t id = 0;
domanpie 1:8c4f93e10af3 82 id = read8(GYRO_REGISTER_WHO_AM_I);
domanpie 1:8c4f93e10af3 83 Serial serial( USBTX, USBRX);
domanpie 1:8c4f93e10af3 84 serial.printf("WHOAMI Gyro %d \r\n", id);
bmanga95 0:772bf4786416 85 if ((id != L3GD20_ID) && (id != L3GD20H_ID))
bmanga95 0:772bf4786416 86 {
bmanga95 0:772bf4786416 87 return false;
bmanga95 0:772bf4786416 88 }
bmanga95 0:772bf4786416 89
bmanga95 0:772bf4786416 90 /* Set CTRL_REG1 (0x20)
bmanga95 0:772bf4786416 91 ====================================================================
bmanga95 0:772bf4786416 92 BIT Symbol Description Default
bmanga95 0:772bf4786416 93 --- ------ --------------------------------------------- -------
bmanga95 0:772bf4786416 94 7-6 DR1/0 Output data rate 00
bmanga95 0:772bf4786416 95 5-4 BW1/0 Bandwidth selection 00
bmanga95 0:772bf4786416 96 3 PD 0 = Power-down mode, 1 = normal/sleep mode 0
bmanga95 0:772bf4786416 97 2 ZEN Z-axis enable (0 = disabled, 1 = enabled) 1
bmanga95 0:772bf4786416 98 1 YEN Y-axis enable (0 = disabled, 1 = enabled) 1
bmanga95 0:772bf4786416 99 0 XEN X-axis enable (0 = disabled, 1 = enabled) 1 */
bmanga95 0:772bf4786416 100
bmanga95 0:772bf4786416 101 /* Reset then switch to normal mode and enable all three channels */
domanpie 1:8c4f93e10af3 102 write8(GYRO_REGISTER_CTRL_REG1, 0x80);
domanpie 1:8c4f93e10af3 103 write8(GYRO_REGISTER_CTRL_REG1, 0xBF);// 380 Hz; 100 Hz cutoff LPF1
bmanga95 0:772bf4786416 104 /* ------------------------------------------------------------------ */
bmanga95 0:772bf4786416 105
bmanga95 0:772bf4786416 106 /* Set CTRL_REG2 (0x21)
bmanga95 0:772bf4786416 107 ====================================================================
bmanga95 0:772bf4786416 108 BIT Symbol Description Default
bmanga95 0:772bf4786416 109 --- ------ --------------------------------------------- -------
bmanga95 0:772bf4786416 110 5-4 HPM1/0 High-pass filter mode selection 00
bmanga95 0:772bf4786416 111 3-0 HPCF3..0 High-pass filter cutoff frequency selection 0000 */
domanpie 1:8c4f93e10af3 112
bmanga95 0:772bf4786416 113 /* Nothing to do ... keep default values */
bmanga95 0:772bf4786416 114 /* ------------------------------------------------------------------ */
domanpie 1:8c4f93e10af3 115 // -> High-pass cutoff frequency: 27 Hz
bmanga95 0:772bf4786416 116 /* Set CTRL_REG3 (0x22)
bmanga95 0:772bf4786416 117 ====================================================================
bmanga95 0:772bf4786416 118 BIT Symbol Description Default
bmanga95 0:772bf4786416 119 --- ------ --------------------------------------------- -------
bmanga95 0:772bf4786416 120 7 I1_Int1 Interrupt enable on INT1 (0=disable,1=enable) 0
bmanga95 0:772bf4786416 121 6 I1_Boot Boot status on INT1 (0=disable,1=enable) 0
bmanga95 0:772bf4786416 122 5 H-Lactive Interrupt active config on INT1 (0=high,1=low) 0
bmanga95 0:772bf4786416 123 4 PP_OD Push-Pull/Open-Drain (0=PP, 1=OD) 0
bmanga95 0:772bf4786416 124 3 I2_DRDY Data ready on DRDY/INT2 (0=disable,1=enable) 0
bmanga95 0:772bf4786416 125 2 I2_WTM FIFO wtrmrk int on DRDY/INT2 (0=dsbl,1=enbl) 0
bmanga95 0:772bf4786416 126 1 I2_ORun FIFO overrun int on DRDY/INT2 (0=dsbl,1=enbl) 0
bmanga95 0:772bf4786416 127 0 I2_Empty FIFI empty int on DRDY/INT2 (0=dsbl,1=enbl) 0 */
bmanga95 0:772bf4786416 128
bmanga95 0:772bf4786416 129 /* Nothing to do ... keep default values */
bmanga95 0:772bf4786416 130 /* ------------------------------------------------------------------ */
bmanga95 0:772bf4786416 131
bmanga95 0:772bf4786416 132 /* Set CTRL_REG4 (0x23)
bmanga95 0:772bf4786416 133 ====================================================================
bmanga95 0:772bf4786416 134 BIT Symbol Description Default
bmanga95 0:772bf4786416 135 --- ------ --------------------------------------------- -------
bmanga95 0:772bf4786416 136 7 BDU Block Data Update (0=continuous, 1=LSB/MSB) 0
bmanga95 0:772bf4786416 137 6 BLE Big/Little-Endian (0=Data LSB, 1=Data MSB) 0
bmanga95 0:772bf4786416 138 5-4 FS1/0 Full scale selection 00
bmanga95 0:772bf4786416 139 00 = 250 dps
bmanga95 0:772bf4786416 140 01 = 500 dps
bmanga95 0:772bf4786416 141 10 = 2000 dps
bmanga95 0:772bf4786416 142 11 = 2000 dps
bmanga95 0:772bf4786416 143 0 SIM SPI Mode (0=4-wire, 1=3-wire) 0 */
bmanga95 0:772bf4786416 144
bmanga95 0:772bf4786416 145 /* Adjust resolution if requested */
bmanga95 0:772bf4786416 146 switch(_range)
bmanga95 0:772bf4786416 147 {
bmanga95 0:772bf4786416 148 case GYRO_RANGE_250DPS:
bmanga95 0:772bf4786416 149 write8(GYRO_REGISTER_CTRL_REG4, 0x00);
bmanga95 0:772bf4786416 150 break;
bmanga95 0:772bf4786416 151 case GYRO_RANGE_500DPS:
bmanga95 0:772bf4786416 152 write8(GYRO_REGISTER_CTRL_REG4, 0x10);
bmanga95 0:772bf4786416 153 break;
bmanga95 0:772bf4786416 154 case GYRO_RANGE_2000DPS:
bmanga95 0:772bf4786416 155 write8(GYRO_REGISTER_CTRL_REG4, 0x20);
bmanga95 0:772bf4786416 156 break;
bmanga95 0:772bf4786416 157 }
bmanga95 0:772bf4786416 158 /* ------------------------------------------------------------------ */
bmanga95 0:772bf4786416 159
bmanga95 0:772bf4786416 160 /* Set CTRL_REG5 (0x24)
bmanga95 0:772bf4786416 161 ====================================================================
bmanga95 0:772bf4786416 162 BIT Symbol Description Default
bmanga95 0:772bf4786416 163 --- ------ --------------------------------------------- -------
bmanga95 0:772bf4786416 164 7 BOOT Reboot memory content (0=normal, 1=reboot) 0
bmanga95 0:772bf4786416 165 6 FIFO_EN FIFO enable (0=FIFO disable, 1=enable) 0
bmanga95 0:772bf4786416 166 4 HPen High-pass filter enable (0=disable,1=enable) 0
bmanga95 0:772bf4786416 167 3-2 INT1_SEL INT1 Selection config 00
bmanga95 0:772bf4786416 168 1-0 OUT_SEL Out selection config 00 */
bmanga95 0:772bf4786416 169
bmanga95 0:772bf4786416 170 /* Nothing to do ... keep default values */
bmanga95 0:772bf4786416 171 /* ------------------------------------------------------------------ */
bmanga95 0:772bf4786416 172
bmanga95 0:772bf4786416 173 return true;
bmanga95 0:772bf4786416 174 }
bmanga95 0:772bf4786416 175
bmanga95 0:772bf4786416 176 /**************************************************************************/
bmanga95 0:772bf4786416 177 /*!
bmanga95 0:772bf4786416 178 @brief Enables or disables auto-ranging
bmanga95 0:772bf4786416 179 */
bmanga95 0:772bf4786416 180 /**************************************************************************/
bmanga95 0:772bf4786416 181 void Adafruit_L3GD20_Unified::enableAutoRange(bool enabled)
bmanga95 0:772bf4786416 182 {
bmanga95 0:772bf4786416 183 _autoRangeEnabled = enabled;
bmanga95 0:772bf4786416 184 }
bmanga95 0:772bf4786416 185
bmanga95 0:772bf4786416 186 /**************************************************************************/
bmanga95 0:772bf4786416 187 /*!
bmanga95 0:772bf4786416 188 @brief Gets the most recent sensor event
bmanga95 0:772bf4786416 189 */
bmanga95 0:772bf4786416 190 /**************************************************************************/
bmanga95 0:772bf4786416 191 void Adafruit_L3GD20_Unified::getEvent(sensors_event_t* event)
bmanga95 0:772bf4786416 192 {
bmanga95 0:772bf4786416 193 bool readingValid = false;
bmanga95 0:772bf4786416 194
bmanga95 0:772bf4786416 195 /* Clear the event */
bmanga95 0:772bf4786416 196 memset(event, 0, sizeof(sensors_event_t));
bmanga95 0:772bf4786416 197
bmanga95 0:772bf4786416 198 event->version = sizeof(sensors_event_t);
bmanga95 0:772bf4786416 199 event->sensor_id = _sensorID;
bmanga95 0:772bf4786416 200 event->type = SENSOR_TYPE_GYROSCOPE;
bmanga95 0:772bf4786416 201
bmanga95 0:772bf4786416 202 while(!readingValid)
bmanga95 0:772bf4786416 203 {
bmanga95 0:772bf4786416 204 event->timestamp = millis();
bmanga95 0:772bf4786416 205
bmanga95 0:772bf4786416 206
bmanga95 0:772bf4786416 207 i2c->writeByte(L3GD20_ADDRESS, GYRO_REGISTER_OUT_X_L | 0x80);
bmanga95 0:772bf4786416 208
bmanga95 0:772bf4786416 209 byte data[6];
bmanga95 0:772bf4786416 210
bmanga95 0:772bf4786416 211 i2c->read(L3GD20_ADDRESS, data, 6);
bmanga95 0:772bf4786416 212
bmanga95 0:772bf4786416 213 /* Shift values to create properly formed integer (low byte first) */
bmanga95 0:772bf4786416 214 event->gyro.x = (int16_t)(data[0] | (data[1] << 8));
domanpie 1:8c4f93e10af3 215 raw.x = (int16_t)(data[0] | (data[1] << 8));
domanpie 1:8c4f93e10af3 216
bmanga95 0:772bf4786416 217 event->gyro.y = (int16_t)(data[2] | (data[3] << 8));
domanpie 1:8c4f93e10af3 218 raw.y = (int16_t)(data[2] | (data[3] << 8));
domanpie 1:8c4f93e10af3 219
bmanga95 0:772bf4786416 220 event->gyro.z = (int16_t)(data[4] | (data[5] << 8));
domanpie 1:8c4f93e10af3 221 raw.y = (int16_t)(data[4] | (data[5] << 8));
bmanga95 0:772bf4786416 222
bmanga95 0:772bf4786416 223 /* Make sure the sensor isn't saturating if auto-ranging is enabled */
bmanga95 0:772bf4786416 224 if (!_autoRangeEnabled)
bmanga95 0:772bf4786416 225 {
bmanga95 0:772bf4786416 226 readingValid = true;
bmanga95 0:772bf4786416 227 }
bmanga95 0:772bf4786416 228 else
bmanga95 0:772bf4786416 229 {
bmanga95 0:772bf4786416 230 /* Check if the sensor is saturating or not */
bmanga95 0:772bf4786416 231 if ( (event->gyro.x >= 32760) | (event->gyro.x <= -32760) |
bmanga95 0:772bf4786416 232 (event->gyro.y >= 32760) | (event->gyro.y <= -32760) |
bmanga95 0:772bf4786416 233 (event->gyro.z >= 32760) | (event->gyro.z <= -32760) )
bmanga95 0:772bf4786416 234 {
bmanga95 0:772bf4786416 235 /* Saturating .... increase the range if we can */
bmanga95 0:772bf4786416 236 switch(_range)
bmanga95 0:772bf4786416 237 {
bmanga95 0:772bf4786416 238 case GYRO_RANGE_500DPS:
bmanga95 0:772bf4786416 239 /* Push the range up to 2000dps */
bmanga95 0:772bf4786416 240 _range = GYRO_RANGE_2000DPS;
bmanga95 0:772bf4786416 241 write8(GYRO_REGISTER_CTRL_REG1, 0x00);
bmanga95 0:772bf4786416 242 write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
bmanga95 0:772bf4786416 243 write8(GYRO_REGISTER_CTRL_REG4, 0x20);
bmanga95 0:772bf4786416 244 write8(GYRO_REGISTER_CTRL_REG5, 0x80);
bmanga95 0:772bf4786416 245 readingValid = false;
bmanga95 0:772bf4786416 246 // Serial.println("Changing range to 2000DPS");
bmanga95 0:772bf4786416 247 break;
bmanga95 0:772bf4786416 248 case GYRO_RANGE_250DPS:
bmanga95 0:772bf4786416 249 /* Push the range up to 500dps */
bmanga95 0:772bf4786416 250 _range = GYRO_RANGE_500DPS;
bmanga95 0:772bf4786416 251 write8(GYRO_REGISTER_CTRL_REG1, 0x00);
bmanga95 0:772bf4786416 252 write8(GYRO_REGISTER_CTRL_REG1, 0x0F);
bmanga95 0:772bf4786416 253 write8(GYRO_REGISTER_CTRL_REG4, 0x10);
bmanga95 0:772bf4786416 254 write8(GYRO_REGISTER_CTRL_REG5, 0x80);
bmanga95 0:772bf4786416 255 readingValid = false;
bmanga95 0:772bf4786416 256 // Serial.println("Changing range to 500DPS");
bmanga95 0:772bf4786416 257 break;
bmanga95 0:772bf4786416 258 default:
bmanga95 0:772bf4786416 259 readingValid = true;
bmanga95 0:772bf4786416 260 break;
bmanga95 0:772bf4786416 261 }
bmanga95 0:772bf4786416 262 }
bmanga95 0:772bf4786416 263 else
bmanga95 0:772bf4786416 264 {
bmanga95 0:772bf4786416 265 /* All values are withing range */
bmanga95 0:772bf4786416 266 readingValid = true;
bmanga95 0:772bf4786416 267 }
bmanga95 0:772bf4786416 268 }
bmanga95 0:772bf4786416 269 }
bmanga95 0:772bf4786416 270
bmanga95 0:772bf4786416 271 /* Compensate values depending on the resolution */
bmanga95 0:772bf4786416 272 switch(_range)
bmanga95 0:772bf4786416 273 {
bmanga95 0:772bf4786416 274 case GYRO_RANGE_250DPS:
bmanga95 0:772bf4786416 275 event->gyro.x *= GYRO_SENSITIVITY_250DPS;
bmanga95 0:772bf4786416 276 event->gyro.y *= GYRO_SENSITIVITY_250DPS;
bmanga95 0:772bf4786416 277 event->gyro.z *= GYRO_SENSITIVITY_250DPS;
bmanga95 0:772bf4786416 278 break;
bmanga95 0:772bf4786416 279 case GYRO_RANGE_500DPS:
bmanga95 0:772bf4786416 280 event->gyro.x *= GYRO_SENSITIVITY_500DPS;
bmanga95 0:772bf4786416 281 event->gyro.y *= GYRO_SENSITIVITY_500DPS;
bmanga95 0:772bf4786416 282 event->gyro.z *= GYRO_SENSITIVITY_500DPS;
bmanga95 0:772bf4786416 283 break;
bmanga95 0:772bf4786416 284 case GYRO_RANGE_2000DPS:
bmanga95 0:772bf4786416 285 event->gyro.x *= GYRO_SENSITIVITY_2000DPS;
bmanga95 0:772bf4786416 286 event->gyro.y *= GYRO_SENSITIVITY_2000DPS;
bmanga95 0:772bf4786416 287 event->gyro.z *= GYRO_SENSITIVITY_2000DPS;
bmanga95 0:772bf4786416 288 break;
bmanga95 0:772bf4786416 289 }
bmanga95 0:772bf4786416 290
bmanga95 0:772bf4786416 291 /* Convert values to rad/s */
bmanga95 0:772bf4786416 292 event->gyro.x *= SENSORS_DPS_TO_RADS;
bmanga95 0:772bf4786416 293 event->gyro.y *= SENSORS_DPS_TO_RADS;
bmanga95 0:772bf4786416 294 event->gyro.z *= SENSORS_DPS_TO_RADS;
bmanga95 0:772bf4786416 295 }
bmanga95 0:772bf4786416 296
bmanga95 0:772bf4786416 297 /**************************************************************************/
bmanga95 0:772bf4786416 298 /*!
bmanga95 0:772bf4786416 299 @brief Gets the sensor_t data
bmanga95 0:772bf4786416 300 */
bmanga95 0:772bf4786416 301 /**************************************************************************/
bmanga95 0:772bf4786416 302 void Adafruit_L3GD20_Unified::getSensor(sensor_t* sensor)
bmanga95 0:772bf4786416 303 {
bmanga95 0:772bf4786416 304 /* Clear the sensor_t object */
bmanga95 0:772bf4786416 305 memset(sensor, 0, sizeof(sensor_t));
bmanga95 0:772bf4786416 306
bmanga95 0:772bf4786416 307 /* Insert the sensor name in the fixed length char array */
bmanga95 0:772bf4786416 308 strncpy (sensor->name, "L3GD20", sizeof(sensor->name) - 1);
bmanga95 0:772bf4786416 309 sensor->name[sizeof(sensor->name)- 1] = 0;
bmanga95 0:772bf4786416 310 sensor->version = 1;
bmanga95 0:772bf4786416 311 sensor->sensor_id = _sensorID;
bmanga95 0:772bf4786416 312 sensor->type = SENSOR_TYPE_GYROSCOPE;
bmanga95 0:772bf4786416 313 sensor->min_delay = 0;
bmanga95 0:772bf4786416 314 sensor->max_value = (float)this->_range * SENSORS_DPS_TO_RADS;
bmanga95 0:772bf4786416 315 sensor->min_value = (this->_range * -1.0) * SENSORS_DPS_TO_RADS;
bmanga95 0:772bf4786416 316 sensor->resolution = 0.0F; // TBD
bmanga95 0:772bf4786416 317 }