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