LDSC_Robotics_TAs / LSM6DS3_20160225

Pin usage: MOSI(D4), MISO(D5), SCK(D3), CS(D6), format(8, 3) Test OK.

Fork of LSM6DS3 by LDSC_Robotics_TAs

LSM6DS3.cpp@0:301b2cb34ead, 2016-02-19 (annotated)

Committer:
adam_z
Date:
Fri Feb 19 09:50:40 2016 +0000
Revision:
0:301b2cb34ead
Child:
1:cc2caaf5536c
compiled unsuccessful

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 0:301b2cb34ead 35 I2C i2c(D14,D15);
adam_z 0:301b2cb34ead 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 0:301b2cb34ead 57 LSM6DS3Core::LSM6DS3Core( uint8_t busType, uint8_t inputArg) : commInterface(SPI_MODE), I2CAddress(0x6B), chipSelectPin(D10)
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 0:301b2cb34ead 83 DigitalOut cs(chipSelectPin);
adam_z 0:301b2cb34ead 84 // Maximum SPI frequency is 10MHz, could divide by 2 here:
adam_z 0:301b2cb34ead 85 spi.frequency(5000000);
adam_z 0:301b2cb34ead 86 // Data is read and written MSb first.
adam_z 0:301b2cb34ead 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 0:301b2cb34ead 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 0:301b2cb34ead 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 0:301b2cb34ead 170 break;
adam_z 0:301b2cb34ead 171
adam_z 0:301b2cb34ead 172 case SPI_MODE:
adam_z 0:301b2cb34ead 173 // take the chip select low to select the device:
adam_z 0:301b2cb34ead 174 digitalWrite(chipSelectPin, LOW);
adam_z 0:301b2cb34ead 175 // send the device the register you want to read:
adam_z 0:301b2cb34ead 176 SPI.transfer(offset | 0x80); //Ored with "read request" bit
adam_z 0:301b2cb34ead 177 while ( i < length ) { // slave may send less than requested
adam_z 0:301b2cb34ead 178 c = SPI.transfer(0x00); // receive a byte as character
adam_z 0:301b2cb34ead 179 if( c == 0xFF ) {
adam_z 0:301b2cb34ead 180 //May have problem
adam_z 0:301b2cb34ead 181 tempFFCounter++;
adam_z 0:301b2cb34ead 182 }
adam_z 0:301b2cb34ead 183 *outputPointer = c;
adam_z 0:301b2cb34ead 184 outputPointer++;
adam_z 0:301b2cb34ead 185 i++;
adam_z 0:301b2cb34ead 186 }
adam_z 0:301b2cb34ead 187 if( tempFFCounter == i ) {
adam_z 0:301b2cb34ead 188 //Ok, we've recieved all ones, report
adam_z 0:301b2cb34ead 189 returnError = IMU_ALL_ONES_WARNING;
adam_z 0:301b2cb34ead 190 }
adam_z 0:301b2cb34ead 191 // take the chip select high to de-select:
adam_z 0:301b2cb34ead 192 digitalWrite(chipSelectPin, HIGH);
adam_z 0:301b2cb34ead 193 break;
adam_z 0:301b2cb34ead 194
adam_z 0:301b2cb34ead 195 default:
adam_z 0:301b2cb34ead 196 break;
adam_z 0:301b2cb34ead 197 }
adam_z 0:301b2cb34ead 198
adam_z 0:301b2cb34ead 199 return returnError;
adam_z 0:301b2cb34ead 200 }
adam_z 0:301b2cb34ead 201
adam_z 0:301b2cb34ead 202 //****************************************************************************//
adam_z 0:301b2cb34ead 203 //
adam_z 0:301b2cb34ead 204 // ReadRegister
adam_z 0:301b2cb34ead 205 //
adam_z 0:301b2cb34ead 206 // Parameters:
adam_z 0:301b2cb34ead 207 // *outputPointer -- Pass &variable (address of) to save read data to
adam_z 0:301b2cb34ead 208 // offset -- register to read
adam_z 0:301b2cb34ead 209 //
adam_z 0:301b2cb34ead 210 //****************************************************************************//
adam_z 0:301b2cb34ead 211 status_t LSM6DS3Core::readRegister(uint8_t* outputPointer, uint8_t offset)
adam_z 0:301b2cb34ead 212 {
adam_z 0:301b2cb34ead 213 //Return value
adam_z 0:301b2cb34ead 214 uint8_t result;
adam_z 0:301b2cb34ead 215 uint8_t numBytes = 1;
adam_z 0:301b2cb34ead 216 status_t returnError = IMU_SUCCESS;
adam_z 0:301b2cb34ead 217
adam_z 0:301b2cb34ead 218 switch (commInterface) {
adam_z 0:301b2cb34ead 219
adam_z 0:301b2cb34ead 220 case I2C_MODE:
adam_z 0:301b2cb34ead 221 Wire.beginTransmission(I2CAddress);
adam_z 0:301b2cb34ead 222 Wire.write(offset);
adam_z 0:301b2cb34ead 223 if( Wire.endTransmission() != 0 ) {
adam_z 0:301b2cb34ead 224 returnError = IMU_HW_ERROR;
adam_z 0:301b2cb34ead 225 }
adam_z 0:301b2cb34ead 226 Wire.requestFrom(I2CAddress, numBytes);
adam_z 0:301b2cb34ead 227 while ( Wire.available() ) { // slave may send less than requested
adam_z 0:301b2cb34ead 228 result = Wire.read(); // receive a byte as a proper uint8_t
adam_z 0:301b2cb34ead 229 }
adam_z 0:301b2cb34ead 230 break;
adam_z 0:301b2cb34ead 231
adam_z 0:301b2cb34ead 232 case SPI_MODE:
adam_z 0:301b2cb34ead 233 // take the chip select low to select the device:
adam_z 0:301b2cb34ead 234 digitalWrite(chipSelectPin, LOW);
adam_z 0:301b2cb34ead 235 // send the device the register you want to read:
adam_z 0:301b2cb34ead 236 SPI.transfer(offset | 0x80); //Ored with "read request" bit
adam_z 0:301b2cb34ead 237 // send a value of 0 to read the first byte returned:
adam_z 0:301b2cb34ead 238 result = SPI.transfer(0x00);
adam_z 0:301b2cb34ead 239 // take the chip select high to de-select:
adam_z 0:301b2cb34ead 240 digitalWrite(chipSelectPin, HIGH);
adam_z 0:301b2cb34ead 241
adam_z 0:301b2cb34ead 242 if( result == 0xFF ) {
adam_z 0:301b2cb34ead 243 //we've recieved all ones, report
adam_z 0:301b2cb34ead 244 returnError = IMU_ALL_ONES_WARNING;
adam_z 0:301b2cb34ead 245 }
adam_z 0:301b2cb34ead 246 break;
adam_z 0:301b2cb34ead 247
adam_z 0:301b2cb34ead 248 default:
adam_z 0:301b2cb34ead 249 break;
adam_z 0:301b2cb34ead 250 }
adam_z 0:301b2cb34ead 251
adam_z 0:301b2cb34ead 252 *outputPointer = result;
adam_z 0:301b2cb34ead 253 return returnError;
adam_z 0:301b2cb34ead 254 }
adam_z 0:301b2cb34ead 255
adam_z 0:301b2cb34ead 256 //****************************************************************************//
adam_z 0:301b2cb34ead 257 //
adam_z 0:301b2cb34ead 258 // readRegisterInt16
adam_z 0:301b2cb34ead 259 //
adam_z 0:301b2cb34ead 260 // Parameters:
adam_z 0:301b2cb34ead 261 // *outputPointer -- Pass &variable (base address of) to save read data to
adam_z 0:301b2cb34ead 262 // offset -- register to read
adam_z 0:301b2cb34ead 263 //
adam_z 0:301b2cb34ead 264 //****************************************************************************//
adam_z 0:301b2cb34ead 265 status_t LSM6DS3Core::readRegisterInt16( int16_t* outputPointer, uint8_t offset )
adam_z 0:301b2cb34ead 266 {
adam_z 0:301b2cb34ead 267 uint8_t myBuffer[2];
adam_z 0:301b2cb34ead 268 status_t returnError = readRegisterRegion(myBuffer, offset, 2); //Does memory transfer
adam_z 0:301b2cb34ead 269 int16_t output = (int16_t)myBuffer[0] | int16_t(myBuffer[1] << 8);
adam_z 0:301b2cb34ead 270
adam_z 0:301b2cb34ead 271 *outputPointer = output;
adam_z 0:301b2cb34ead 272 return returnError;
adam_z 0:301b2cb34ead 273 }
adam_z 0:301b2cb34ead 274
adam_z 0:301b2cb34ead 275 //****************************************************************************//
adam_z 0:301b2cb34ead 276 //
adam_z 0:301b2cb34ead 277 // writeRegister
adam_z 0:301b2cb34ead 278 //
adam_z 0:301b2cb34ead 279 // Parameters:
adam_z 0:301b2cb34ead 280 // offset -- register to write
adam_z 0:301b2cb34ead 281 // dataToWrite -- 8 bit data to write to register
adam_z 0:301b2cb34ead 282 //
adam_z 0:301b2cb34ead 283 //****************************************************************************//
adam_z 0:301b2cb34ead 284 status_t LSM6DS3Core::writeRegister(uint8_t offset, uint8_t dataToWrite)
adam_z 0:301b2cb34ead 285 {
adam_z 0:301b2cb34ead 286 status_t returnError = IMU_SUCCESS;
adam_z 0:301b2cb34ead 287 switch (commInterface) {
adam_z 0:301b2cb34ead 288 case I2C_MODE:
adam_z 0:301b2cb34ead 289 //Write the byte
adam_z 0:301b2cb34ead 290 Wire.beginTransmission(I2CAddress);
adam_z 0:301b2cb34ead 291 Wire.write(offset);
adam_z 0:301b2cb34ead 292 Wire.write(dataToWrite);
adam_z 0:301b2cb34ead 293 if( Wire.endTransmission() != 0 ) {
adam_z 0:301b2cb34ead 294 returnError = IMU_HW_ERROR;
adam_z 0:301b2cb34ead 295 }
adam_z 0:301b2cb34ead 296 break;
adam_z 0:301b2cb34ead 297
adam_z 0:301b2cb34ead 298 case SPI_MODE:
adam_z 0:301b2cb34ead 299 // take the chip select low to select the device:
adam_z 0:301b2cb34ead 300 digitalWrite(chipSelectPin, LOW);
adam_z 0:301b2cb34ead 301 // send the device the register you want to read:
adam_z 0:301b2cb34ead 302 SPI.transfer(offset);
adam_z 0:301b2cb34ead 303 // send a value of 0 to read the first byte returned:
adam_z 0:301b2cb34ead 304 SPI.transfer(dataToWrite);
adam_z 0:301b2cb34ead 305 // decrement the number of bytes left to read:
adam_z 0:301b2cb34ead 306 // take the chip select high to de-select:
adam_z 0:301b2cb34ead 307 digitalWrite(chipSelectPin, HIGH);
adam_z 0:301b2cb34ead 308 break;
adam_z 0:301b2cb34ead 309
adam_z 0:301b2cb34ead 310 //No way to check error on this write (Except to read back but that's not reliable)
adam_z 0:301b2cb34ead 311
adam_z 0:301b2cb34ead 312 default:
adam_z 0:301b2cb34ead 313 break;
adam_z 0:301b2cb34ead 314 }
adam_z 0:301b2cb34ead 315
adam_z 0:301b2cb34ead 316 return returnError;
adam_z 0:301b2cb34ead 317 }
adam_z 0:301b2cb34ead 318
adam_z 0:301b2cb34ead 319 status_t LSM6DS3Core::embeddedPage( void )
adam_z 0:301b2cb34ead 320 {
adam_z 0:301b2cb34ead 321 status_t returnError = writeRegister( LSM6DS3_ACC_GYRO_RAM_ACCESS, 0x80 );
adam_z 0:301b2cb34ead 322
adam_z 0:301b2cb34ead 323 return returnError;
adam_z 0:301b2cb34ead 324 }
adam_z 0:301b2cb34ead 325
adam_z 0:301b2cb34ead 326 status_t LSM6DS3Core::basePage( void )
adam_z 0:301b2cb34ead 327 {
adam_z 0:301b2cb34ead 328 status_t returnError = writeRegister( LSM6DS3_ACC_GYRO_RAM_ACCESS, 0x00 );
adam_z 0:301b2cb34ead 329
adam_z 0:301b2cb34ead 330 return returnError;
adam_z 0:301b2cb34ead 331 }
adam_z 0:301b2cb34ead 332
adam_z 0:301b2cb34ead 333
adam_z 0:301b2cb34ead 334 //****************************************************************************//
adam_z 0:301b2cb34ead 335 //
adam_z 0:301b2cb34ead 336 // Main user class -- wrapper for the core class + maths
adam_z 0:301b2cb34ead 337 //
adam_z 0:301b2cb34ead 338 // Construct with same rules as the core ( uint8_t busType, uint8_t inputArg )
adam_z 0:301b2cb34ead 339 //
adam_z 0:301b2cb34ead 340 //****************************************************************************//
adam_z 0:301b2cb34ead 341 LSM6DS3::LSM6DS3( uint8_t busType, uint8_t inputArg ) : LSM6DS3Core( busType, inputArg )
adam_z 0:301b2cb34ead 342 {
adam_z 0:301b2cb34ead 343 //Construct with these default settings
adam_z 0:301b2cb34ead 344
adam_z 0:301b2cb34ead 345 settings.gyroEnabled = 1; //Can be 0 or 1
adam_z 0:301b2cb34ead 346 settings.gyroRange = 2000; //Max deg/s. Can be: 125, 245, 500, 1000, 2000
adam_z 0:301b2cb34ead 347 settings.gyroSampleRate = 833; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666
adam_z 0:301b2cb34ead 348 settings.gyroBandWidth = 400; //Hz. Can be: 50, 100, 200, 400;
adam_z 0:301b2cb34ead 349 settings.gyroFifoEnabled = 0; //Set to include gyro in FIFO
adam_z 0:301b2cb34ead 350 settings.gyroFifoDecimation = 1; //set 1 for on /1
adam_z 0:301b2cb34ead 351
adam_z 0:301b2cb34ead 352 settings.accelEnabled = 1;
adam_z 0:301b2cb34ead 353 settings.accelODROff = 1;
adam_z 0:301b2cb34ead 354 settings.accelRange = 8; //Max G force readable. Can be: 2, 4, 8, 16
adam_z 0:301b2cb34ead 355 settings.accelSampleRate = 833; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 13330
adam_z 0:301b2cb34ead 356 settings.accelBandWidth = 400; //Hz. Can be: 50, 100, 200, 400;
adam_z 0:301b2cb34ead 357 settings.accelFifoEnabled = 0; //Set to include accelerometer in the FIFO
adam_z 0:301b2cb34ead 358 settings.accelFifoDecimation = 1; //set 1 for on /1
adam_z 0:301b2cb34ead 359
adam_z 0:301b2cb34ead 360 settings.tempEnabled = 1;
adam_z 0:301b2cb34ead 361
adam_z 0:301b2cb34ead 362 //Select interface mode
adam_z 0:301b2cb34ead 363 settings.commMode = 1; //Can be modes 1, 2 or 3
adam_z 0:301b2cb34ead 364
adam_z 0:301b2cb34ead 365 //FIFO control data
adam_z 0:301b2cb34ead 366 settings.fifoThreshold = 3000; //Can be 0 to 4096 (16 bit bytes)
adam_z 0:301b2cb34ead 367 settings.fifoSampleRate = 10; //default 10Hz
adam_z 0:301b2cb34ead 368 settings.fifoModeWord = 0; //Default off
adam_z 0:301b2cb34ead 369
adam_z 0:301b2cb34ead 370 allOnesCounter = 0;
adam_z 0:301b2cb34ead 371 nonSuccessCounter = 0;
adam_z 0:301b2cb34ead 372
adam_z 0:301b2cb34ead 373 }
adam_z 0:301b2cb34ead 374
adam_z 0:301b2cb34ead 375 //****************************************************************************//
adam_z 0:301b2cb34ead 376 //
adam_z 0:301b2cb34ead 377 // Configuration section
adam_z 0:301b2cb34ead 378 //
adam_z 0:301b2cb34ead 379 // This uses the stored SensorSettings to start the IMU
adam_z 0:301b2cb34ead 380 // Use statements such as "myIMU.settings.commInterface = SPI_MODE;" or
adam_z 0:301b2cb34ead 381 // "myIMU.settings.accelEnabled = 1;" to configure before calling .begin();
adam_z 0:301b2cb34ead 382 //
adam_z 0:301b2cb34ead 383 //****************************************************************************//
adam_z 0:301b2cb34ead 384 status_t LSM6DS3::begin()
adam_z 0:301b2cb34ead 385 {
adam_z 0:301b2cb34ead 386 //Check the settings structure values to determine how to setup the device
adam_z 0:301b2cb34ead 387 uint8_t dataToWrite = 0; //Temporary variable
adam_z 0:301b2cb34ead 388
adam_z 0:301b2cb34ead 389 //Begin the inherited core. This gets the physical wires connected
adam_z 0:301b2cb34ead 390 status_t returnError = beginCore();
adam_z 0:301b2cb34ead 391
adam_z 0:301b2cb34ead 392 //setOffset(-61, -25, -66, 35, -81, -32);
adam_z 0:301b2cb34ead 393
adam_z 0:301b2cb34ead 394 //Setup the accelerometer******************************
adam_z 0:301b2cb34ead 395 dataToWrite = 0; //Start Fresh!
adam_z 0:301b2cb34ead 396 if ( settings.accelEnabled == 1) {
adam_z 0:301b2cb34ead 397 //Build config reg
adam_z 0:301b2cb34ead 398 //First patch in filter bandwidth
adam_z 0:301b2cb34ead 399 switch (settings.accelBandWidth) {
adam_z 0:301b2cb34ead 400 case 50:
adam_z 0:301b2cb34ead 401 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_50Hz;
adam_z 0:301b2cb34ead 402 break;
adam_z 0:301b2cb34ead 403 case 100:
adam_z 0:301b2cb34ead 404 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_100Hz;
adam_z 0:301b2cb34ead 405 break;
adam_z 0:301b2cb34ead 406 case 200:
adam_z 0:301b2cb34ead 407 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_200Hz;
adam_z 0:301b2cb34ead 408 break;
adam_z 0:301b2cb34ead 409 default: //set default case to max passthrough
adam_z 0:301b2cb34ead 410 case 400:
adam_z 0:301b2cb34ead 411 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_400Hz;
adam_z 0:301b2cb34ead 412 break;
adam_z 0:301b2cb34ead 413 }
adam_z 0:301b2cb34ead 414 //Next, patch in full scale
adam_z 0:301b2cb34ead 415 switch (settings.accelRange) {
adam_z 0:301b2cb34ead 416 case 2:
adam_z 0:301b2cb34ead 417 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_2g;
adam_z 0:301b2cb34ead 418 break;
adam_z 0:301b2cb34ead 419 case 4:
adam_z 0:301b2cb34ead 420 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_4g;
adam_z 0:301b2cb34ead 421 break;
adam_z 0:301b2cb34ead 422 case 8:
adam_z 0:301b2cb34ead 423 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_8g;
adam_z 0:301b2cb34ead 424 break;
adam_z 0:301b2cb34ead 425 default: //set default case to 16(max)
adam_z 0:301b2cb34ead 426 case 16:
adam_z 0:301b2cb34ead 427 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_16g;
adam_z 0:301b2cb34ead 428 break;
adam_z 0:301b2cb34ead 429 }
adam_z 0:301b2cb34ead 430 //Lastly, patch in accelerometer ODR
adam_z 0:301b2cb34ead 431 switch (settings.accelSampleRate) {
adam_z 0:301b2cb34ead 432 case 13:
adam_z 0:301b2cb34ead 433 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_13Hz;
adam_z 0:301b2cb34ead 434 break;
adam_z 0:301b2cb34ead 435 case 26:
adam_z 0:301b2cb34ead 436 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_26Hz;
adam_z 0:301b2cb34ead 437 break;
adam_z 0:301b2cb34ead 438 case 52:
adam_z 0:301b2cb34ead 439 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_52Hz;
adam_z 0:301b2cb34ead 440 break;
adam_z 0:301b2cb34ead 441 default: //Set default to 104
adam_z 0:301b2cb34ead 442 case 104:
adam_z 0:301b2cb34ead 443 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_104Hz;
adam_z 0:301b2cb34ead 444 break;
adam_z 0:301b2cb34ead 445 case 208:
adam_z 0:301b2cb34ead 446 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_208Hz;
adam_z 0:301b2cb34ead 447 break;
adam_z 0:301b2cb34ead 448 case 416:
adam_z 0:301b2cb34ead 449 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_416Hz;
adam_z 0:301b2cb34ead 450 break;
adam_z 0:301b2cb34ead 451 case 833:
adam_z 0:301b2cb34ead 452 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_833Hz;
adam_z 0:301b2cb34ead 453 break;
adam_z 0:301b2cb34ead 454 case 1660:
adam_z 0:301b2cb34ead 455 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_1660Hz;
adam_z 0:301b2cb34ead 456 break;
adam_z 0:301b2cb34ead 457 case 3330:
adam_z 0:301b2cb34ead 458 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_3330Hz;
adam_z 0:301b2cb34ead 459 break;
adam_z 0:301b2cb34ead 460 case 6660:
adam_z 0:301b2cb34ead 461 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_6660Hz;
adam_z 0:301b2cb34ead 462 break;
adam_z 0:301b2cb34ead 463 case 13330:
adam_z 0:301b2cb34ead 464 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_13330Hz;
adam_z 0:301b2cb34ead 465 break;
adam_z 0:301b2cb34ead 466 }
adam_z 0:301b2cb34ead 467 } else {
adam_z 0:301b2cb34ead 468 //dataToWrite already = 0 (powerdown);
adam_z 0:301b2cb34ead 469 }
adam_z 0:301b2cb34ead 470
adam_z 0:301b2cb34ead 471 //Now, write the patched together data
adam_z 0:301b2cb34ead 472 writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, dataToWrite);
adam_z 0:301b2cb34ead 473
adam_z 0:301b2cb34ead 474 //Set the ODR bit
adam_z 0:301b2cb34ead 475 readRegister(&dataToWrite, LSM6DS3_ACC_GYRO_CTRL4_C);
adam_z 0:301b2cb34ead 476 dataToWrite &= ~((uint8_t)LSM6DS3_ACC_GYRO_BW_SCAL_ODR_ENABLED);
adam_z 0:301b2cb34ead 477 if ( settings.accelODROff == 1) {
adam_z 0:301b2cb34ead 478 dataToWrite |= LSM6DS3_ACC_GYRO_BW_SCAL_ODR_ENABLED;
adam_z 0:301b2cb34ead 479 }
adam_z 0:301b2cb34ead 480 writeRegister(LSM6DS3_ACC_GYRO_CTRL4_C, dataToWrite);
adam_z 0:301b2cb34ead 481
adam_z 0:301b2cb34ead 482 //Setup the gyroscope**********************************************
adam_z 0:301b2cb34ead 483 dataToWrite = 0; //Start Fresh!
adam_z 0:301b2cb34ead 484 if ( settings.gyroEnabled == 1) {
adam_z 0:301b2cb34ead 485 //Build config reg
adam_z 0:301b2cb34ead 486 //First, patch in full scale
adam_z 0:301b2cb34ead 487 switch (settings.gyroRange) {
adam_z 0:301b2cb34ead 488 case 125:
adam_z 0:301b2cb34ead 489 dataToWrite |= LSM6DS3_ACC_GYRO_FS_125_ENABLED;
adam_z 0:301b2cb34ead 490 break;
adam_z 0:301b2cb34ead 491 case 245:
adam_z 0:301b2cb34ead 492 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_245dps;
adam_z 0:301b2cb34ead 493 break;
adam_z 0:301b2cb34ead 494 case 500:
adam_z 0:301b2cb34ead 495 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_500dps;
adam_z 0:301b2cb34ead 496 break;
adam_z 0:301b2cb34ead 497 case 1000:
adam_z 0:301b2cb34ead 498 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_1000dps;
adam_z 0:301b2cb34ead 499 break;
adam_z 0:301b2cb34ead 500 default: //Default to full 2000DPS range
adam_z 0:301b2cb34ead 501 case 2000:
adam_z 0:301b2cb34ead 502 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_2000dps;
adam_z 0:301b2cb34ead 503 break;
adam_z 0:301b2cb34ead 504 }
adam_z 0:301b2cb34ead 505 //Lastly, patch in gyro ODR
adam_z 0:301b2cb34ead 506 switch (settings.gyroSampleRate) {
adam_z 0:301b2cb34ead 507 case 13:
adam_z 0:301b2cb34ead 508 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_13Hz;
adam_z 0:301b2cb34ead 509 break;
adam_z 0:301b2cb34ead 510 case 26:
adam_z 0:301b2cb34ead 511 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_26Hz;
adam_z 0:301b2cb34ead 512 break;
adam_z 0:301b2cb34ead 513 case 52:
adam_z 0:301b2cb34ead 514 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_52Hz;
adam_z 0:301b2cb34ead 515 break;
adam_z 0:301b2cb34ead 516 default: //Set default to 104
adam_z 0:301b2cb34ead 517 case 104:
adam_z 0:301b2cb34ead 518 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_104Hz;
adam_z 0:301b2cb34ead 519 break;
adam_z 0:301b2cb34ead 520 case 208:
adam_z 0:301b2cb34ead 521 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_208Hz;
adam_z 0:301b2cb34ead 522 break;
adam_z 0:301b2cb34ead 523 case 416:
adam_z 0:301b2cb34ead 524 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_416Hz;
adam_z 0:301b2cb34ead 525 break;
adam_z 0:301b2cb34ead 526 case 833:
adam_z 0:301b2cb34ead 527 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_833Hz;
adam_z 0:301b2cb34ead 528 break;
adam_z 0:301b2cb34ead 529 case 1660:
adam_z 0:301b2cb34ead 530 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_1660Hz;
adam_z 0:301b2cb34ead 531 break;
adam_z 0:301b2cb34ead 532 }
adam_z 0:301b2cb34ead 533 } else {
adam_z 0:301b2cb34ead 534 //dataToWrite already = 0 (powerdown);
adam_z 0:301b2cb34ead 535 }
adam_z 0:301b2cb34ead 536 //Write the byte
adam_z 0:301b2cb34ead 537 writeRegister(LSM6DS3_ACC_GYRO_CTRL2_G, dataToWrite);
adam_z 0:301b2cb34ead 538
adam_z 0:301b2cb34ead 539 //Setup the internal temperature sensor
adam_z 0:301b2cb34ead 540 if ( settings.tempEnabled == 1) {
adam_z 0:301b2cb34ead 541 }
adam_z 0:301b2cb34ead 542
adam_z 0:301b2cb34ead 543 //Return WHO AM I reg //Not no mo!
adam_z 0:301b2cb34ead 544 uint8_t result;
adam_z 0:301b2cb34ead 545 readRegister(&result, LSM6DS3_ACC_GYRO_WHO_AM_I_REG);
adam_z 0:301b2cb34ead 546
adam_z 0:301b2cb34ead 547 return returnError;
adam_z 0:301b2cb34ead 548 }
adam_z 0:301b2cb34ead 549
adam_z 0:301b2cb34ead 550 //****************************************************************************//
adam_z 0:301b2cb34ead 551 //
adam_z 0:301b2cb34ead 552 // Accelerometer section
adam_z 0:301b2cb34ead 553 //
adam_z 0:301b2cb34ead 554 //****************************************************************************//
adam_z 0:301b2cb34ead 555
adam_z 0:301b2cb34ead 556 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 557 {
adam_z 0:301b2cb34ead 558 accelOffset[0] = ax;
adam_z 0:301b2cb34ead 559 accelOffset[1] = ay;
adam_z 0:301b2cb34ead 560 accelOffset[2] = az;
adam_z 0:301b2cb34ead 561 gyroOffset[0] = gx;
adam_z 0:301b2cb34ead 562 gyroOffset[1] = gy;
adam_z 0:301b2cb34ead 563 gyroOffset[2] = gz;
adam_z 0:301b2cb34ead 564 }
adam_z 0:301b2cb34ead 565
adam_z 0:301b2cb34ead 566 int16_t LSM6DS3::readRawAccelX( void )
adam_z 0:301b2cb34ead 567 {
adam_z 0:301b2cb34ead 568 int16_t output;
adam_z 0:301b2cb34ead 569 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTX_L_XL );
adam_z 0:301b2cb34ead 570 if( errorLevel != IMU_SUCCESS ) {
adam_z 0:301b2cb34ead 571 if( errorLevel == IMU_ALL_ONES_WARNING ) {
adam_z 0:301b2cb34ead 572 allOnesCounter++;
adam_z 0:301b2cb34ead 573 } else {
adam_z 0:301b2cb34ead 574 nonSuccessCounter++;
adam_z 0:301b2cb34ead 575 }
adam_z 0:301b2cb34ead 576 }
adam_z 0:301b2cb34ead 577 return output;
adam_z 0:301b2cb34ead 578 }
adam_z 0:301b2cb34ead 579 float LSM6DS3::readFloatAccelX( void )
adam_z 0:301b2cb34ead 580 {
adam_z 0:301b2cb34ead 581 float output = calcAccel((int32_t)readRawAccelX() - (int32_t)accelOffset[0]);
adam_z 0:301b2cb34ead 582 return output;
adam_z 0:301b2cb34ead 583 }
adam_z 0:301b2cb34ead 584
adam_z 0:301b2cb34ead 585 int16_t LSM6DS3::readRawAccelY( void )
adam_z 0:301b2cb34ead 586 {
adam_z 0:301b2cb34ead 587 int16_t output;
adam_z 0:301b2cb34ead 588 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTY_L_XL );
adam_z 0:301b2cb34ead 589 if( errorLevel != IMU_SUCCESS ) {
adam_z 0:301b2cb34ead 590 if( errorLevel == IMU_ALL_ONES_WARNING ) {
adam_z 0:301b2cb34ead 591 allOnesCounter++;
adam_z 0:301b2cb34ead 592 } else {
adam_z 0:301b2cb34ead 593 nonSuccessCounter++;
adam_z 0:301b2cb34ead 594 }
adam_z 0:301b2cb34ead 595 }
adam_z 0:301b2cb34ead 596 return output;
adam_z 0:301b2cb34ead 597 }
adam_z 0:301b2cb34ead 598 float LSM6DS3::readFloatAccelY( void )
adam_z 0:301b2cb34ead 599 {
adam_z 0:301b2cb34ead 600 float output = calcAccel((int32_t)readRawAccelY() - (int32_t)accelOffset[1]);
adam_z 0:301b2cb34ead 601 return output;
adam_z 0:301b2cb34ead 602 }
adam_z 0:301b2cb34ead 603
adam_z 0:301b2cb34ead 604 int16_t LSM6DS3::readRawAccelZ( void )
adam_z 0:301b2cb34ead 605 {
adam_z 0:301b2cb34ead 606 int16_t output;
adam_z 0:301b2cb34ead 607 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTZ_L_XL );
adam_z 0:301b2cb34ead 608 if( errorLevel != IMU_SUCCESS ) {
adam_z 0:301b2cb34ead 609 if( errorLevel == IMU_ALL_ONES_WARNING ) {
adam_z 0:301b2cb34ead 610 allOnesCounter++;
adam_z 0:301b2cb34ead 611 } else {
adam_z 0:301b2cb34ead 612 nonSuccessCounter++;
adam_z 0:301b2cb34ead 613 }
adam_z 0:301b2cb34ead 614 }
adam_z 0:301b2cb34ead 615 return output;
adam_z 0:301b2cb34ead 616 }
adam_z 0:301b2cb34ead 617 float LSM6DS3::readFloatAccelZ( void )
adam_z 0:301b2cb34ead 618 {
adam_z 0:301b2cb34ead 619 float output = calcAccel((int32_t)readRawAccelZ() - (int32_t)accelOffset[2]);
adam_z 0:301b2cb34ead 620 return output;
adam_z 0:301b2cb34ead 621 }
adam_z 0:301b2cb34ead 622
adam_z 0:301b2cb34ead 623 float LSM6DS3::calcAccel( int32_t input )
adam_z 0:301b2cb34ead 624 {
adam_z 0:301b2cb34ead 625 float output = (float)input * 0.061 * (settings.accelRange >> 1) / 1000;
adam_z 0:301b2cb34ead 626 return output;
adam_z 0:301b2cb34ead 627 }
adam_z 0:301b2cb34ead 628
adam_z 0:301b2cb34ead 629 //****************************************************************************//
adam_z 0:301b2cb34ead 630 //
adam_z 0:301b2cb34ead 631 // Gyroscope section
adam_z 0:301b2cb34ead 632 //
adam_z 0:301b2cb34ead 633 //****************************************************************************//
adam_z 0:301b2cb34ead 634 int16_t LSM6DS3::readRawGyroX( void )
adam_z 0:301b2cb34ead 635 {
adam_z 0:301b2cb34ead 636 int16_t output;
adam_z 0:301b2cb34ead 637 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTX_L_G );
adam_z 0:301b2cb34ead 638 if( errorLevel != IMU_SUCCESS ) {
adam_z 0:301b2cb34ead 639 if( errorLevel == IMU_ALL_ONES_WARNING ) {
adam_z 0:301b2cb34ead 640 allOnesCounter++;
adam_z 0:301b2cb34ead 641 } else {
adam_z 0:301b2cb34ead 642 nonSuccessCounter++;
adam_z 0:301b2cb34ead 643 }
adam_z 0:301b2cb34ead 644 }
adam_z 0:301b2cb34ead 645 return output;
adam_z 0:301b2cb34ead 646 }
adam_z 0:301b2cb34ead 647 float LSM6DS3::readFloatGyroX( void )
adam_z 0:301b2cb34ead 648 {
adam_z 0:301b2cb34ead 649 float output = calcGyro((int32_t)readRawGyroX() - (int32_t)gyroOffset[0]);
adam_z 0:301b2cb34ead 650 return output;
adam_z 0:301b2cb34ead 651 }
adam_z 0:301b2cb34ead 652
adam_z 0:301b2cb34ead 653 int16_t LSM6DS3::readRawGyroY( void )
adam_z 0:301b2cb34ead 654 {
adam_z 0:301b2cb34ead 655 int16_t output;
adam_z 0:301b2cb34ead 656 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTY_L_G );
adam_z 0:301b2cb34ead 657 if( errorLevel != IMU_SUCCESS ) {
adam_z 0:301b2cb34ead 658 if( errorLevel == IMU_ALL_ONES_WARNING ) {
adam_z 0:301b2cb34ead 659 allOnesCounter++;
adam_z 0:301b2cb34ead 660 } else {
adam_z 0:301b2cb34ead 661 nonSuccessCounter++;
adam_z 0:301b2cb34ead 662 }
adam_z 0:301b2cb34ead 663 }
adam_z 0:301b2cb34ead 664 return output;
adam_z 0:301b2cb34ead 665 }
adam_z 0:301b2cb34ead 666 float LSM6DS3::readFloatGyroY( void )
adam_z 0:301b2cb34ead 667 {
adam_z 0:301b2cb34ead 668 float output = calcGyro((int32_t)readRawGyroY() - (int32_t)gyroOffset[1]);
adam_z 0:301b2cb34ead 669 return output;
adam_z 0:301b2cb34ead 670 }
adam_z 0:301b2cb34ead 671
adam_z 0:301b2cb34ead 672 int16_t LSM6DS3::readRawGyroZ( void )
adam_z 0:301b2cb34ead 673 {
adam_z 0:301b2cb34ead 674 int16_t output;
adam_z 0:301b2cb34ead 675 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTZ_L_G );
adam_z 0:301b2cb34ead 676 if( errorLevel != IMU_SUCCESS ) {
adam_z 0:301b2cb34ead 677 if( errorLevel == IMU_ALL_ONES_WARNING ) {
adam_z 0:301b2cb34ead 678 allOnesCounter++;
adam_z 0:301b2cb34ead 679 } else {
adam_z 0:301b2cb34ead 680 nonSuccessCounter++;
adam_z 0:301b2cb34ead 681 }
adam_z 0:301b2cb34ead 682 }
adam_z 0:301b2cb34ead 683 return output;
adam_z 0:301b2cb34ead 684 }
adam_z 0:301b2cb34ead 685 float LSM6DS3::readFloatGyroZ( void )
adam_z 0:301b2cb34ead 686 {
adam_z 0:301b2cb34ead 687 float output = calcGyro((int32_t)readRawGyroZ() - (int32_t)gyroOffset[2]);
adam_z 0:301b2cb34ead 688 return output;
adam_z 0:301b2cb34ead 689 }
adam_z 0:301b2cb34ead 690
adam_z 0:301b2cb34ead 691 float LSM6DS3::calcGyro( int32_t input )
adam_z 0:301b2cb34ead 692 {
adam_z 0:301b2cb34ead 693 uint8_t gyroRangeDivisor = settings.gyroRange / 125;
adam_z 0:301b2cb34ead 694 if ( settings.gyroRange == 245 ) {
adam_z 0:301b2cb34ead 695 gyroRangeDivisor = 2;
adam_z 0:301b2cb34ead 696 }
adam_z 0:301b2cb34ead 697
adam_z 0:301b2cb34ead 698 float output = (float)input * 4.375 * (gyroRangeDivisor) / 1000;
adam_z 0:301b2cb34ead 699 return output;
adam_z 0:301b2cb34ead 700 }
adam_z 0:301b2cb34ead 701
adam_z 0:301b2cb34ead 702 //****************************************************************************//
adam_z 0:301b2cb34ead 703 //
adam_z 0:301b2cb34ead 704 // Temperature section
adam_z 0:301b2cb34ead 705 //
adam_z 0:301b2cb34ead 706 //****************************************************************************//
adam_z 0:301b2cb34ead 707 int16_t LSM6DS3::readRawTemp( void )
adam_z 0:301b2cb34ead 708 {
adam_z 0:301b2cb34ead 709 int16_t output;
adam_z 0:301b2cb34ead 710 readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUT_TEMP_L );
adam_z 0:301b2cb34ead 711 return output;
adam_z 0:301b2cb34ead 712 }
adam_z 0:301b2cb34ead 713
adam_z 0:301b2cb34ead 714 float LSM6DS3::readTempC( void )
adam_z 0:301b2cb34ead 715 {
adam_z 0:301b2cb34ead 716 float output = (float)readRawTemp() / 16; //divide by 16 to scale
adam_z 0:301b2cb34ead 717 output += 25; //Add 25 degrees to remove offset
adam_z 0:301b2cb34ead 718
adam_z 0:301b2cb34ead 719 return output;
adam_z 0:301b2cb34ead 720
adam_z 0:301b2cb34ead 721 }
adam_z 0:301b2cb34ead 722
adam_z 0:301b2cb34ead 723 float LSM6DS3::readTempF( void )
adam_z 0:301b2cb34ead 724 {
adam_z 0:301b2cb34ead 725 float output = (float)readRawTemp() / 16; //divide by 16 to scale
adam_z 0:301b2cb34ead 726 output += 25; //Add 25 degrees to remove offset
adam_z 0:301b2cb34ead 727 output = (output * 9) / 5 + 32;
adam_z 0:301b2cb34ead 728
adam_z 0:301b2cb34ead 729 return output;
adam_z 0:301b2cb34ead 730
adam_z 0:301b2cb34ead 731 }
adam_z 0:301b2cb34ead 732
adam_z 0:301b2cb34ead 733 //****************************************************************************//
adam_z 0:301b2cb34ead 734 //
adam_z 0:301b2cb34ead 735 // FIFO section
adam_z 0:301b2cb34ead 736 //
adam_z 0:301b2cb34ead 737 //****************************************************************************//
adam_z 0:301b2cb34ead 738 void LSM6DS3::fifoBegin( void )
adam_z 0:301b2cb34ead 739 {
adam_z 0:301b2cb34ead 740 //CONFIGURE THE VARIOUS FIFO SETTINGS
adam_z 0:301b2cb34ead 741 //
adam_z 0:301b2cb34ead 742 //
adam_z 0:301b2cb34ead 743 //This section first builds a bunch of config words, then goes through
adam_z 0:301b2cb34ead 744 //and writes them all.
adam_z 0:301b2cb34ead 745
adam_z 0:301b2cb34ead 746 //Split and mask the threshold
adam_z 0:301b2cb34ead 747 uint8_t thresholdLByte = settings.fifoThreshold & 0x00FF;
adam_z 0:301b2cb34ead 748 uint8_t thresholdHByte = (settings.fifoThreshold & 0x0F00) >> 8;
adam_z 0:301b2cb34ead 749 //Pedo bits not configured (ctl2)
adam_z 0:301b2cb34ead 750
adam_z 0:301b2cb34ead 751 //CONFIGURE FIFO_CTRL3
adam_z 0:301b2cb34ead 752 uint8_t tempFIFO_CTRL3 = 0;
adam_z 0:301b2cb34ead 753 if (settings.gyroFifoEnabled == 1) {
adam_z 0:301b2cb34ead 754 //Set up gyro stuff
adam_z 0:301b2cb34ead 755 //Build on FIFO_CTRL3
adam_z 0:301b2cb34ead 756 //Set decimation
adam_z 0:301b2cb34ead 757 tempFIFO_CTRL3 |= (settings.gyroFifoDecimation & 0x07) << 3;
adam_z 0:301b2cb34ead 758
adam_z 0:301b2cb34ead 759 }
adam_z 0:301b2cb34ead 760 if (settings.accelFifoEnabled == 1) {
adam_z 0:301b2cb34ead 761 //Set up accelerometer stuff
adam_z 0:301b2cb34ead 762 //Build on FIFO_CTRL3
adam_z 0:301b2cb34ead 763 //Set decimation
adam_z 0:301b2cb34ead 764 tempFIFO_CTRL3 |= (settings.accelFifoDecimation & 0x07);
adam_z 0:301b2cb34ead 765 }
adam_z 0:301b2cb34ead 766
adam_z 0:301b2cb34ead 767 //CONFIGURE FIFO_CTRL4 (nothing for now-- sets data sets 3 and 4
adam_z 0:301b2cb34ead 768 uint8_t tempFIFO_CTRL4 = 0;
adam_z 0:301b2cb34ead 769
adam_z 0:301b2cb34ead 770
adam_z 0:301b2cb34ead 771 //CONFIGURE FIFO_CTRL5
adam_z 0:301b2cb34ead 772 uint8_t tempFIFO_CTRL5 = 0;
adam_z 0:301b2cb34ead 773 switch (settings.fifoSampleRate) {
adam_z 0:301b2cb34ead 774 default: //set default case to 10Hz(slowest)
adam_z 0:301b2cb34ead 775 case 10:
adam_z 0:301b2cb34ead 776 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_10Hz;
adam_z 0:301b2cb34ead 777 break;
adam_z 0:301b2cb34ead 778 case 25:
adam_z 0:301b2cb34ead 779 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_25Hz;
adam_z 0:301b2cb34ead 780 break;
adam_z 0:301b2cb34ead 781 case 50:
adam_z 0:301b2cb34ead 782 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_50Hz;
adam_z 0:301b2cb34ead 783 break;
adam_z 0:301b2cb34ead 784 case 100:
adam_z 0:301b2cb34ead 785 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_100Hz;
adam_z 0:301b2cb34ead 786 break;
adam_z 0:301b2cb34ead 787 case 200:
adam_z 0:301b2cb34ead 788 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_200Hz;
adam_z 0:301b2cb34ead 789 break;
adam_z 0:301b2cb34ead 790 case 400:
adam_z 0:301b2cb34ead 791 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_400Hz;
adam_z 0:301b2cb34ead 792 break;
adam_z 0:301b2cb34ead 793 case 800:
adam_z 0:301b2cb34ead 794 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_800Hz;
adam_z 0:301b2cb34ead 795 break;
adam_z 0:301b2cb34ead 796 case 1600:
adam_z 0:301b2cb34ead 797 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_1600Hz;
adam_z 0:301b2cb34ead 798 break;
adam_z 0:301b2cb34ead 799 case 3300:
adam_z 0:301b2cb34ead 800 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_3300Hz;
adam_z 0:301b2cb34ead 801 break;
adam_z 0:301b2cb34ead 802 case 6600:
adam_z 0:301b2cb34ead 803 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_6600Hz;
adam_z 0:301b2cb34ead 804 break;
adam_z 0:301b2cb34ead 805 }
adam_z 0:301b2cb34ead 806 //Hard code the fifo mode here:
adam_z 0:301b2cb34ead 807 tempFIFO_CTRL5 |= settings.fifoModeWord = 6; //set mode:
adam_z 0:301b2cb34ead 808
adam_z 0:301b2cb34ead 809 //Write the data
adam_z 0:301b2cb34ead 810 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL1, thresholdLByte);
adam_z 0:301b2cb34ead 811 //Serial.println(thresholdLByte, HEX);
adam_z 0:301b2cb34ead 812 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL2, thresholdHByte);
adam_z 0:301b2cb34ead 813 //Serial.println(thresholdHByte, HEX);
adam_z 0:301b2cb34ead 814 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL3, tempFIFO_CTRL3);
adam_z 0:301b2cb34ead 815 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL4, tempFIFO_CTRL4);
adam_z 0:301b2cb34ead 816 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL5, tempFIFO_CTRL5);
adam_z 0:301b2cb34ead 817
adam_z 0:301b2cb34ead 818 }
adam_z 0:301b2cb34ead 819 void LSM6DS3::fifoClear( void )
adam_z 0:301b2cb34ead 820 {
adam_z 0:301b2cb34ead 821 //Drain the fifo data and dump it
adam_z 0:301b2cb34ead 822 while( (fifoGetStatus() & 0x1000 ) == 0 ) {
adam_z 0:301b2cb34ead 823 fifoRead();
adam_z 0:301b2cb34ead 824 }
adam_z 0:301b2cb34ead 825
adam_z 0:301b2cb34ead 826 }
adam_z 0:301b2cb34ead 827 int16_t LSM6DS3::fifoRead( void )
adam_z 0:301b2cb34ead 828 {
adam_z 0:301b2cb34ead 829 //Pull the last data from the fifo
adam_z 0:301b2cb34ead 830 uint8_t tempReadByte = 0;
adam_z 0:301b2cb34ead 831 uint16_t tempAccumulator = 0;
adam_z 0:301b2cb34ead 832 readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_DATA_OUT_L);
adam_z 0:301b2cb34ead 833 tempAccumulator = tempReadByte;
adam_z 0:301b2cb34ead 834 readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_DATA_OUT_H);
adam_z 0:301b2cb34ead 835 tempAccumulator |= ((uint16_t)tempReadByte << 8);
adam_z 0:301b2cb34ead 836
adam_z 0:301b2cb34ead 837 return tempAccumulator;
adam_z 0:301b2cb34ead 838 }
adam_z 0:301b2cb34ead 839
adam_z 0:301b2cb34ead 840 uint16_t LSM6DS3::fifoGetStatus( void )
adam_z 0:301b2cb34ead 841 {
adam_z 0:301b2cb34ead 842 //Return some data on the state of the fifo
adam_z 0:301b2cb34ead 843 uint8_t tempReadByte = 0;
adam_z 0:301b2cb34ead 844 uint16_t tempAccumulator = 0;
adam_z 0:301b2cb34ead 845 readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_STATUS1);
adam_z 0:301b2cb34ead 846 tempAccumulator = tempReadByte;
adam_z 0:301b2cb34ead 847 readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_STATUS2);
adam_z 0:301b2cb34ead 848 tempAccumulator |= (tempReadByte << 8);
adam_z 0:301b2cb34ead 849
adam_z 0:301b2cb34ead 850 return tempAccumulator;
adam_z 0:301b2cb34ead 851
adam_z 0:301b2cb34ead 852 }
adam_z 0:301b2cb34ead 853 void LSM6DS3::fifoEnd( void )
adam_z 0:301b2cb34ead 854 {
adam_z 0:301b2cb34ead 855 // turn off the fifo
adam_z 0:301b2cb34ead 856 writeRegister(LSM6DS3_ACC_GYRO_FIFO_STATUS1, 0x00); //Disable
adam_z 0:301b2cb34ead 857 }