LDSC_Robotics_TAs / LSM6DS3

compiled unsuccessful but not sure if it works

Fork of LSM6DS3 by adam&chuang

LSM6DS3.cpp@2:c19d384b2896, 2016-02-23 (annotated)

Committer:
adam_z
Date:
Tue Feb 23 08:48:56 2016 +0000
Revision:
2:c19d384b2896
Parent:
1:cc2caaf5536c
Child:
3:d5307f077753
output remain constant

Who changed what in which revision?

UserRevisionLine numberNew contents of line
adam_z 0:301b2cb34ead 1 /******************************************************************************
adam_z 0:301b2cb34ead 2 SparkFunLSM6DS3.cpp
adam_z 0:301b2cb34ead 3 LSM6DS3 Arduino and Teensy Driver
adam_z 0:301b2cb34ead 4
adam_z 0:301b2cb34ead 5 Marshall Taylor @ SparkFun Electronics
adam_z 0:301b2cb34ead 6 May 20, 2015
adam_z 0:301b2cb34ead 7 https://github.com/sparkfun/LSM6DS3_Breakout
adam_z 0:301b2cb34ead 8 https://github.com/sparkfun/SparkFun_LSM6DS3_Arduino_Library
adam_z 0:301b2cb34ead 9
adam_z 0:301b2cb34ead 10 Resources:
adam_z 0:301b2cb34ead 11 Uses Wire.h for i2c operation
adam_z 0:301b2cb34ead 12 Uses SPI.h for SPI operation
adam_z 0:301b2cb34ead 13 Either can be omitted if not used
adam_z 0:301b2cb34ead 14
adam_z 0:301b2cb34ead 15 Development environment specifics:
adam_z 0:301b2cb34ead 16 Arduino IDE 1.6.4
adam_z 0:301b2cb34ead 17 Teensy loader 1.23
adam_z 0:301b2cb34ead 18
adam_z 0:301b2cb34ead 19 This code is released under the [MIT License](http://opensource.org/licenses/MIT).
adam_z 0:301b2cb34ead 20
adam_z 0:301b2cb34ead 21 Please review the LICENSE.md file included with this example. If you have any questions
adam_z 0:301b2cb34ead 22 or concerns with licensing, please contact techsupport@sparkfun.com.
adam_z 0:301b2cb34ead 23
adam_z 0:301b2cb34ead 24 Distributed as-is; no warranty is given.
adam_z 0:301b2cb34ead 25 ******************************************************************************/
adam_z 0:301b2cb34ead 26
adam_z 0:301b2cb34ead 27 //See SparkFunLSM6DS3.h for additional topology notes.
adam_z 0:301b2cb34ead 28 #include "mbed.h"
adam_z 0:301b2cb34ead 29 #include "LSM6DS3.h"
adam_z 0:301b2cb34ead 30 #include "LSM6DS3_Types.h"
adam_z 0:301b2cb34ead 31 #include "LSM6DS3_Registers.h"
adam_z 0:301b2cb34ead 32 #include "stdint.h"
adam_z 0:301b2cb34ead 33 #include "math.h"
adam_z 0:301b2cb34ead 34
adam_z 1:cc2caaf5536c 35 //I2C i2c(D14,D15);
adam_z 1:cc2caaf5536c 36 //SPI spi(D11,D12,D13);
adam_z 0:301b2cb34ead 37 //****************************************************************************//
adam_z 0:301b2cb34ead 38 //
adam_z 0:301b2cb34ead 39 // LSM6DS3Core functions.
adam_z 0:301b2cb34ead 40 //
adam_z 0:301b2cb34ead 41 // Construction arguments:
adam_z 0:301b2cb34ead 42 // ( uint8_t busType, uint8_t inputArg ),
adam_z 0:301b2cb34ead 43 //
adam_z 0:301b2cb34ead 44 // where inputArg is address for I2C_MODE and chip select pin
adam_z 0:301b2cb34ead 45 // number for SPI_MODE
adam_z 0:301b2cb34ead 46 //
adam_z 0:301b2cb34ead 47 // For SPI, construct LSM6DS3Core myIMU(SPI_MODE, 10);
adam_z 0:301b2cb34ead 48 // For I2C, construct LSM6DS3Core myIMU(I2C_MODE, 0x6B);
adam_z 0:301b2cb34ead 49 //
adam_z 0:301b2cb34ead 50 // Default construction is I2C mode, address 0x6B.
adam_z 0:301b2cb34ead 51 //
adam_z 0:301b2cb34ead 52
adam_z 0:301b2cb34ead 53 //=================================
adam_z 0:301b2cb34ead 54
adam_z 0:301b2cb34ead 55 //=================================
adam_z 0:301b2cb34ead 56 //****************************************************************************//
adam_z 2:c19d384b2896 57 LSM6DS3Core::LSM6DS3Core( uint8_t busType, uint8_t inputArg) : commInterface(SPI_MODE), I2CAddress(0x6B), spi_(SPI_MOSI,SPI_MISO,SPI_SCK), i2c_(I2C_SDA,I2C_SCL), cs_(SPI_CS)
adam_z 0:301b2cb34ead 58 {
adam_z 0:301b2cb34ead 59 commInterface = busType;
adam_z 0:301b2cb34ead 60 if( commInterface == I2C_MODE ) {
adam_z 0:301b2cb34ead 61 I2CAddress = inputArg;
adam_z 0:301b2cb34ead 62 }
adam_z 0:301b2cb34ead 63 if( commInterface == SPI_MODE ) {
adam_z 0:301b2cb34ead 64 chipSelectPin = inputArg;
adam_z 0:301b2cb34ead 65 }
adam_z 0:301b2cb34ead 66
adam_z 0:301b2cb34ead 67
adam_z 0:301b2cb34ead 68 }
adam_z 0:301b2cb34ead 69
adam_z 0:301b2cb34ead 70 status_t LSM6DS3Core::beginCore(void)
adam_z 0:301b2cb34ead 71 {
adam_z 0:301b2cb34ead 72 status_t returnError = IMU_SUCCESS;
adam_z 0:301b2cb34ead 73
adam_z 0:301b2cb34ead 74 switch (commInterface) {
adam_z 0:301b2cb34ead 75
adam_z 0:301b2cb34ead 76 case I2C_MODE:
adam_z 0:301b2cb34ead 77
adam_z 0:301b2cb34ead 78 break;
adam_z 0:301b2cb34ead 79
adam_z 0:301b2cb34ead 80 case SPI_MODE:
adam_z 0:301b2cb34ead 81 // start the SPI library:
adam_z 0:301b2cb34ead 82
adam_z 1:cc2caaf5536c 83 //DigitalOut cs((PinName)chipSelectPin);
adam_z 0:301b2cb34ead 84 // Maximum SPI frequency is 10MHz, could divide by 2 here:
adam_z 2:c19d384b2896 85 spi_.frequency(5000000);
adam_z 0:301b2cb34ead 86 // Data is read and written MSb first.
adam_z 2:c19d384b2896 87 spi_.format(8,0);
adam_z 0:301b2cb34ead 88 // Data is captured on rising edge of clock (CPHA = 0)
adam_z 0:301b2cb34ead 89 // Base value of the clock is HIGH (CPOL = 1)
adam_z 0:301b2cb34ead 90
adam_z 0:301b2cb34ead 91 /* // MODE3 for 328p operation
adam_z 0:301b2cb34ead 92 #ifdef __AVR__
adam_z 0:301b2cb34ead 93 SPI.setDataMode(SPI_MODE3);
adam_z 0:301b2cb34ead 94 #else
adam_z 0:301b2cb34ead 95 #endif
adam_z 0:301b2cb34ead 96
adam_z 0:301b2cb34ead 97 // MODE0 for Teensy 3.1 operation
adam_z 0:301b2cb34ead 98 #ifdef __MK20DX256__
adam_z 0:301b2cb34ead 99 SPI.setDataMode(SPI_MODE0);
adam_z 0:301b2cb34ead 100 #else
adam_z 0:301b2cb34ead 101 #endif
adam_z 0:301b2cb34ead 102 */
adam_z 0:301b2cb34ead 103 //SPI.format(8,0);//8 bits mode0
adam_z 0:301b2cb34ead 104 // initalize the data ready and chip select pins:
adam_z 0:301b2cb34ead 105
adam_z 2:c19d384b2896 106 cs_ = 1;
adam_z 0:301b2cb34ead 107 break;
adam_z 0:301b2cb34ead 108 default:
adam_z 0:301b2cb34ead 109 break;
adam_z 0:301b2cb34ead 110 }
adam_z 0:301b2cb34ead 111
adam_z 0:301b2cb34ead 112 //Spin for a few ms
adam_z 0:301b2cb34ead 113 volatile uint8_t temp = 0;
adam_z 0:301b2cb34ead 114 for( uint16_t i = 0; i < 10000; i++ ) {
adam_z 0:301b2cb34ead 115 temp++;
adam_z 0:301b2cb34ead 116 }
adam_z 0:301b2cb34ead 117
adam_z 0:301b2cb34ead 118 //Check the ID register to determine if the operation was a success.
adam_z 0:301b2cb34ead 119 uint8_t readCheck;
adam_z 0:301b2cb34ead 120 readRegister(&readCheck, LSM6DS3_ACC_GYRO_WHO_AM_I_REG);
adam_z 0:301b2cb34ead 121 if( readCheck != 0x69 ) {
adam_z 0:301b2cb34ead 122 returnError = IMU_HW_ERROR;
adam_z 0:301b2cb34ead 123 }
adam_z 0:301b2cb34ead 124
adam_z 0:301b2cb34ead 125 return returnError;
adam_z 0:301b2cb34ead 126
adam_z 0:301b2cb34ead 127 }
adam_z 0:301b2cb34ead 128
adam_z 0:301b2cb34ead 129 //****************************************************************************//
adam_z 0:301b2cb34ead 130 //
adam_z 0:301b2cb34ead 131 // ReadRegisterRegion
adam_z 0:301b2cb34ead 132 //
adam_z 0:301b2cb34ead 133 // Parameters:
adam_z 0:301b2cb34ead 134 // *outputPointer -- Pass &variable (base address of) to save read data to
adam_z 0:301b2cb34ead 135 // offset -- register to read
adam_z 0:301b2cb34ead 136 // length -- number of bytes to read
adam_z 0:301b2cb34ead 137 //
adam_z 0:301b2cb34ead 138 // Note: Does not know if the target memory space is an array or not, or
adam_z 0:301b2cb34ead 139 // if there is the array is big enough. if the variable passed is only
adam_z 0:301b2cb34ead 140 // two bytes long and 3 bytes are requested, this will over-write some
adam_z 0:301b2cb34ead 141 // other memory!
adam_z 0:301b2cb34ead 142 //
adam_z 0:301b2cb34ead 143 //****************************************************************************//
adam_z 0:301b2cb34ead 144 status_t LSM6DS3Core::readRegisterRegion(uint8_t *outputPointer , uint8_t offset, uint8_t length)
adam_z 0:301b2cb34ead 145 {
adam_z 0:301b2cb34ead 146 status_t returnError = IMU_SUCCESS;
adam_z 0:301b2cb34ead 147
adam_z 0:301b2cb34ead 148 //define pointer that will point to the external space
adam_z 0:301b2cb34ead 149 uint8_t i = 0;
adam_z 0:301b2cb34ead 150 uint8_t c = 0;
adam_z 0:301b2cb34ead 151 uint8_t tempFFCounter = 0;
adam_z 0:301b2cb34ead 152
adam_z 0:301b2cb34ead 153 switch (commInterface) {
adam_z 0:301b2cb34ead 154
adam_z 0:301b2cb34ead 155 case I2C_MODE:
adam_z 1:cc2caaf5536c 156 /*
adam_z 0:301b2cb34ead 157 i2c.write(offset);
adam_z 0:301b2cb34ead 158 if( i2c.stop() != 0 ) {
adam_z 0:301b2cb34ead 159 returnError = IMU_HW_ERROR;
adam_z 0:301b2cb34ead 160 } else { //OK, all worked, keep going
adam_z 0:301b2cb34ead 161 // request 6 bytes from slave device
adam_z 0:301b2cb34ead 162 Wire.requestFrom(I2CAddress, length);
adam_z 0:301b2cb34ead 163 while ( (Wire.available()) && (i < length)) { // slave may send less than requested
adam_z 0:301b2cb34ead 164 c = Wire.read(); // receive a byte as character
adam_z 0:301b2cb34ead 165 *outputPointer = c;
adam_z 0:301b2cb34ead 166 outputPointer++;
adam_z 0:301b2cb34ead 167 i++;
adam_z 0:301b2cb34ead 168 }
adam_z 0:301b2cb34ead 169 }
adam_z 1:cc2caaf5536c 170 */
adam_z 0:301b2cb34ead 171 break;
adam_z 0:301b2cb34ead 172
adam_z 0:301b2cb34ead 173 case SPI_MODE:
adam_z 0:301b2cb34ead 174 // take the chip select low to select the device:
adam_z 2:c19d384b2896 175 cs_ = 0;
adam_z 0:301b2cb34ead 176 // send the device the register you want to read:
adam_z 2:c19d384b2896 177 spi_.write(offset | 0x80); //Ored with "read request" bit
adam_z 0:301b2cb34ead 178 while ( i < length ) { // slave may send less than requested
adam_z 2:c19d384b2896 179 c = spi_.write(0x00); // receive a byte as character
adam_z 0:301b2cb34ead 180 if( c == 0xFF ) {
adam_z 0:301b2cb34ead 181 //May have problem
adam_z 0:301b2cb34ead 182 tempFFCounter++;
adam_z 0:301b2cb34ead 183 }
adam_z 0:301b2cb34ead 184 *outputPointer = c;
adam_z 0:301b2cb34ead 185 outputPointer++;
adam_z 0:301b2cb34ead 186 i++;
adam_z 0:301b2cb34ead 187 }
adam_z 0:301b2cb34ead 188 if( tempFFCounter == i ) {
adam_z 0:301b2cb34ead 189 //Ok, we've recieved all ones, report
adam_z 0:301b2cb34ead 190 returnError = IMU_ALL_ONES_WARNING;
adam_z 0:301b2cb34ead 191 }
adam_z 0:301b2cb34ead 192 // take the chip select high to de-select:
adam_z 2:c19d384b2896 193 cs_ = 1; //digitalWrite(chipSelectPin, HIGH);
adam_z 0:301b2cb34ead 194 break;
adam_z 0:301b2cb34ead 195
adam_z 0:301b2cb34ead 196 default:
adam_z 0:301b2cb34ead 197 break;
adam_z 0:301b2cb34ead 198 }
adam_z 0:301b2cb34ead 199
adam_z 0:301b2cb34ead 200 return returnError;
adam_z 0:301b2cb34ead 201 }
adam_z 0:301b2cb34ead 202
adam_z 0:301b2cb34ead 203 //****************************************************************************//
adam_z 0:301b2cb34ead 204 //
adam_z 0:301b2cb34ead 205 // ReadRegister
adam_z 0:301b2cb34ead 206 //
adam_z 0:301b2cb34ead 207 // Parameters:
adam_z 0:301b2cb34ead 208 // *outputPointer -- Pass &variable (address of) to save read data to
adam_z 0:301b2cb34ead 209 // offset -- register to read
adam_z 0:301b2cb34ead 210 //
adam_z 0:301b2cb34ead 211 //****************************************************************************//
adam_z 0:301b2cb34ead 212 status_t LSM6DS3Core::readRegister(uint8_t* outputPointer, uint8_t offset)
adam_z 0:301b2cb34ead 213 {
adam_z 0:301b2cb34ead 214 //Return value
adam_z 0:301b2cb34ead 215 uint8_t result;
adam_z 2:c19d384b2896 216 //uint8_t numBytes = 1;
adam_z 0:301b2cb34ead 217 status_t returnError = IMU_SUCCESS;
adam_z 0:301b2cb34ead 218
adam_z 0:301b2cb34ead 219 switch (commInterface) {
adam_z 0:301b2cb34ead 220
adam_z 0:301b2cb34ead 221 case I2C_MODE:
adam_z 1:cc2caaf5536c 222 /* Wire.beginTransmission(I2CAddress);
adam_z 0:301b2cb34ead 223 Wire.write(offset);
adam_z 0:301b2cb34ead 224 if( Wire.endTransmission() != 0 ) {
adam_z 0:301b2cb34ead 225 returnError = IMU_HW_ERROR;
adam_z 0:301b2cb34ead 226 }
adam_z 0:301b2cb34ead 227 Wire.requestFrom(I2CAddress, numBytes);
adam_z 0:301b2cb34ead 228 while ( Wire.available() ) { // slave may send less than requested
adam_z 0:301b2cb34ead 229 result = Wire.read(); // receive a byte as a proper uint8_t
adam_z 0:301b2cb34ead 230 }
adam_z 1:cc2caaf5536c 231 */
adam_z 0:301b2cb34ead 232 break;
adam_z 0:301b2cb34ead 233
adam_z 0:301b2cb34ead 234 case SPI_MODE:
adam_z 0:301b2cb34ead 235 // take the chip select low to select the device:
adam_z 2:c19d384b2896 236 cs_ = 0; //digitalWrite(chipSelectPin, LOW);
adam_z 0:301b2cb34ead 237 // send the device the register you want to read:
adam_z 2:c19d384b2896 238 spi_.write(offset | 0x80); //Ored with "read request" bit
adam_z 0:301b2cb34ead 239 // send a value of 0 to read the first byte returned:
adam_z 2:c19d384b2896 240 result = spi_.write(0x00);
adam_z 0:301b2cb34ead 241 // take the chip select high to de-select:
adam_z 2:c19d384b2896 242 cs_ = 1; //digitalWrite(chipSelectPin, HIGH);
adam_z 0:301b2cb34ead 243
adam_z 0:301b2cb34ead 244 if( result == 0xFF ) {
adam_z 0:301b2cb34ead 245 //we've recieved all ones, report
adam_z 0:301b2cb34ead 246 returnError = IMU_ALL_ONES_WARNING;
adam_z 0:301b2cb34ead 247 }
adam_z 0:301b2cb34ead 248 break;
adam_z 0:301b2cb34ead 249
adam_z 0:301b2cb34ead 250 default:
adam_z 0:301b2cb34ead 251 break;
adam_z 0:301b2cb34ead 252 }
adam_z 0:301b2cb34ead 253
adam_z 0:301b2cb34ead 254 *outputPointer = result;
adam_z 0:301b2cb34ead 255 return returnError;
adam_z 0:301b2cb34ead 256 }
adam_z 0:301b2cb34ead 257
adam_z 0:301b2cb34ead 258 //****************************************************************************//
adam_z 0:301b2cb34ead 259 //
adam_z 0:301b2cb34ead 260 // readRegisterInt16
adam_z 0:301b2cb34ead 261 //
adam_z 0:301b2cb34ead 262 // Parameters:
adam_z 0:301b2cb34ead 263 // *outputPointer -- Pass &variable (base address of) to save read data to
adam_z 0:301b2cb34ead 264 // offset -- register to read
adam_z 0:301b2cb34ead 265 //
adam_z 0:301b2cb34ead 266 //****************************************************************************//
adam_z 0:301b2cb34ead 267 status_t LSM6DS3Core::readRegisterInt16( int16_t* outputPointer, uint8_t offset )
adam_z 0:301b2cb34ead 268 {
adam_z 0:301b2cb34ead 269 uint8_t myBuffer[2];
adam_z 0:301b2cb34ead 270 status_t returnError = readRegisterRegion(myBuffer, offset, 2); //Does memory transfer
adam_z 0:301b2cb34ead 271 int16_t output = (int16_t)myBuffer[0] | int16_t(myBuffer[1] << 8);
adam_z 0:301b2cb34ead 272
adam_z 0:301b2cb34ead 273 *outputPointer = output;
adam_z 0:301b2cb34ead 274 return returnError;
adam_z 0:301b2cb34ead 275 }
adam_z 0:301b2cb34ead 276
adam_z 0:301b2cb34ead 277 //****************************************************************************//
adam_z 0:301b2cb34ead 278 //
adam_z 0:301b2cb34ead 279 // writeRegister
adam_z 0:301b2cb34ead 280 //
adam_z 0:301b2cb34ead 281 // Parameters:
adam_z 0:301b2cb34ead 282 // offset -- register to write
adam_z 0:301b2cb34ead 283 // dataToWrite -- 8 bit data to write to register
adam_z 0:301b2cb34ead 284 //
adam_z 0:301b2cb34ead 285 //****************************************************************************//
adam_z 0:301b2cb34ead 286 status_t LSM6DS3Core::writeRegister(uint8_t offset, uint8_t dataToWrite)
adam_z 0:301b2cb34ead 287 {
adam_z 0:301b2cb34ead 288 status_t returnError = IMU_SUCCESS;
adam_z 0:301b2cb34ead 289 switch (commInterface) {
adam_z 0:301b2cb34ead 290 case I2C_MODE:
adam_z 1:cc2caaf5536c 291 /* //Write the byte
adam_z 0:301b2cb34ead 292 Wire.beginTransmission(I2CAddress);
adam_z 0:301b2cb34ead 293 Wire.write(offset);
adam_z 0:301b2cb34ead 294 Wire.write(dataToWrite);
adam_z 0:301b2cb34ead 295 if( Wire.endTransmission() != 0 ) {
adam_z 0:301b2cb34ead 296 returnError = IMU_HW_ERROR;
adam_z 0:301b2cb34ead 297 }
adam_z 1:cc2caaf5536c 298 */ break;
adam_z 0:301b2cb34ead 299
adam_z 0:301b2cb34ead 300 case SPI_MODE:
adam_z 0:301b2cb34ead 301 // take the chip select low to select the device:
adam_z 2:c19d384b2896 302 cs_ = 0; //digitalWrite(chipSelectPin, LOW);
adam_z 0:301b2cb34ead 303 // send the device the register you want to read:
adam_z 2:c19d384b2896 304 spi_.write(offset);
adam_z 0:301b2cb34ead 305 // send a value of 0 to read the first byte returned:
adam_z 2:c19d384b2896 306 spi_.write(dataToWrite);
adam_z 0:301b2cb34ead 307 // decrement the number of bytes left to read:
adam_z 0:301b2cb34ead 308 // take the chip select high to de-select:
adam_z 2:c19d384b2896 309 cs_ = 1;//digitalWrite(chipSelectPin, HIGH);
adam_z 0:301b2cb34ead 310 break;
adam_z 0:301b2cb34ead 311
adam_z 0:301b2cb34ead 312 //No way to check error on this write (Except to read back but that's not reliable)
adam_z 0:301b2cb34ead 313
adam_z 0:301b2cb34ead 314 default:
adam_z 0:301b2cb34ead 315 break;
adam_z 0:301b2cb34ead 316 }
adam_z 0:301b2cb34ead 317
adam_z 0:301b2cb34ead 318 return returnError;
adam_z 0:301b2cb34ead 319 }
adam_z 0:301b2cb34ead 320
adam_z 0:301b2cb34ead 321 status_t LSM6DS3Core::embeddedPage( void )
adam_z 0:301b2cb34ead 322 {
adam_z 0:301b2cb34ead 323 status_t returnError = writeRegister( LSM6DS3_ACC_GYRO_RAM_ACCESS, 0x80 );
adam_z 0:301b2cb34ead 324
adam_z 0:301b2cb34ead 325 return returnError;
adam_z 0:301b2cb34ead 326 }
adam_z 0:301b2cb34ead 327
adam_z 0:301b2cb34ead 328 status_t LSM6DS3Core::basePage( void )
adam_z 0:301b2cb34ead 329 {
adam_z 0:301b2cb34ead 330 status_t returnError = writeRegister( LSM6DS3_ACC_GYRO_RAM_ACCESS, 0x00 );
adam_z 0:301b2cb34ead 331
adam_z 0:301b2cb34ead 332 return returnError;
adam_z 0:301b2cb34ead 333 }
adam_z 0:301b2cb34ead 334
adam_z 0:301b2cb34ead 335
adam_z 0:301b2cb34ead 336 //****************************************************************************//
adam_z 0:301b2cb34ead 337 //
adam_z 0:301b2cb34ead 338 // Main user class -- wrapper for the core class + maths
adam_z 0:301b2cb34ead 339 //
adam_z 0:301b2cb34ead 340 // Construct with same rules as the core ( uint8_t busType, uint8_t inputArg )
adam_z 0:301b2cb34ead 341 //
adam_z 0:301b2cb34ead 342 //****************************************************************************//
adam_z 0:301b2cb34ead 343 LSM6DS3::LSM6DS3( uint8_t busType, uint8_t inputArg ) : LSM6DS3Core( busType, inputArg )
adam_z 0:301b2cb34ead 344 {
adam_z 0:301b2cb34ead 345 //Construct with these default settings
adam_z 0:301b2cb34ead 346
adam_z 0:301b2cb34ead 347 settings.gyroEnabled = 1; //Can be 0 or 1
adam_z 0:301b2cb34ead 348 settings.gyroRange = 2000; //Max deg/s. Can be: 125, 245, 500, 1000, 2000
adam_z 0:301b2cb34ead 349 settings.gyroSampleRate = 833; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666
adam_z 0:301b2cb34ead 350 settings.gyroBandWidth = 400; //Hz. Can be: 50, 100, 200, 400;
adam_z 0:301b2cb34ead 351 settings.gyroFifoEnabled = 0; //Set to include gyro in FIFO
adam_z 0:301b2cb34ead 352 settings.gyroFifoDecimation = 1; //set 1 for on /1
adam_z 0:301b2cb34ead 353
adam_z 0:301b2cb34ead 354 settings.accelEnabled = 1;
adam_z 0:301b2cb34ead 355 settings.accelODROff = 1;
adam_z 0:301b2cb34ead 356 settings.accelRange = 8; //Max G force readable. Can be: 2, 4, 8, 16
adam_z 0:301b2cb34ead 357 settings.accelSampleRate = 833; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 13330
adam_z 0:301b2cb34ead 358 settings.accelBandWidth = 400; //Hz. Can be: 50, 100, 200, 400;
adam_z 0:301b2cb34ead 359 settings.accelFifoEnabled = 0; //Set to include accelerometer in the FIFO
adam_z 0:301b2cb34ead 360 settings.accelFifoDecimation = 1; //set 1 for on /1
adam_z 0:301b2cb34ead 361
adam_z 0:301b2cb34ead 362 settings.tempEnabled = 1;
adam_z 0:301b2cb34ead 363
adam_z 0:301b2cb34ead 364 //Select interface mode
adam_z 0:301b2cb34ead 365 settings.commMode = 1; //Can be modes 1, 2 or 3
adam_z 0:301b2cb34ead 366
adam_z 0:301b2cb34ead 367 //FIFO control data
adam_z 0:301b2cb34ead 368 settings.fifoThreshold = 3000; //Can be 0 to 4096 (16 bit bytes)
adam_z 0:301b2cb34ead 369 settings.fifoSampleRate = 10; //default 10Hz
adam_z 0:301b2cb34ead 370 settings.fifoModeWord = 0; //Default off
adam_z 0:301b2cb34ead 371
adam_z 0:301b2cb34ead 372 allOnesCounter = 0;
adam_z 0:301b2cb34ead 373 nonSuccessCounter = 0;
adam_z 0:301b2cb34ead 374
adam_z 0:301b2cb34ead 375 }
adam_z 0:301b2cb34ead 376
adam_z 0:301b2cb34ead 377 //****************************************************************************//
adam_z 0:301b2cb34ead 378 //
adam_z 0:301b2cb34ead 379 // Configuration section
adam_z 0:301b2cb34ead 380 //
adam_z 0:301b2cb34ead 381 // This uses the stored SensorSettings to start the IMU
adam_z 0:301b2cb34ead 382 // Use statements such as "myIMU.settings.commInterface = SPI_MODE;" or
adam_z 0:301b2cb34ead 383 // "myIMU.settings.accelEnabled = 1;" to configure before calling .begin();
adam_z 0:301b2cb34ead 384 //
adam_z 0:301b2cb34ead 385 //****************************************************************************//
adam_z 0:301b2cb34ead 386 status_t LSM6DS3::begin()
adam_z 0:301b2cb34ead 387 {
adam_z 0:301b2cb34ead 388 //Check the settings structure values to determine how to setup the device
adam_z 0:301b2cb34ead 389 uint8_t dataToWrite = 0; //Temporary variable
adam_z 0:301b2cb34ead 390
adam_z 0:301b2cb34ead 391 //Begin the inherited core. This gets the physical wires connected
adam_z 0:301b2cb34ead 392 status_t returnError = beginCore();
adam_z 0:301b2cb34ead 393
adam_z 0:301b2cb34ead 394 //setOffset(-61, -25, -66, 35, -81, -32);
adam_z 0:301b2cb34ead 395
adam_z 0:301b2cb34ead 396 //Setup the accelerometer******************************
adam_z 0:301b2cb34ead 397 dataToWrite = 0; //Start Fresh!
adam_z 0:301b2cb34ead 398 if ( settings.accelEnabled == 1) {
adam_z 0:301b2cb34ead 399 //Build config reg
adam_z 0:301b2cb34ead 400 //First patch in filter bandwidth
adam_z 0:301b2cb34ead 401 switch (settings.accelBandWidth) {
adam_z 0:301b2cb34ead 402 case 50:
adam_z 0:301b2cb34ead 403 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_50Hz;
adam_z 0:301b2cb34ead 404 break;
adam_z 0:301b2cb34ead 405 case 100:
adam_z 0:301b2cb34ead 406 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_100Hz;
adam_z 0:301b2cb34ead 407 break;
adam_z 0:301b2cb34ead 408 case 200:
adam_z 0:301b2cb34ead 409 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_200Hz;
adam_z 0:301b2cb34ead 410 break;
adam_z 0:301b2cb34ead 411 default: //set default case to max passthrough
adam_z 0:301b2cb34ead 412 case 400:
adam_z 0:301b2cb34ead 413 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_400Hz;
adam_z 0:301b2cb34ead 414 break;
adam_z 0:301b2cb34ead 415 }
adam_z 0:301b2cb34ead 416 //Next, patch in full scale
adam_z 0:301b2cb34ead 417 switch (settings.accelRange) {
adam_z 0:301b2cb34ead 418 case 2:
adam_z 0:301b2cb34ead 419 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_2g;
adam_z 0:301b2cb34ead 420 break;
adam_z 0:301b2cb34ead 421 case 4:
adam_z 0:301b2cb34ead 422 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_4g;
adam_z 0:301b2cb34ead 423 break;
adam_z 0:301b2cb34ead 424 case 8:
adam_z 0:301b2cb34ead 425 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_8g;
adam_z 0:301b2cb34ead 426 break;
adam_z 0:301b2cb34ead 427 default: //set default case to 16(max)
adam_z 0:301b2cb34ead 428 case 16:
adam_z 0:301b2cb34ead 429 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_16g;
adam_z 0:301b2cb34ead 430 break;
adam_z 0:301b2cb34ead 431 }
adam_z 0:301b2cb34ead 432 //Lastly, patch in accelerometer ODR
adam_z 0:301b2cb34ead 433 switch (settings.accelSampleRate) {
adam_z 0:301b2cb34ead 434 case 13:
adam_z 0:301b2cb34ead 435 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_13Hz;
adam_z 0:301b2cb34ead 436 break;
adam_z 0:301b2cb34ead 437 case 26:
adam_z 0:301b2cb34ead 438 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_26Hz;
adam_z 0:301b2cb34ead 439 break;
adam_z 0:301b2cb34ead 440 case 52:
adam_z 0:301b2cb34ead 441 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_52Hz;
adam_z 0:301b2cb34ead 442 break;
adam_z 0:301b2cb34ead 443 default: //Set default to 104
adam_z 0:301b2cb34ead 444 case 104:
adam_z 0:301b2cb34ead 445 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_104Hz;
adam_z 0:301b2cb34ead 446 break;
adam_z 0:301b2cb34ead 447 case 208:
adam_z 0:301b2cb34ead 448 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_208Hz;
adam_z 0:301b2cb34ead 449 break;
adam_z 0:301b2cb34ead 450 case 416:
adam_z 0:301b2cb34ead 451 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_416Hz;
adam_z 0:301b2cb34ead 452 break;
adam_z 0:301b2cb34ead 453 case 833:
adam_z 0:301b2cb34ead 454 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_833Hz;
adam_z 0:301b2cb34ead 455 break;
adam_z 0:301b2cb34ead 456 case 1660:
adam_z 0:301b2cb34ead 457 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_1660Hz;
adam_z 0:301b2cb34ead 458 break;
adam_z 0:301b2cb34ead 459 case 3330:
adam_z 0:301b2cb34ead 460 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_3330Hz;
adam_z 0:301b2cb34ead 461 break;
adam_z 0:301b2cb34ead 462 case 6660:
adam_z 0:301b2cb34ead 463 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_6660Hz;
adam_z 0:301b2cb34ead 464 break;
adam_z 0:301b2cb34ead 465 case 13330:
adam_z 0:301b2cb34ead 466 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_13330Hz;
adam_z 0:301b2cb34ead 467 break;
adam_z 0:301b2cb34ead 468 }
adam_z 0:301b2cb34ead 469 } else {
adam_z 0:301b2cb34ead 470 //dataToWrite already = 0 (powerdown);
adam_z 0:301b2cb34ead 471 }
adam_z 0:301b2cb34ead 472
adam_z 0:301b2cb34ead 473 //Now, write the patched together data
adam_z 0:301b2cb34ead 474 writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, dataToWrite);
adam_z 0:301b2cb34ead 475
adam_z 0:301b2cb34ead 476 //Set the ODR bit
adam_z 0:301b2cb34ead 477 readRegister(&dataToWrite, LSM6DS3_ACC_GYRO_CTRL4_C);
adam_z 0:301b2cb34ead 478 dataToWrite &= ~((uint8_t)LSM6DS3_ACC_GYRO_BW_SCAL_ODR_ENABLED);
adam_z 0:301b2cb34ead 479 if ( settings.accelODROff == 1) {
adam_z 0:301b2cb34ead 480 dataToWrite |= LSM6DS3_ACC_GYRO_BW_SCAL_ODR_ENABLED;
adam_z 0:301b2cb34ead 481 }
adam_z 0:301b2cb34ead 482 writeRegister(LSM6DS3_ACC_GYRO_CTRL4_C, dataToWrite);
adam_z 0:301b2cb34ead 483
adam_z 0:301b2cb34ead 484 //Setup the gyroscope**********************************************
adam_z 0:301b2cb34ead 485 dataToWrite = 0; //Start Fresh!
adam_z 0:301b2cb34ead 486 if ( settings.gyroEnabled == 1) {
adam_z 0:301b2cb34ead 487 //Build config reg
adam_z 0:301b2cb34ead 488 //First, patch in full scale
adam_z 0:301b2cb34ead 489 switch (settings.gyroRange) {
adam_z 0:301b2cb34ead 490 case 125:
adam_z 0:301b2cb34ead 491 dataToWrite |= LSM6DS3_ACC_GYRO_FS_125_ENABLED;
adam_z 0:301b2cb34ead 492 break;
adam_z 0:301b2cb34ead 493 case 245:
adam_z 0:301b2cb34ead 494 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_245dps;
adam_z 0:301b2cb34ead 495 break;
adam_z 0:301b2cb34ead 496 case 500:
adam_z 0:301b2cb34ead 497 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_500dps;
adam_z 0:301b2cb34ead 498 break;
adam_z 0:301b2cb34ead 499 case 1000:
adam_z 0:301b2cb34ead 500 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_1000dps;
adam_z 0:301b2cb34ead 501 break;
adam_z 0:301b2cb34ead 502 default: //Default to full 2000DPS range
adam_z 0:301b2cb34ead 503 case 2000:
adam_z 0:301b2cb34ead 504 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_2000dps;
adam_z 0:301b2cb34ead 505 break;
adam_z 0:301b2cb34ead 506 }
adam_z 0:301b2cb34ead 507 //Lastly, patch in gyro ODR
adam_z 0:301b2cb34ead 508 switch (settings.gyroSampleRate) {
adam_z 0:301b2cb34ead 509 case 13:
adam_z 0:301b2cb34ead 510 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_13Hz;
adam_z 0:301b2cb34ead 511 break;
adam_z 0:301b2cb34ead 512 case 26:
adam_z 0:301b2cb34ead 513 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_26Hz;
adam_z 0:301b2cb34ead 514 break;
adam_z 0:301b2cb34ead 515 case 52:
adam_z 0:301b2cb34ead 516 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_52Hz;
adam_z 0:301b2cb34ead 517 break;
adam_z 0:301b2cb34ead 518 default: //Set default to 104
adam_z 0:301b2cb34ead 519 case 104:
adam_z 0:301b2cb34ead 520 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_104Hz;
adam_z 0:301b2cb34ead 521 break;
adam_z 0:301b2cb34ead 522 case 208:
adam_z 0:301b2cb34ead 523 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_208Hz;
adam_z 0:301b2cb34ead 524 break;
adam_z 0:301b2cb34ead 525 case 416:
adam_z 0:301b2cb34ead 526 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_416Hz;
adam_z 0:301b2cb34ead 527 break;
adam_z 0:301b2cb34ead 528 case 833:
adam_z 0:301b2cb34ead 529 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_833Hz;
adam_z 0:301b2cb34ead 530 break;
adam_z 0:301b2cb34ead 531 case 1660:
adam_z 0:301b2cb34ead 532 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_1660Hz;
adam_z 0:301b2cb34ead 533 break;
adam_z 0:301b2cb34ead 534 }
adam_z 0:301b2cb34ead 535 } else {
adam_z 0:301b2cb34ead 536 //dataToWrite already = 0 (powerdown);
adam_z 0:301b2cb34ead 537 }
adam_z 0:301b2cb34ead 538 //Write the byte
adam_z 0:301b2cb34ead 539 writeRegister(LSM6DS3_ACC_GYRO_CTRL2_G, dataToWrite);
adam_z 0:301b2cb34ead 540
adam_z 0:301b2cb34ead 541 //Setup the internal temperature sensor
adam_z 0:301b2cb34ead 542 if ( settings.tempEnabled == 1) {
adam_z 0:301b2cb34ead 543 }
adam_z 0:301b2cb34ead 544
adam_z 0:301b2cb34ead 545 //Return WHO AM I reg //Not no mo!
adam_z 0:301b2cb34ead 546 uint8_t result;
adam_z 0:301b2cb34ead 547 readRegister(&result, LSM6DS3_ACC_GYRO_WHO_AM_I_REG);
adam_z 0:301b2cb34ead 548
adam_z 0:301b2cb34ead 549 return returnError;
adam_z 0:301b2cb34ead 550 }
adam_z 0:301b2cb34ead 551
adam_z 0:301b2cb34ead 552 //****************************************************************************//
adam_z 0:301b2cb34ead 553 //
adam_z 0:301b2cb34ead 554 // Accelerometer section
adam_z 0:301b2cb34ead 555 //
adam_z 0:301b2cb34ead 556 //****************************************************************************//
adam_z 0:301b2cb34ead 557
adam_z 0:301b2cb34ead 558 void LSM6DS3::setOffset(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz)
adam_z 0:301b2cb34ead 559 {
adam_z 0:301b2cb34ead 560 accelOffset[0] = ax;
adam_z 0:301b2cb34ead 561 accelOffset[1] = ay;
adam_z 0:301b2cb34ead 562 accelOffset[2] = az;
adam_z 0:301b2cb34ead 563 gyroOffset[0] = gx;
adam_z 0:301b2cb34ead 564 gyroOffset[1] = gy;
adam_z 0:301b2cb34ead 565 gyroOffset[2] = gz;
adam_z 0:301b2cb34ead 566 }
adam_z 0:301b2cb34ead 567
adam_z 0:301b2cb34ead 568 int16_t LSM6DS3::readRawAccelX( void )
adam_z 0:301b2cb34ead 569 {
adam_z 0:301b2cb34ead 570 int16_t output;
adam_z 0:301b2cb34ead 571 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTX_L_XL );
adam_z 0:301b2cb34ead 572 if( errorLevel != IMU_SUCCESS ) {
adam_z 0:301b2cb34ead 573 if( errorLevel == IMU_ALL_ONES_WARNING ) {
adam_z 0:301b2cb34ead 574 allOnesCounter++;
adam_z 0:301b2cb34ead 575 } else {
adam_z 0:301b2cb34ead 576 nonSuccessCounter++;
adam_z 0:301b2cb34ead 577 }
adam_z 0:301b2cb34ead 578 }
adam_z 0:301b2cb34ead 579 return output;
adam_z 0:301b2cb34ead 580 }
adam_z 0:301b2cb34ead 581 float LSM6DS3::readFloatAccelX( void )
adam_z 0:301b2cb34ead 582 {
adam_z 0:301b2cb34ead 583 float output = calcAccel((int32_t)readRawAccelX() - (int32_t)accelOffset[0]);
adam_z 0:301b2cb34ead 584 return output;
adam_z 0:301b2cb34ead 585 }
adam_z 0:301b2cb34ead 586
adam_z 0:301b2cb34ead 587 int16_t LSM6DS3::readRawAccelY( void )
adam_z 0:301b2cb34ead 588 {
adam_z 0:301b2cb34ead 589 int16_t output;
adam_z 0:301b2cb34ead 590 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTY_L_XL );
adam_z 0:301b2cb34ead 591 if( errorLevel != IMU_SUCCESS ) {
adam_z 0:301b2cb34ead 592 if( errorLevel == IMU_ALL_ONES_WARNING ) {
adam_z 0:301b2cb34ead 593 allOnesCounter++;
adam_z 0:301b2cb34ead 594 } else {
adam_z 0:301b2cb34ead 595 nonSuccessCounter++;
adam_z 0:301b2cb34ead 596 }
adam_z 0:301b2cb34ead 597 }
adam_z 0:301b2cb34ead 598 return output;
adam_z 0:301b2cb34ead 599 }
adam_z 0:301b2cb34ead 600 float LSM6DS3::readFloatAccelY( void )
adam_z 0:301b2cb34ead 601 {
adam_z 0:301b2cb34ead 602 float output = calcAccel((int32_t)readRawAccelY() - (int32_t)accelOffset[1]);
adam_z 0:301b2cb34ead 603 return output;
adam_z 0:301b2cb34ead 604 }
adam_z 0:301b2cb34ead 605
adam_z 0:301b2cb34ead 606 int16_t LSM6DS3::readRawAccelZ( void )
adam_z 0:301b2cb34ead 607 {
adam_z 0:301b2cb34ead 608 int16_t output;
adam_z 0:301b2cb34ead 609 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTZ_L_XL );
adam_z 0:301b2cb34ead 610 if( errorLevel != IMU_SUCCESS ) {
adam_z 0:301b2cb34ead 611 if( errorLevel == IMU_ALL_ONES_WARNING ) {
adam_z 0:301b2cb34ead 612 allOnesCounter++;
adam_z 0:301b2cb34ead 613 } else {
adam_z 0:301b2cb34ead 614 nonSuccessCounter++;
adam_z 0:301b2cb34ead 615 }
adam_z 0:301b2cb34ead 616 }
adam_z 0:301b2cb34ead 617 return output;
adam_z 0:301b2cb34ead 618 }
adam_z 0:301b2cb34ead 619 float LSM6DS3::readFloatAccelZ( void )
adam_z 0:301b2cb34ead 620 {
adam_z 0:301b2cb34ead 621 float output = calcAccel((int32_t)readRawAccelZ() - (int32_t)accelOffset[2]);
adam_z 0:301b2cb34ead 622 return output;
adam_z 0:301b2cb34ead 623 }
adam_z 0:301b2cb34ead 624
adam_z 0:301b2cb34ead 625 float LSM6DS3::calcAccel( int32_t input )
adam_z 0:301b2cb34ead 626 {
adam_z 0:301b2cb34ead 627 float output = (float)input * 0.061 * (settings.accelRange >> 1) / 1000;
adam_z 0:301b2cb34ead 628 return output;
adam_z 0:301b2cb34ead 629 }
adam_z 0:301b2cb34ead 630
adam_z 0:301b2cb34ead 631 //****************************************************************************//
adam_z 0:301b2cb34ead 632 //
adam_z 0:301b2cb34ead 633 // Gyroscope section
adam_z 0:301b2cb34ead 634 //
adam_z 0:301b2cb34ead 635 //****************************************************************************//
adam_z 0:301b2cb34ead 636 int16_t LSM6DS3::readRawGyroX( void )
adam_z 0:301b2cb34ead 637 {
adam_z 0:301b2cb34ead 638 int16_t output;
adam_z 0:301b2cb34ead 639 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTX_L_G );
adam_z 0:301b2cb34ead 640 if( errorLevel != IMU_SUCCESS ) {
adam_z 0:301b2cb34ead 641 if( errorLevel == IMU_ALL_ONES_WARNING ) {
adam_z 0:301b2cb34ead 642 allOnesCounter++;
adam_z 0:301b2cb34ead 643 } else {
adam_z 0:301b2cb34ead 644 nonSuccessCounter++;
adam_z 0:301b2cb34ead 645 }
adam_z 0:301b2cb34ead 646 }
adam_z 0:301b2cb34ead 647 return output;
adam_z 0:301b2cb34ead 648 }
adam_z 0:301b2cb34ead 649 float LSM6DS3::readFloatGyroX( void )
adam_z 0:301b2cb34ead 650 {
adam_z 0:301b2cb34ead 651 float output = calcGyro((int32_t)readRawGyroX() - (int32_t)gyroOffset[0]);
adam_z 0:301b2cb34ead 652 return output;
adam_z 0:301b2cb34ead 653 }
adam_z 0:301b2cb34ead 654
adam_z 0:301b2cb34ead 655 int16_t LSM6DS3::readRawGyroY( void )
adam_z 0:301b2cb34ead 656 {
adam_z 0:301b2cb34ead 657 int16_t output;
adam_z 0:301b2cb34ead 658 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTY_L_G );
adam_z 0:301b2cb34ead 659 if( errorLevel != IMU_SUCCESS ) {
adam_z 0:301b2cb34ead 660 if( errorLevel == IMU_ALL_ONES_WARNING ) {
adam_z 0:301b2cb34ead 661 allOnesCounter++;
adam_z 0:301b2cb34ead 662 } else {
adam_z 0:301b2cb34ead 663 nonSuccessCounter++;
adam_z 0:301b2cb34ead 664 }
adam_z 0:301b2cb34ead 665 }
adam_z 0:301b2cb34ead 666 return output;
adam_z 0:301b2cb34ead 667 }
adam_z 0:301b2cb34ead 668 float LSM6DS3::readFloatGyroY( void )
adam_z 0:301b2cb34ead 669 {
adam_z 0:301b2cb34ead 670 float output = calcGyro((int32_t)readRawGyroY() - (int32_t)gyroOffset[1]);
adam_z 0:301b2cb34ead 671 return output;
adam_z 0:301b2cb34ead 672 }
adam_z 0:301b2cb34ead 673
adam_z 0:301b2cb34ead 674 int16_t LSM6DS3::readRawGyroZ( void )
adam_z 0:301b2cb34ead 675 {
adam_z 0:301b2cb34ead 676 int16_t output;
adam_z 0:301b2cb34ead 677 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTZ_L_G );
adam_z 0:301b2cb34ead 678 if( errorLevel != IMU_SUCCESS ) {
adam_z 0:301b2cb34ead 679 if( errorLevel == IMU_ALL_ONES_WARNING ) {
adam_z 0:301b2cb34ead 680 allOnesCounter++;
adam_z 0:301b2cb34ead 681 } else {
adam_z 0:301b2cb34ead 682 nonSuccessCounter++;
adam_z 0:301b2cb34ead 683 }
adam_z 0:301b2cb34ead 684 }
adam_z 0:301b2cb34ead 685 return output;
adam_z 0:301b2cb34ead 686 }
adam_z 0:301b2cb34ead 687 float LSM6DS3::readFloatGyroZ( void )
adam_z 0:301b2cb34ead 688 {
adam_z 0:301b2cb34ead 689 float output = calcGyro((int32_t)readRawGyroZ() - (int32_t)gyroOffset[2]);
adam_z 0:301b2cb34ead 690 return output;
adam_z 0:301b2cb34ead 691 }
adam_z 0:301b2cb34ead 692
adam_z 0:301b2cb34ead 693 float LSM6DS3::calcGyro( int32_t input )
adam_z 0:301b2cb34ead 694 {
adam_z 0:301b2cb34ead 695 uint8_t gyroRangeDivisor = settings.gyroRange / 125;
adam_z 0:301b2cb34ead 696 if ( settings.gyroRange == 245 ) {
adam_z 0:301b2cb34ead 697 gyroRangeDivisor = 2;
adam_z 0:301b2cb34ead 698 }
adam_z 0:301b2cb34ead 699
adam_z 0:301b2cb34ead 700 float output = (float)input * 4.375 * (gyroRangeDivisor) / 1000;
adam_z 0:301b2cb34ead 701 return output;
adam_z 0:301b2cb34ead 702 }
adam_z 0:301b2cb34ead 703
adam_z 0:301b2cb34ead 704 //****************************************************************************//
adam_z 0:301b2cb34ead 705 //
adam_z 0:301b2cb34ead 706 // Temperature section
adam_z 0:301b2cb34ead 707 //
adam_z 0:301b2cb34ead 708 //****************************************************************************//
adam_z 0:301b2cb34ead 709 int16_t LSM6DS3::readRawTemp( void )
adam_z 0:301b2cb34ead 710 {
adam_z 0:301b2cb34ead 711 int16_t output;
adam_z 0:301b2cb34ead 712 readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUT_TEMP_L );
adam_z 0:301b2cb34ead 713 return output;
adam_z 0:301b2cb34ead 714 }
adam_z 0:301b2cb34ead 715
adam_z 0:301b2cb34ead 716 float LSM6DS3::readTempC( void )
adam_z 0:301b2cb34ead 717 {
adam_z 0:301b2cb34ead 718 float output = (float)readRawTemp() / 16; //divide by 16 to scale
adam_z 0:301b2cb34ead 719 output += 25; //Add 25 degrees to remove offset
adam_z 0:301b2cb34ead 720
adam_z 0:301b2cb34ead 721 return output;
adam_z 0:301b2cb34ead 722
adam_z 0:301b2cb34ead 723 }
adam_z 0:301b2cb34ead 724
adam_z 0:301b2cb34ead 725 float LSM6DS3::readTempF( void )
adam_z 0:301b2cb34ead 726 {
adam_z 0:301b2cb34ead 727 float output = (float)readRawTemp() / 16; //divide by 16 to scale
adam_z 0:301b2cb34ead 728 output += 25; //Add 25 degrees to remove offset
adam_z 0:301b2cb34ead 729 output = (output * 9) / 5 + 32;
adam_z 0:301b2cb34ead 730
adam_z 0:301b2cb34ead 731 return output;
adam_z 0:301b2cb34ead 732
adam_z 0:301b2cb34ead 733 }
adam_z 0:301b2cb34ead 734
adam_z 0:301b2cb34ead 735 //****************************************************************************//
adam_z 0:301b2cb34ead 736 //
adam_z 0:301b2cb34ead 737 // FIFO section
adam_z 0:301b2cb34ead 738 //
adam_z 0:301b2cb34ead 739 //****************************************************************************//
adam_z 0:301b2cb34ead 740 void LSM6DS3::fifoBegin( void )
adam_z 0:301b2cb34ead 741 {
adam_z 0:301b2cb34ead 742 //CONFIGURE THE VARIOUS FIFO SETTINGS
adam_z 0:301b2cb34ead 743 //
adam_z 0:301b2cb34ead 744 //
adam_z 0:301b2cb34ead 745 //This section first builds a bunch of config words, then goes through
adam_z 0:301b2cb34ead 746 //and writes them all.
adam_z 0:301b2cb34ead 747
adam_z 0:301b2cb34ead 748 //Split and mask the threshold
adam_z 0:301b2cb34ead 749 uint8_t thresholdLByte = settings.fifoThreshold & 0x00FF;
adam_z 0:301b2cb34ead 750 uint8_t thresholdHByte = (settings.fifoThreshold & 0x0F00) >> 8;
adam_z 0:301b2cb34ead 751 //Pedo bits not configured (ctl2)
adam_z 0:301b2cb34ead 752
adam_z 0:301b2cb34ead 753 //CONFIGURE FIFO_CTRL3
adam_z 0:301b2cb34ead 754 uint8_t tempFIFO_CTRL3 = 0;
adam_z 0:301b2cb34ead 755 if (settings.gyroFifoEnabled == 1) {
adam_z 0:301b2cb34ead 756 //Set up gyro stuff
adam_z 0:301b2cb34ead 757 //Build on FIFO_CTRL3
adam_z 0:301b2cb34ead 758 //Set decimation
adam_z 0:301b2cb34ead 759 tempFIFO_CTRL3 |= (settings.gyroFifoDecimation & 0x07) << 3;
adam_z 0:301b2cb34ead 760
adam_z 0:301b2cb34ead 761 }
adam_z 0:301b2cb34ead 762 if (settings.accelFifoEnabled == 1) {
adam_z 0:301b2cb34ead 763 //Set up accelerometer stuff
adam_z 0:301b2cb34ead 764 //Build on FIFO_CTRL3
adam_z 0:301b2cb34ead 765 //Set decimation
adam_z 0:301b2cb34ead 766 tempFIFO_CTRL3 |= (settings.accelFifoDecimation & 0x07);
adam_z 0:301b2cb34ead 767 }
adam_z 0:301b2cb34ead 768
adam_z 0:301b2cb34ead 769 //CONFIGURE FIFO_CTRL4 (nothing for now-- sets data sets 3 and 4
adam_z 0:301b2cb34ead 770 uint8_t tempFIFO_CTRL4 = 0;
adam_z 0:301b2cb34ead 771
adam_z 0:301b2cb34ead 772
adam_z 0:301b2cb34ead 773 //CONFIGURE FIFO_CTRL5
adam_z 0:301b2cb34ead 774 uint8_t tempFIFO_CTRL5 = 0;
adam_z 0:301b2cb34ead 775 switch (settings.fifoSampleRate) {
adam_z 0:301b2cb34ead 776 default: //set default case to 10Hz(slowest)
adam_z 0:301b2cb34ead 777 case 10:
adam_z 0:301b2cb34ead 778 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_10Hz;
adam_z 0:301b2cb34ead 779 break;
adam_z 0:301b2cb34ead 780 case 25:
adam_z 0:301b2cb34ead 781 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_25Hz;
adam_z 0:301b2cb34ead 782 break;
adam_z 0:301b2cb34ead 783 case 50:
adam_z 0:301b2cb34ead 784 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_50Hz;
adam_z 0:301b2cb34ead 785 break;
adam_z 0:301b2cb34ead 786 case 100:
adam_z 0:301b2cb34ead 787 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_100Hz;
adam_z 0:301b2cb34ead 788 break;
adam_z 0:301b2cb34ead 789 case 200:
adam_z 0:301b2cb34ead 790 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_200Hz;
adam_z 0:301b2cb34ead 791 break;
adam_z 0:301b2cb34ead 792 case 400:
adam_z 0:301b2cb34ead 793 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_400Hz;
adam_z 0:301b2cb34ead 794 break;
adam_z 0:301b2cb34ead 795 case 800:
adam_z 0:301b2cb34ead 796 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_800Hz;
adam_z 0:301b2cb34ead 797 break;
adam_z 0:301b2cb34ead 798 case 1600:
adam_z 0:301b2cb34ead 799 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_1600Hz;
adam_z 0:301b2cb34ead 800 break;
adam_z 0:301b2cb34ead 801 case 3300:
adam_z 0:301b2cb34ead 802 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_3300Hz;
adam_z 0:301b2cb34ead 803 break;
adam_z 0:301b2cb34ead 804 case 6600:
adam_z 0:301b2cb34ead 805 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_6600Hz;
adam_z 0:301b2cb34ead 806 break;
adam_z 0:301b2cb34ead 807 }
adam_z 0:301b2cb34ead 808 //Hard code the fifo mode here:
adam_z 0:301b2cb34ead 809 tempFIFO_CTRL5 |= settings.fifoModeWord = 6; //set mode:
adam_z 0:301b2cb34ead 810
adam_z 0:301b2cb34ead 811 //Write the data
adam_z 0:301b2cb34ead 812 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL1, thresholdLByte);
adam_z 0:301b2cb34ead 813 //Serial.println(thresholdLByte, HEX);
adam_z 0:301b2cb34ead 814 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL2, thresholdHByte);
adam_z 0:301b2cb34ead 815 //Serial.println(thresholdHByte, HEX);
adam_z 0:301b2cb34ead 816 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL3, tempFIFO_CTRL3);
adam_z 0:301b2cb34ead 817 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL4, tempFIFO_CTRL4);
adam_z 0:301b2cb34ead 818 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL5, tempFIFO_CTRL5);
adam_z 0:301b2cb34ead 819
adam_z 0:301b2cb34ead 820 }
adam_z 0:301b2cb34ead 821 void LSM6DS3::fifoClear( void )
adam_z 0:301b2cb34ead 822 {
adam_z 0:301b2cb34ead 823 //Drain the fifo data and dump it
adam_z 0:301b2cb34ead 824 while( (fifoGetStatus() & 0x1000 ) == 0 ) {
adam_z 0:301b2cb34ead 825 fifoRead();
adam_z 0:301b2cb34ead 826 }
adam_z 0:301b2cb34ead 827
adam_z 0:301b2cb34ead 828 }
adam_z 0:301b2cb34ead 829 int16_t LSM6DS3::fifoRead( void )
adam_z 0:301b2cb34ead 830 {
adam_z 0:301b2cb34ead 831 //Pull the last data from the fifo
adam_z 0:301b2cb34ead 832 uint8_t tempReadByte = 0;
adam_z 0:301b2cb34ead 833 uint16_t tempAccumulator = 0;
adam_z 0:301b2cb34ead 834 readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_DATA_OUT_L);
adam_z 0:301b2cb34ead 835 tempAccumulator = tempReadByte;
adam_z 0:301b2cb34ead 836 readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_DATA_OUT_H);
adam_z 0:301b2cb34ead 837 tempAccumulator |= ((uint16_t)tempReadByte << 8);
adam_z 0:301b2cb34ead 838
adam_z 0:301b2cb34ead 839 return tempAccumulator;
adam_z 0:301b2cb34ead 840 }
adam_z 0:301b2cb34ead 841
adam_z 0:301b2cb34ead 842 uint16_t LSM6DS3::fifoGetStatus( void )
adam_z 0:301b2cb34ead 843 {
adam_z 0:301b2cb34ead 844 //Return some data on the state of the fifo
adam_z 0:301b2cb34ead 845 uint8_t tempReadByte = 0;
adam_z 0:301b2cb34ead 846 uint16_t tempAccumulator = 0;
adam_z 0:301b2cb34ead 847 readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_STATUS1);
adam_z 0:301b2cb34ead 848 tempAccumulator = tempReadByte;
adam_z 0:301b2cb34ead 849 readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_STATUS2);
adam_z 0:301b2cb34ead 850 tempAccumulator |= (tempReadByte << 8);
adam_z 0:301b2cb34ead 851
adam_z 0:301b2cb34ead 852 return tempAccumulator;
adam_z 0:301b2cb34ead 853
adam_z 0:301b2cb34ead 854 }
adam_z 0:301b2cb34ead 855 void LSM6DS3::fifoEnd( void )
adam_z 0:301b2cb34ead 856 {
adam_z 0:301b2cb34ead 857 // turn off the fifo
adam_z 0:301b2cb34ead 858 writeRegister(LSM6DS3_ACC_GYRO_FIFO_STATUS1, 0x00); //Disable
adam_z 0:301b2cb34ead 859 }