Pin usage: MOSI(D4), MISO(D5), SCK(D3), CS(D6), format(8, 3) Test OK.
Fork of LSM6DS3 by
LSM6DS3.cpp
00001 /****************************************************************************** 00002 SparkFunLSM6DS3.cpp 00003 LSM6DS3 Arduino and Teensy Driver 00004 00005 Marshall Taylor @ SparkFun Electronics 00006 May 20, 2015 00007 https://github.com/sparkfun/LSM6DS3_Breakout 00008 https://github.com/sparkfun/SparkFun_LSM6DS3_Arduino_Library 00009 00010 Resources: 00011 Uses Wire.h for i2c operation 00012 Uses SPI.h for SPI operation 00013 Either can be omitted if not used 00014 00015 Development environment specifics: 00016 Arduino IDE 1.6.4 00017 Teensy loader 1.23 00018 00019 This code is released under the [MIT License](http://opensource.org/licenses/MIT). 00020 00021 Please review the LICENSE.md file included with this example. If you have any questions 00022 or concerns with licensing, please contact techsupport@sparkfun.com. 00023 00024 Distributed as-is; no warranty is given. 00025 ******************************************************************************/ 00026 00027 //See SparkFunLSM6DS3.h for additional topology notes. 00028 #include "mbed.h" 00029 #include "LSM6DS3.h" 00030 #include "LSM6DS3_Types.h" 00031 #include "LSM6DS3_Registers.h" 00032 #include "stdint.h" 00033 #include "math.h" 00034 00035 //I2C i2c(D14,D15); 00036 //SPI spi(D4,D5,D3); 00037 //****************************************************************************// 00038 // 00039 // LSM6DS3Core functions. 00040 // 00041 // Construction arguments: 00042 // ( uint8_t busType, uint8_t inputArg ), 00043 // 00044 // where inputArg is address for I2C_MODE and chip select pin 00045 // number for SPI_MODE 00046 // 00047 // For SPI, construct LSM6DS3Core myIMU(SPI_MODE, D6); 00048 // For I2C, construct LSM6DS3Core myIMU(I2C_MODE, 0x6B); 00049 // 00050 // Default construction is I2C mode, address 0x6B. 00051 // 00052 00053 //================================= 00054 00055 //================================= 00056 //****************************************************************************// 00057 LSM6DS3Core::LSM6DS3Core( uint8_t busType, uint8_t inputArg) : commInterface(SPI_MODE), I2CAddress(0x6B), spi_(D4,D5,D3), i2c_(I2C_SDA,I2C_SCL), cs_(D6) 00058 { 00059 commInterface = busType; 00060 if( commInterface == I2C_MODE ) { 00061 I2CAddress = inputArg; 00062 } 00063 if( commInterface == SPI_MODE ) { 00064 chipSelectPin = inputArg; 00065 } 00066 00067 00068 } 00069 00070 status_t LSM6DS3Core::beginCore(void) 00071 { 00072 status_t returnError = IMU_SUCCESS; 00073 00074 switch (commInterface) { 00075 00076 case I2C_MODE: 00077 00078 break; 00079 00080 case SPI_MODE: 00081 // start the SPI library: 00082 00083 //DigitalOut cs((PinName)chipSelectPin); 00084 // Maximum SPI frequency is 10MHz, could divide by 2 here: 00085 spi_.frequency(1000000); 00086 // Data is read and written MSb first. 00087 spi_.format(8,0b11); 00088 // Data is captured on rising edge of clock (CPHA = 0) 00089 // Base value of the clock is HIGH (CPOL = 1) 00090 00091 /* // MODE3 for 328p operation 00092 #ifdef __AVR__ 00093 SPI.setDataMode(SPI_MODE3); 00094 #else 00095 #endif 00096 00097 // MODE0 for Teensy 3.1 operation 00098 #ifdef __MK20DX256__ 00099 SPI.setDataMode(SPI_MODE0); 00100 #else 00101 #endif 00102 */ 00103 //SPI.format(8,0);//8 bits mode0 00104 // initalize the data ready and chip select pins: 00105 00106 cs_ = 1; 00107 break; 00108 default: 00109 break; 00110 } 00111 00112 //Spin for a few ms 00113 volatile uint8_t temp = 0; 00114 for( uint16_t i = 0; i < 10000; i++ ) { 00115 temp++; 00116 } 00117 00118 //Check the ID register to determine if the operation was a success. 00119 uint8_t readCheck; 00120 readRegister(&readCheck, LSM6DS3_ACC_GYRO_WHO_AM_I_REG); 00121 if( readCheck != 0x69 ) { 00122 returnError = IMU_HW_ERROR; 00123 } 00124 00125 return returnError; 00126 00127 } 00128 00129 //****************************************************************************// 00130 // 00131 // ReadRegisterRegion 00132 // 00133 // Parameters: 00134 // *outputPointer -- Pass &variable (base address of) to save read data to 00135 // offset -- register to read 00136 // length -- number of bytes to read 00137 // 00138 // Note: Does not know if the target memory space is an array or not, or 00139 // if there is the array is big enough. if the variable passed is only 00140 // two bytes long and 3 bytes are requested, this will over-write some 00141 // other memory! 00142 // 00143 //****************************************************************************// 00144 status_t LSM6DS3Core::readRegisterRegion(uint8_t *outputPointer , uint8_t offset, uint8_t length) 00145 { 00146 status_t returnError = IMU_SUCCESS; 00147 00148 //define pointer that will point to the external space 00149 uint8_t i = 0; 00150 uint8_t c = 0; 00151 uint8_t tempFFCounter = 0; 00152 00153 switch (commInterface) { 00154 00155 case I2C_MODE: 00156 /* 00157 i2c.write(offset); 00158 if( i2c.stop() != 0 ) { 00159 returnError = IMU_HW_ERROR; 00160 } else { //OK, all worked, keep going 00161 // request 6 bytes from slave device 00162 Wire.requestFrom(I2CAddress, length); 00163 while ( (Wire.available()) && (i < length)) { // slave may send less than requested 00164 c = Wire.read(); // receive a byte as character 00165 *outputPointer = c; 00166 outputPointer++; 00167 i++; 00168 } 00169 } 00170 */ 00171 break; 00172 00173 case SPI_MODE: 00174 // take the chip select low to select the device: 00175 cs_ = 0; 00176 // send the device the register you want to read: 00177 spi_.write(offset | 0x80); //Ored with "read request" bit 00178 while ( i < length ) { // slave may send less than requested 00179 c = spi_.write(0x00); // receive a byte as character 00180 if( c == 0xFF ) { 00181 //May have problem 00182 tempFFCounter++; 00183 } 00184 *outputPointer = c; 00185 outputPointer++; 00186 i++; 00187 } 00188 if( tempFFCounter == i ) { 00189 //Ok, we've recieved all ones, report 00190 returnError = IMU_ALL_ONES_WARNING; 00191 } 00192 // take the chip select high to de-select: 00193 cs_ = 1; //digitalWrite(chipSelectPin, HIGH); 00194 break; 00195 00196 default: 00197 break; 00198 } 00199 00200 return returnError; 00201 } 00202 00203 //****************************************************************************// 00204 // 00205 // ReadRegister 00206 // 00207 // Parameters: 00208 // *outputPointer -- Pass &variable (address of) to save read data to 00209 // offset -- register to read 00210 // 00211 //****************************************************************************// 00212 status_t LSM6DS3Core::readRegister(uint8_t* outputPointer, uint8_t offset) 00213 { 00214 //Return value 00215 uint8_t result; 00216 //uint8_t numBytes = 1; 00217 status_t returnError = IMU_SUCCESS; 00218 00219 switch (commInterface) { 00220 00221 case I2C_MODE: 00222 /* Wire.beginTransmission(I2CAddress); 00223 Wire.write(offset); 00224 if( Wire.endTransmission() != 0 ) { 00225 returnError = IMU_HW_ERROR; 00226 } 00227 Wire.requestFrom(I2CAddress, numBytes); 00228 while ( Wire.available() ) { // slave may send less than requested 00229 result = Wire.read(); // receive a byte as a proper uint8_t 00230 } 00231 */ 00232 break; 00233 00234 case SPI_MODE: 00235 // take the chip select low to select the device: 00236 cs_ = 0; //digitalWrite(chipSelectPin, LOW); 00237 // send the device the register you want to read: 00238 spi_.write(offset | 0x80); //Ored with "read request" bit 00239 // send a value of 0 to read the first byte returned: 00240 result = spi_.write(0x00); 00241 // take the chip select high to de-select: 00242 cs_ = 1; //digitalWrite(chipSelectPin, HIGH); 00243 00244 if( result == 0xFF ) { 00245 //we've recieved all ones, report 00246 returnError = IMU_ALL_ONES_WARNING; 00247 } 00248 break; 00249 00250 default: 00251 break; 00252 } 00253 00254 *outputPointer = result; 00255 return returnError; 00256 } 00257 00258 //****************************************************************************// 00259 // 00260 // readRegisterInt16 00261 // 00262 // Parameters: 00263 // *outputPointer -- Pass &variable (base address of) to save read data to 00264 // offset -- register to read 00265 // 00266 //****************************************************************************// 00267 status_t LSM6DS3Core::readRegisterInt16( int16_t* outputPointer, uint8_t offset ) 00268 { 00269 uint8_t myBuffer[2]; 00270 status_t returnError = readRegisterRegion(myBuffer, offset, 2); //Does memory transfer 00271 int16_t output = (int16_t)myBuffer[0] | int16_t(myBuffer[1] << 8); 00272 00273 *outputPointer = output; 00274 return returnError; 00275 } 00276 00277 //****************************************************************************// 00278 // 00279 // writeRegister 00280 // 00281 // Parameters: 00282 // offset -- register to write 00283 // dataToWrite -- 8 bit data to write to register 00284 // 00285 //****************************************************************************// 00286 status_t LSM6DS3Core::writeRegister(uint8_t offset, uint8_t dataToWrite) 00287 { 00288 status_t returnError = IMU_SUCCESS; 00289 switch (commInterface) { 00290 case I2C_MODE: 00291 /* //Write the byte 00292 Wire.beginTransmission(I2CAddress); 00293 Wire.write(offset); 00294 Wire.write(dataToWrite); 00295 if( Wire.endTransmission() != 0 ) { 00296 returnError = IMU_HW_ERROR; 00297 } 00298 */ break; 00299 00300 case SPI_MODE: 00301 // take the chip select low to select the device: 00302 cs_ = 0; //digitalWrite(chipSelectPin, LOW); 00303 // send the device the register you want to read: 00304 spi_.write(offset | 0x00); 00305 // send a value of 0 to read the first byte returned: 00306 spi_.write(dataToWrite); 00307 // decrement the number of bytes left to read: 00308 // take the chip select high to de-select: 00309 cs_ = 1;//digitalWrite(chipSelectPin, HIGH); 00310 break; 00311 00312 //No way to check error on this write (Except to read back but that's not reliable) 00313 00314 default: 00315 break; 00316 } 00317 00318 return returnError; 00319 } 00320 00321 status_t LSM6DS3Core::embeddedPage( void ) 00322 { 00323 status_t returnError = writeRegister( LSM6DS3_ACC_GYRO_RAM_ACCESS, 0x80 ); 00324 00325 return returnError; 00326 } 00327 00328 status_t LSM6DS3Core::basePage( void ) 00329 { 00330 status_t returnError = writeRegister( LSM6DS3_ACC_GYRO_RAM_ACCESS, 0x00 ); 00331 00332 return returnError; 00333 } 00334 00335 00336 //****************************************************************************// 00337 // 00338 // Main user class -- wrapper for the core class + maths 00339 // 00340 // Construct with same rules as the core ( uint8_t busType, uint8_t inputArg ) 00341 // 00342 //****************************************************************************// 00343 LSM6DS3::LSM6DS3( uint8_t busType, uint8_t inputArg ) : LSM6DS3Core( busType, inputArg ) 00344 { 00345 //Construct with these default settings 00346 00347 settings.gyroEnabled = 1; //Can be 0 or 1 00348 settings.gyroRange = 2000; //Max deg/s. Can be: 125, 245, 500, 1000, 2000 00349 settings.gyroSampleRate = 833; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666 00350 settings.gyroBandWidth = 400; //Hz. Can be: 50, 100, 200, 400; 00351 settings.gyroFifoEnabled = 0; //Set to include gyro in FIFO 00352 settings.gyroFifoDecimation = 1; //set 1 for on /1 00353 00354 settings.accelEnabled = 1; 00355 settings.accelODROff = 1; //Accelerometer bandwidth selection. 0:determined by ODR, 1:determined by BW_XL 00356 settings.accelRange = 8; //Max G force readable. Can be: 2, 4, 8, 16 00357 settings.accelSampleRate = 833; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666, 3332, 6664, 13330 00358 settings.accelBandWidth = 400; //Hz. Can be: 50, 100, 200, 400; 00359 settings.accelFifoEnabled = 0; //Set to include accelerometer in the FIFO 00360 settings.accelFifoDecimation = 1; //set 1 for on /1 00361 00362 settings.tempEnabled = 0; 00363 00364 //Select interface mode 00365 settings.commMode = 1; //Can be modes 1, 2 or 3 00366 00367 //FIFO control data 00368 settings.fifoThreshold = 3000; //Can be 0 to 4096 (16 bit bytes) 00369 settings.fifoSampleRate = 10; //default 10Hz 00370 settings.fifoModeWord = 0; //Default off 00371 00372 allOnesCounter = 0; 00373 nonSuccessCounter = 0; 00374 00375 } 00376 00377 //****************************************************************************// 00378 // 00379 // Configuration section 00380 // 00381 // This uses the stored SensorSettings to start the IMU 00382 // Use statements such as "myIMU.settings.commInterface = SPI_MODE;" or 00383 // "myIMU.settings.accelEnabled = 1;" to configure before calling .begin(); 00384 // 00385 //****************************************************************************// 00386 status_t LSM6DS3::begin() 00387 { 00388 //Check the settings structure values to determine how to setup the device 00389 uint8_t dataToWrite = 0; //Temporary variable 00390 00391 //Begin the inherited core. This gets the physical wires connected 00392 status_t returnError = beginCore(); 00393 00394 //setOffset(-61, -25, -66, 35, -81, -32); 00395 00396 //Setup the accelerometer****************************** 00397 dataToWrite = 0; //Start Fresh! 00398 if ( settings.accelEnabled == 1) { 00399 //Build config reg 00400 //First patch in filter bandwidth 00401 switch (settings.accelBandWidth) { 00402 case 50: 00403 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_50Hz; 00404 break; 00405 case 100: 00406 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_100Hz; 00407 break; 00408 case 200: 00409 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_200Hz; 00410 break; 00411 default: //set default case to max passthrough 00412 case 400: 00413 dataToWrite |= LSM6DS3_ACC_GYRO_BW_XL_400Hz; 00414 break; 00415 } 00416 //Next, patch in full scale 00417 switch (settings.accelRange) { 00418 case 2: 00419 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_2g; 00420 break; 00421 case 4: 00422 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_4g; 00423 break; 00424 case 8: 00425 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_8g; 00426 break; 00427 default: //set default case to 16(max) 00428 case 16: 00429 dataToWrite |= LSM6DS3_ACC_GYRO_FS_XL_16g; 00430 break; 00431 } 00432 //Lastly, patch in accelerometer ODR 00433 switch (settings.accelSampleRate) { 00434 case 13: 00435 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_13Hz; 00436 break; 00437 case 26: 00438 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_26Hz; 00439 break; 00440 case 52: 00441 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_52Hz; 00442 break; 00443 default: //Set default to 104 00444 case 104: 00445 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_104Hz; 00446 break; 00447 case 208: 00448 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_208Hz; 00449 break; 00450 case 416: 00451 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_416Hz; 00452 break; 00453 case 833: 00454 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_833Hz; 00455 break; 00456 case 1660: 00457 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_1660Hz; 00458 break; 00459 case 3330: 00460 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_3330Hz; 00461 break; 00462 case 6660: 00463 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_6660Hz; 00464 break; 00465 case 13330: 00466 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_XL_13330Hz; 00467 break; 00468 } 00469 } else { 00470 //dataToWrite already = 0 (powerdown); 00471 } 00472 00473 //Now, write the patched together data 00474 writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, dataToWrite); 00475 00476 //Set the ODR bit 00477 readRegister(&dataToWrite, LSM6DS3_ACC_GYRO_CTRL4_C); 00478 dataToWrite &= ~((uint8_t)LSM6DS3_ACC_GYRO_BW_SCAL_ODR_ENABLED); 00479 if ( settings.accelODROff == 1) { 00480 dataToWrite |= LSM6DS3_ACC_GYRO_BW_SCAL_ODR_ENABLED; 00481 } 00482 writeRegister(LSM6DS3_ACC_GYRO_CTRL4_C, dataToWrite); 00483 00484 //Setup the gyroscope********************************************** 00485 dataToWrite = 0; //Start Fresh! 00486 if ( settings.gyroEnabled == 1) { 00487 //Build config reg 00488 //First, patch in full scale 00489 switch (settings.gyroRange) { 00490 case 125: 00491 dataToWrite |= LSM6DS3_ACC_GYRO_FS_125_ENABLED; 00492 break; 00493 case 245: 00494 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_245dps; 00495 break; 00496 case 500: 00497 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_500dps; 00498 break; 00499 case 1000: 00500 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_1000dps; 00501 break; 00502 default: //Default to full 2000DPS range 00503 case 2000: 00504 dataToWrite |= LSM6DS3_ACC_GYRO_FS_G_2000dps; 00505 break; 00506 } 00507 //Lastly, patch in gyro ODR 00508 switch (settings.gyroSampleRate) { 00509 case 13: 00510 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_13Hz; 00511 break; 00512 case 26: 00513 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_26Hz; 00514 break; 00515 case 52: 00516 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_52Hz; 00517 break; 00518 default: //Set default to 104 00519 case 104: 00520 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_104Hz; 00521 break; 00522 case 208: 00523 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_208Hz; 00524 break; 00525 case 416: 00526 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_416Hz; 00527 break; 00528 case 833: 00529 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_833Hz; 00530 break; 00531 case 1660: 00532 dataToWrite |= LSM6DS3_ACC_GYRO_ODR_G_1660Hz; 00533 break; 00534 } 00535 } else { 00536 //dataToWrite already = 0 (powerdown); 00537 } 00538 //Write the byte 00539 writeRegister(LSM6DS3_ACC_GYRO_CTRL2_G, dataToWrite); 00540 00541 //Setup the internal temperature sensor 00542 if ( settings.tempEnabled == 1) { 00543 } 00544 00545 //Return WHO AM I reg //Not no mo! 00546 uint8_t result; 00547 readRegister(&result, LSM6DS3_ACC_GYRO_WHO_AM_I_REG); 00548 00549 return returnError; 00550 } 00551 00552 //****************************************************************************// 00553 // 00554 // Accelerometer section 00555 // 00556 //****************************************************************************// 00557 00558 void LSM6DS3::setOffset(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz) 00559 { 00560 accelOffset[0] = ax; 00561 accelOffset[1] = ay; 00562 accelOffset[2] = az; 00563 gyroOffset[0] = gx; 00564 gyroOffset[1] = gy; 00565 gyroOffset[2] = gz; 00566 } 00567 00568 int16_t LSM6DS3::readRawAccelX( void ) 00569 { 00570 int16_t output; 00571 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTX_L_XL ); 00572 if( errorLevel != IMU_SUCCESS ) { 00573 if( errorLevel == IMU_ALL_ONES_WARNING ) { 00574 allOnesCounter++; 00575 } else { 00576 nonSuccessCounter++; 00577 } 00578 } 00579 return output; 00580 } 00581 float LSM6DS3::readFloatAccelX( void ) 00582 { 00583 float output = calcAccel((int32_t)readRawAccelX() - (int32_t)accelOffset[0]); 00584 return output; 00585 } 00586 00587 int16_t LSM6DS3::readRawAccelY( void ) 00588 { 00589 int16_t output; 00590 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTY_L_XL ); 00591 if( errorLevel != IMU_SUCCESS ) { 00592 if( errorLevel == IMU_ALL_ONES_WARNING ) { 00593 allOnesCounter++; 00594 } else { 00595 nonSuccessCounter++; 00596 } 00597 } 00598 return output; 00599 } 00600 float LSM6DS3::readFloatAccelY( void ) 00601 { 00602 float output = calcAccel((int32_t)readRawAccelY() - (int32_t)accelOffset[1]); 00603 return output; 00604 } 00605 00606 int16_t LSM6DS3::readRawAccelZ( void ) 00607 { 00608 int16_t output; 00609 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTZ_L_XL ); 00610 if( errorLevel != IMU_SUCCESS ) { 00611 if( errorLevel == IMU_ALL_ONES_WARNING ) { 00612 allOnesCounter++; 00613 } else { 00614 nonSuccessCounter++; 00615 } 00616 } 00617 return output; 00618 } 00619 float LSM6DS3::readFloatAccelZ( void ) 00620 { 00621 float output = calcAccel((int32_t)readRawAccelZ() - (int32_t)accelOffset[2]); 00622 return output; 00623 } 00624 00625 float LSM6DS3::calcAccel( int32_t input ) 00626 { 00627 float output = (float)input * 0.061f * (settings.accelRange >> 1) / 1000.0f; 00628 return output; 00629 } 00630 00631 //****************************************************************************// 00632 // 00633 // Gyroscope section 00634 // 00635 //****************************************************************************// 00636 int16_t LSM6DS3::readRawGyroX( void ) 00637 { 00638 int16_t output; 00639 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTX_L_G ); 00640 if( errorLevel != IMU_SUCCESS ) { 00641 if( errorLevel == IMU_ALL_ONES_WARNING ) { 00642 allOnesCounter++; 00643 } else { 00644 nonSuccessCounter++; 00645 } 00646 } 00647 return output; 00648 } 00649 float LSM6DS3::readFloatGyroX( void ) 00650 { 00651 float output = calcGyro((int32_t)readRawGyroX() - (int32_t)gyroOffset[0]); 00652 return output; 00653 } 00654 00655 int16_t LSM6DS3::readRawGyroY( void ) 00656 { 00657 int16_t output; 00658 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTY_L_G ); 00659 if( errorLevel != IMU_SUCCESS ) { 00660 if( errorLevel == IMU_ALL_ONES_WARNING ) { 00661 allOnesCounter++; 00662 } else { 00663 nonSuccessCounter++; 00664 } 00665 } 00666 return output; 00667 } 00668 float LSM6DS3::readFloatGyroY( void ) 00669 { 00670 float output = calcGyro((int32_t)readRawGyroY() - (int32_t)gyroOffset[1]); 00671 return output; 00672 } 00673 00674 int16_t LSM6DS3::readRawGyroZ( void ) 00675 { 00676 int16_t output; 00677 status_t errorLevel = readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUTZ_L_G ); 00678 if( errorLevel != IMU_SUCCESS ) { 00679 if( errorLevel == IMU_ALL_ONES_WARNING ) { 00680 allOnesCounter++; 00681 } else { 00682 nonSuccessCounter++; 00683 } 00684 } 00685 return output; 00686 } 00687 float LSM6DS3::readFloatGyroZ( void ) 00688 { 00689 float output = calcGyro((int32_t)readRawGyroZ() - (int32_t)gyroOffset[2]); 00690 return output; 00691 } 00692 00693 float LSM6DS3::calcGyro( int32_t input ) 00694 { 00695 uint8_t gyroRangeDivisor = settings.gyroRange / 125; 00696 if ( settings.gyroRange == 245 ) { 00697 gyroRangeDivisor = 2; 00698 } 00699 00700 float output = (float)input * 4.375f * (gyroRangeDivisor) / 1000.0f; 00701 return output; 00702 } 00703 00704 //****************************************************************************// 00705 // 00706 // Temperature section 00707 // 00708 //****************************************************************************// 00709 int16_t LSM6DS3::readRawTemp( void ) 00710 { 00711 int16_t output; 00712 readRegisterInt16( &output, LSM6DS3_ACC_GYRO_OUT_TEMP_L ); 00713 return output; 00714 } 00715 00716 float LSM6DS3::readTempC( void ) 00717 { 00718 float output = (float)readRawTemp() / 16; //divide by 16 to scale 00719 output += 25; //Add 25 degrees to remove offset 00720 00721 return output; 00722 00723 } 00724 00725 float LSM6DS3::readTempF( void ) 00726 { 00727 float output = (float)readRawTemp() / 16; //divide by 16 to scale 00728 output += 25; //Add 25 degrees to remove offset 00729 output = (output * 9) / 5 + 32; 00730 00731 return output; 00732 00733 } 00734 00735 //****************************************************************************// 00736 // 00737 // FIFO section 00738 // 00739 //****************************************************************************// 00740 void LSM6DS3::fifoBegin( void ) 00741 { 00742 //CONFIGURE THE VARIOUS FIFO SETTINGS 00743 // 00744 // 00745 //This section first builds a bunch of config words, then goes through 00746 //and writes them all. 00747 00748 //Split and mask the threshold 00749 uint8_t thresholdLByte = settings.fifoThreshold & 0x00FF; 00750 uint8_t thresholdHByte = (settings.fifoThreshold & 0x0F00) >> 8; 00751 //Pedo bits not configured (ctl2) 00752 00753 //CONFIGURE FIFO_CTRL3 00754 uint8_t tempFIFO_CTRL3 = 0; 00755 if (settings.gyroFifoEnabled == 1) { 00756 //Set up gyro stuff 00757 //Build on FIFO_CTRL3 00758 //Set decimation 00759 tempFIFO_CTRL3 |= (settings.gyroFifoDecimation & 0x07) << 3; 00760 00761 } 00762 if (settings.accelFifoEnabled == 1) { 00763 //Set up accelerometer stuff 00764 //Build on FIFO_CTRL3 00765 //Set decimation 00766 tempFIFO_CTRL3 |= (settings.accelFifoDecimation & 0x07); 00767 } 00768 00769 //CONFIGURE FIFO_CTRL4 (nothing for now-- sets data sets 3 and 4 00770 uint8_t tempFIFO_CTRL4 = 0; 00771 00772 00773 //CONFIGURE FIFO_CTRL5 00774 uint8_t tempFIFO_CTRL5 = 0; 00775 switch (settings.fifoSampleRate) { 00776 default: //set default case to 10Hz(slowest) 00777 case 10: 00778 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_10Hz; 00779 break; 00780 case 25: 00781 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_25Hz; 00782 break; 00783 case 50: 00784 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_50Hz; 00785 break; 00786 case 100: 00787 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_100Hz; 00788 break; 00789 case 200: 00790 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_200Hz; 00791 break; 00792 case 400: 00793 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_400Hz; 00794 break; 00795 case 800: 00796 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_800Hz; 00797 break; 00798 case 1600: 00799 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_1600Hz; 00800 break; 00801 case 3300: 00802 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_3300Hz; 00803 break; 00804 case 6600: 00805 tempFIFO_CTRL5 |= LSM6DS3_ACC_GYRO_ODR_FIFO_6600Hz; 00806 break; 00807 } 00808 //Hard code the fifo mode here: 00809 tempFIFO_CTRL5 |= settings.fifoModeWord = 6; //set mode: 00810 00811 //Write the data 00812 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL1, thresholdLByte); 00813 //Serial.println(thresholdLByte, HEX); 00814 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL2, thresholdHByte); 00815 //Serial.println(thresholdHByte, HEX); 00816 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL3, tempFIFO_CTRL3); 00817 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL4, tempFIFO_CTRL4); 00818 writeRegister(LSM6DS3_ACC_GYRO_FIFO_CTRL5, tempFIFO_CTRL5); 00819 00820 } 00821 void LSM6DS3::fifoClear( void ) 00822 { 00823 //Drain the fifo data and dump it 00824 while( (fifoGetStatus() & 0x1000 ) == 0 ) { 00825 fifoRead(); 00826 } 00827 00828 } 00829 int16_t LSM6DS3::fifoRead( void ) 00830 { 00831 //Pull the last data from the fifo 00832 uint8_t tempReadByte = 0; 00833 uint16_t tempAccumulator = 0; 00834 readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_DATA_OUT_L); 00835 tempAccumulator = tempReadByte; 00836 readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_DATA_OUT_H); 00837 tempAccumulator |= ((uint16_t)tempReadByte << 8); 00838 00839 return tempAccumulator; 00840 } 00841 00842 uint16_t LSM6DS3::fifoGetStatus( void ) 00843 { 00844 //Return some data on the state of the fifo 00845 uint8_t tempReadByte = 0; 00846 uint16_t tempAccumulator = 0; 00847 readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_STATUS1); 00848 tempAccumulator = tempReadByte; 00849 readRegister(&tempReadByte, LSM6DS3_ACC_GYRO_FIFO_STATUS2); 00850 tempAccumulator |= (tempReadByte << 8); 00851 00852 return tempAccumulator; 00853 00854 } 00855 void LSM6DS3::fifoEnd( void ) 00856 { 00857 // turn off the fifo 00858 writeRegister(LSM6DS3_ACC_GYRO_FIFO_STATUS1, 0x00); //Disable 00859 }
Generated on Wed Jul 13 2022 10:08:35 by
1.7.2
