compiled unsuccessful but not sure if it works
Fork of LSM6DS3 by
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?
User | Revision | Line number | New 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 | } |