Alton Stovall
debug
Dependencies: mbed mbed-rtos PinDetect X_NUCLEO_53L0A1
Embed:
(wiki syntax)
Show/hide line numbers
LSM9DS1.cpp
00001 /****************************************************************************** 00002 SFE_LSM9DS1.cpp 00003 SFE_LSM9DS1 Library Source File 00004 Jim Lindblom @ SparkFun Electronics 00005 Original Creation Date: February 27, 2015 00006 https://github.com/sparkfun/LSM9DS1_Breakout 00007 00008 This file implements all functions of the LSM9DS1 class. Functions here range 00009 from higher level stuff, like reading/writing LSM9DS1 registers to low-level, 00010 hardware reads and writes. Both SPI and I2C handler functions can be found 00011 towards the bottom of this file. 00012 00013 Development environment specifics: 00014 IDE: Arduino 1.6 00015 Hardware Platform: Arduino Uno 00016 LSM9DS1 Breakout Version: 1.0 00017 00018 This code is beerware; if you see me (or any other SparkFun employee) at the 00019 local, and you've found our code helpful, please buy us a round! 00020 00021 Distributed as-is; no warranty is given. 00022 ******************************************************************************/ 00023 00024 #include "LSM9DS1.h" 00025 #include "LSM9DS1_Registers.h" 00026 #include "LSM9DS1_Types.h" 00027 //#include <Wire.h> // Wire library is used for I2C 00028 //#include <SPI.h> // SPI library is used for...SPI. 00029 00030 //#if defined(ARDUINO) && ARDUINO >= 100 00031 // #include "Arduino.h" 00032 //#else 00033 // #include "WProgram.h" 00034 //#endif 00035 00036 #define LSM9DS1_COMMUNICATION_TIMEOUT 1000 00037 00038 float magSensitivity[4] = {0.00014, 0.00029, 0.00043, 0.00058}; 00039 extern Serial pc; 00040 00041 LSM9DS1::LSM9DS1(PinName sda, PinName scl, uint8_t xgAddr, uint8_t mAddr) 00042 :i2c(sda, scl) 00043 { 00044 init(IMU_MODE_I2C, xgAddr, mAddr); // dont know about 0xD6 or 0x3B 00045 } 00046 /* 00047 LSM9DS1::LSM9DS1() 00048 { 00049 init(IMU_MODE_I2C, LSM9DS1_AG_ADDR(1), LSM9DS1_M_ADDR(1)); 00050 } 00051 00052 LSM9DS1::LSM9DS1(interface_mode interface, uint8_t xgAddr, uint8_t mAddr) 00053 { 00054 init(interface, xgAddr, mAddr); 00055 } 00056 */ 00057 00058 void LSM9DS1::init(interface_mode interface, uint8_t xgAddr, uint8_t mAddr) 00059 { 00060 settings.device.commInterface = interface; 00061 settings.device.agAddress = xgAddr; 00062 settings.device.mAddress = mAddr; 00063 00064 settings.gyro.enabled = true; 00065 settings.gyro.enableX = true; 00066 settings.gyro.enableY = true; 00067 settings.gyro.enableZ = true; 00068 // gyro scale can be 245, 500, or 2000 00069 settings.gyro.scale = 245; 00070 // gyro sample rate: value between 1-6 00071 // 1 = 14.9 4 = 238 00072 // 2 = 59.5 5 = 476 00073 // 3 = 119 6 = 952 00074 settings.gyro.sampleRate = 6; 00075 // gyro cutoff frequency: value between 0-3 00076 // Actual value of cutoff frequency depends 00077 // on sample rate. 00078 settings.gyro.bandwidth = 0; 00079 settings.gyro.lowPowerEnable = false; 00080 settings.gyro.HPFEnable = false; 00081 // Gyro HPF cutoff frequency: value between 0-9 00082 // Actual value depends on sample rate. Only applies 00083 // if gyroHPFEnable is true. 00084 settings.gyro.HPFCutoff = 0; 00085 settings.gyro.flipX = false; 00086 settings.gyro.flipY = false; 00087 settings.gyro.flipZ = false; 00088 settings.gyro.orientation = 0; 00089 settings.gyro.latchInterrupt = true; 00090 00091 settings.accel.enabled = true; 00092 settings.accel.enableX = true; 00093 settings.accel.enableY = true; 00094 settings.accel.enableZ = true; 00095 // accel scale can be 2, 4, 8, or 16 00096 settings.accel.scale = 2; 00097 // accel sample rate can be 1-6 00098 // 1 = 10 Hz 4 = 238 Hz 00099 // 2 = 50 Hz 5 = 476 Hz 00100 // 3 = 119 Hz 6 = 952 Hz 00101 settings.accel.sampleRate = 6; 00102 // Accel cutoff freqeuncy can be any value between -1 - 3. 00103 // -1 = bandwidth determined by sample rate 00104 // 0 = 408 Hz 2 = 105 Hz 00105 // 1 = 211 Hz 3 = 50 Hz 00106 settings.accel.bandwidth = -1; 00107 settings.accel.highResEnable = false; 00108 // accelHighResBandwidth can be any value between 0-3 00109 // LP cutoff is set to a factor of sample rate 00110 // 0 = ODR/50 2 = ODR/9 00111 // 1 = ODR/100 3 = ODR/400 00112 settings.accel.highResBandwidth = 0; 00113 00114 settings.mag.enabled = true; 00115 // mag scale can be 4, 8, 12, or 16 00116 settings.mag.scale = 4; 00117 // mag data rate can be 0-7 00118 // 0 = 0.625 Hz 4 = 10 Hz 00119 // 1 = 1.25 Hz 5 = 20 Hz 00120 // 2 = 2.5 Hz 6 = 40 Hz 00121 // 3 = 5 Hz 7 = 80 Hz 00122 settings.mag.sampleRate = 7; 00123 settings.mag.tempCompensationEnable = false; 00124 // magPerformance can be any value between 0-3 00125 // 0 = Low power mode 2 = high performance 00126 // 1 = medium performance 3 = ultra-high performance 00127 settings.mag.XYPerformance = 3; 00128 settings.mag.ZPerformance = 3; 00129 settings.mag.lowPowerEnable = false; 00130 // magOperatingMode can be 0-2 00131 // 0 = continuous conversion 00132 // 1 = single-conversion 00133 // 2 = power down 00134 settings.mag.operatingMode = 0; 00135 00136 settings.temp.enabled = true; 00137 for (int i=0; i<3; i++) 00138 { 00139 gBias[i] = 0; 00140 aBias[i] = 0; 00141 mBias[i] = 0; 00142 gBiasRaw[i] = 0; 00143 aBiasRaw[i] = 0; 00144 mBiasRaw[i] = 0; 00145 } 00146 _autoCalc = false; 00147 } 00148 00149 00150 uint16_t LSM9DS1::begin() 00151 { 00152 //! Todo: don't use _xgAddress or _mAddress, duplicating memory 00153 _xgAddress = settings.device.agAddress; 00154 _mAddress = settings.device.mAddress; 00155 00156 constrainScales(); 00157 // Once we have the scale values, we can calculate the resolution 00158 // of each sensor. That's what these functions are for. One for each sensor 00159 calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable 00160 calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable 00161 calcaRes(); // Calculate g / ADC tick, stored in aRes variable 00162 00163 // Now, initialize our hardware interface. 00164 if (settings.device.commInterface == IMU_MODE_I2C) // If we're using I2C 00165 initI2C(); // Initialize I2C 00166 else if (settings.device.commInterface == IMU_MODE_SPI) // else, if we're using SPI 00167 initSPI(); // Initialize SPI 00168 00169 // To verify communication, we can read from the WHO_AM_I register of 00170 // each device. Store those in a variable so we can return them. 00171 uint8_t mTest = mReadByte(WHO_AM_I_M); // Read the gyro WHO_AM_I 00172 uint8_t xgTest = xgReadByte(WHO_AM_I_XG); // Read the accel/mag WHO_AM_I 00173 pc.printf("%x, %x, %x, %x\n\r", mTest, xgTest, _xgAddress, _mAddress); 00174 uint16_t whoAmICombined = (xgTest << 8) | mTest; 00175 00176 if (whoAmICombined != ((WHO_AM_I_AG_RSP << 8) | WHO_AM_I_M_RSP)) 00177 return 0; 00178 00179 // Gyro initialization stuff: 00180 initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc. 00181 00182 // Accelerometer initialization stuff: 00183 initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc. 00184 00185 // Magnetometer initialization stuff: 00186 initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc. 00187 00188 // Once everything is initialized, return the WHO_AM_I registers we read: 00189 return whoAmICombined; 00190 } 00191 00192 void LSM9DS1::initGyro() 00193 { 00194 uint8_t tempRegValue = 0; 00195 00196 // CTRL_REG1_G (Default value: 0x00) 00197 // [ODR_G2][ODR_G1][ODR_G0][FS_G1][FS_G0][0][BW_G1][BW_G0] 00198 // ODR_G[2:0] - Output data rate selection 00199 // FS_G[1:0] - Gyroscope full-scale selection 00200 // BW_G[1:0] - Gyroscope bandwidth selection 00201 00202 // To disable gyro, set sample rate bits to 0. We'll only set sample 00203 // rate if the gyro is enabled. 00204 if (settings.gyro.enabled) 00205 { 00206 tempRegValue = (settings.gyro.sampleRate & 0x07) << 5; 00207 } 00208 switch (settings.gyro.scale) 00209 { 00210 case 500: 00211 tempRegValue |= (0x1 << 3); 00212 break; 00213 case 2000: 00214 tempRegValue |= (0x3 << 3); 00215 break; 00216 // Otherwise we'll set it to 245 dps (0x0 << 4) 00217 } 00218 tempRegValue |= (settings.gyro.bandwidth & 0x3); 00219 xgWriteByte(CTRL_REG1_G, tempRegValue); 00220 00221 // CTRL_REG2_G (Default value: 0x00) 00222 // [0][0][0][0][INT_SEL1][INT_SEL0][OUT_SEL1][OUT_SEL0] 00223 // INT_SEL[1:0] - INT selection configuration 00224 // OUT_SEL[1:0] - Out selection configuration 00225 xgWriteByte(CTRL_REG2_G, 0x00); 00226 00227 // CTRL_REG3_G (Default value: 0x00) 00228 // [LP_mode][HP_EN][0][0][HPCF3_G][HPCF2_G][HPCF1_G][HPCF0_G] 00229 // LP_mode - Low-power mode enable (0: disabled, 1: enabled) 00230 // HP_EN - HPF enable (0:disabled, 1: enabled) 00231 // HPCF_G[3:0] - HPF cutoff frequency 00232 tempRegValue = settings.gyro.lowPowerEnable ? (1<<7) : 0; 00233 if (settings.gyro.HPFEnable) 00234 { 00235 tempRegValue |= (1<<6) | (settings.gyro.HPFCutoff & 0x0F); 00236 } 00237 xgWriteByte(CTRL_REG3_G, tempRegValue); 00238 00239 // CTRL_REG4 (Default value: 0x38) 00240 // [0][0][Zen_G][Yen_G][Xen_G][0][LIR_XL1][4D_XL1] 00241 // Zen_G - Z-axis output enable (0:disable, 1:enable) 00242 // Yen_G - Y-axis output enable (0:disable, 1:enable) 00243 // Xen_G - X-axis output enable (0:disable, 1:enable) 00244 // LIR_XL1 - Latched interrupt (0:not latched, 1:latched) 00245 // 4D_XL1 - 4D option on interrupt (0:6D used, 1:4D used) 00246 tempRegValue = 0; 00247 if (settings.gyro.enableZ) tempRegValue |= (1<<5); 00248 if (settings.gyro.enableY) tempRegValue |= (1<<4); 00249 if (settings.gyro.enableX) tempRegValue |= (1<<3); 00250 if (settings.gyro.latchInterrupt) tempRegValue |= (1<<1); 00251 xgWriteByte(CTRL_REG4, tempRegValue); 00252 00253 // ORIENT_CFG_G (Default value: 0x00) 00254 // [0][0][SignX_G][SignY_G][SignZ_G][Orient_2][Orient_1][Orient_0] 00255 // SignX_G - Pitch axis (X) angular rate sign (0: positive, 1: negative) 00256 // Orient [2:0] - Directional user orientation selection 00257 tempRegValue = 0; 00258 if (settings.gyro.flipX) tempRegValue |= (1<<5); 00259 if (settings.gyro.flipY) tempRegValue |= (1<<4); 00260 if (settings.gyro.flipZ) tempRegValue |= (1<<3); 00261 xgWriteByte(ORIENT_CFG_G, tempRegValue); 00262 } 00263 00264 void LSM9DS1::initAccel() 00265 { 00266 uint8_t tempRegValue = 0; 00267 00268 // CTRL_REG5_XL (0x1F) (Default value: 0x38) 00269 // [DEC_1][DEC_0][Zen_XL][Yen_XL][Zen_XL][0][0][0] 00270 // DEC[0:1] - Decimation of accel data on OUT REG and FIFO. 00271 // 00: None, 01: 2 samples, 10: 4 samples 11: 8 samples 00272 // Zen_XL - Z-axis output enabled 00273 // Yen_XL - Y-axis output enabled 00274 // Xen_XL - X-axis output enabled 00275 if (settings.accel.enableZ) tempRegValue |= (1<<5); 00276 if (settings.accel.enableY) tempRegValue |= (1<<4); 00277 if (settings.accel.enableX) tempRegValue |= (1<<3); 00278 00279 xgWriteByte(CTRL_REG5_XL, tempRegValue); 00280 00281 // CTRL_REG6_XL (0x20) (Default value: 0x00) 00282 // [ODR_XL2][ODR_XL1][ODR_XL0][FS1_XL][FS0_XL][BW_SCAL_ODR][BW_XL1][BW_XL0] 00283 // ODR_XL[2:0] - Output data rate & power mode selection 00284 // FS_XL[1:0] - Full-scale selection 00285 // BW_SCAL_ODR - Bandwidth selection 00286 // BW_XL[1:0] - Anti-aliasing filter bandwidth selection 00287 tempRegValue = 0; 00288 // To disable the accel, set the sampleRate bits to 0. 00289 if (settings.accel.enabled) 00290 { 00291 tempRegValue |= (settings.accel.sampleRate & 0x07) << 5; 00292 } 00293 switch (settings.accel.scale) 00294 { 00295 case 4: 00296 tempRegValue |= (0x2 << 3); 00297 break; 00298 case 8: 00299 tempRegValue |= (0x3 << 3); 00300 break; 00301 case 16: 00302 tempRegValue |= (0x1 << 3); 00303 break; 00304 // Otherwise it'll be set to 2g (0x0 << 3) 00305 } 00306 if (settings.accel.bandwidth >= 0) 00307 { 00308 tempRegValue |= (1<<2); // Set BW_SCAL_ODR 00309 tempRegValue |= (settings.accel.bandwidth & 0x03); 00310 } 00311 xgWriteByte(CTRL_REG6_XL, tempRegValue); 00312 00313 // CTRL_REG7_XL (0x21) (Default value: 0x00) 00314 // [HR][DCF1][DCF0][0][0][FDS][0][HPIS1] 00315 // HR - High resolution mode (0: disable, 1: enable) 00316 // DCF[1:0] - Digital filter cutoff frequency 00317 // FDS - Filtered data selection 00318 // HPIS1 - HPF enabled for interrupt function 00319 tempRegValue = 0; 00320 if (settings.accel.highResEnable) 00321 { 00322 tempRegValue |= (1<<7); // Set HR bit 00323 tempRegValue |= (settings.accel.highResBandwidth & 0x3) << 5; 00324 } 00325 xgWriteByte(CTRL_REG7_XL, tempRegValue); 00326 } 00327 00328 // This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average 00329 // them, scales them to gs and deg/s, respectively, and then passes the biases to the main sketch 00330 // for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store 00331 // the data as there are in the ADXL345, a precursor to the LSM9DS0, or the MPU-9150, so we have to 00332 // subtract the biases ourselves. This results in a more accurate measurement in general and can 00333 // remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner 00334 // is good practice. 00335 void LSM9DS1::calibrate(bool autoCalc) 00336 { 00337 uint8_t data[6] = {0, 0, 0, 0, 0, 0}; 00338 uint8_t samples = 0; 00339 int ii; 00340 int32_t aBiasRawTemp[3] = {0, 0, 0}; 00341 int32_t gBiasRawTemp[3] = {0, 0, 0}; 00342 00343 // Turn on FIFO and set threshold to 32 samples 00344 enableFIFO(true); 00345 setFIFO(FIFO_THS, 0x1F); 00346 while (samples < 0x1F) 00347 { 00348 samples = (xgReadByte(FIFO_SRC) & 0x3F); // Read number of stored samples 00349 } 00350 for(ii = 0; ii < samples ; ii++) 00351 { // Read the gyro data stored in the FIFO 00352 readGyro(); 00353 gBiasRawTemp[0] += gx; 00354 gBiasRawTemp[1] += gy; 00355 gBiasRawTemp[2] += gz; 00356 readAccel(); 00357 aBiasRawTemp[0] += ax; 00358 aBiasRawTemp[1] += ay; 00359 aBiasRawTemp[2] += az - (int16_t)(1./aRes); // Assumes sensor facing up! 00360 } 00361 for (ii = 0; ii < 3; ii++) 00362 { 00363 gBiasRaw[ii] = gBiasRawTemp[ii] / samples; 00364 gBias[ii] = calcGyro(gBiasRaw[ii]); 00365 aBiasRaw[ii] = aBiasRawTemp[ii] / samples; 00366 aBias[ii] = calcAccel(aBiasRaw[ii]); 00367 } 00368 00369 enableFIFO(false); 00370 setFIFO(FIFO_OFF, 0x00); 00371 00372 if (autoCalc) _autoCalc = true; 00373 } 00374 00375 void LSM9DS1::calibrateMag(bool loadIn) 00376 { 00377 int i, j; 00378 int16_t magMin[3] = {0, 0, 0}; 00379 int16_t magMax[3] = {0, 0, 0}; // The road warrior 00380 00381 for (i=0; i<128; i++) 00382 { 00383 while (!magAvailable()) 00384 ; 00385 readMag(); 00386 int16_t magTemp[3] = {0, 0, 0}; 00387 magTemp[0] = mx; 00388 magTemp[1] = my; 00389 magTemp[2] = mz; 00390 for (j = 0; j < 3; j++) 00391 { 00392 if (magTemp[j] > magMax[j]) magMax[j] = magTemp[j]; 00393 if (magTemp[j] < magMin[j]) magMin[j] = magTemp[j]; 00394 } 00395 } 00396 for (j = 0; j < 3; j++) 00397 { 00398 mBiasRaw[j] = (magMax[j] + magMin[j]) / 2; 00399 mBias[j] = calcMag(mBiasRaw[j]); 00400 if (loadIn) 00401 magOffset(j, mBiasRaw[j]); 00402 } 00403 00404 } 00405 void LSM9DS1::magOffset(uint8_t axis, int16_t offset) 00406 { 00407 if (axis > 2) 00408 return; 00409 uint8_t msb, lsb; 00410 msb = (offset & 0xFF00) >> 8; 00411 lsb = offset & 0x00FF; 00412 mWriteByte(OFFSET_X_REG_L_M + (2 * axis), lsb); 00413 mWriteByte(OFFSET_X_REG_H_M + (2 * axis), msb); 00414 } 00415 00416 void LSM9DS1::initMag() 00417 { 00418 uint8_t tempRegValue = 0; 00419 00420 // CTRL_REG1_M (Default value: 0x10) 00421 // [TEMP_COMP][OM1][OM0][DO2][DO1][DO0][0][ST] 00422 // TEMP_COMP - Temperature compensation 00423 // OM[1:0] - X & Y axes op mode selection 00424 // 00:low-power, 01:medium performance 00425 // 10: high performance, 11:ultra-high performance 00426 // DO[2:0] - Output data rate selection 00427 // ST - Self-test enable 00428 if (settings.mag.tempCompensationEnable) tempRegValue |= (1<<7); 00429 tempRegValue |= (settings.mag.XYPerformance & 0x3) << 5; 00430 tempRegValue |= (settings.mag.sampleRate & 0x7) << 2; 00431 mWriteByte(CTRL_REG1_M, tempRegValue); 00432 00433 // CTRL_REG2_M (Default value 0x00) 00434 // [0][FS1][FS0][0][REBOOT][SOFT_RST][0][0] 00435 // FS[1:0] - Full-scale configuration 00436 // REBOOT - Reboot memory content (0:normal, 1:reboot) 00437 // SOFT_RST - Reset config and user registers (0:default, 1:reset) 00438 tempRegValue = 0; 00439 switch (settings.mag.scale) 00440 { 00441 case 8: 00442 tempRegValue |= (0x1 << 5); 00443 break; 00444 case 12: 00445 tempRegValue |= (0x2 << 5); 00446 break; 00447 case 16: 00448 tempRegValue |= (0x3 << 5); 00449 break; 00450 // Otherwise we'll default to 4 gauss (00) 00451 } 00452 mWriteByte(CTRL_REG2_M, tempRegValue); // +/-4Gauss 00453 00454 // CTRL_REG3_M (Default value: 0x03) 00455 // [I2C_DISABLE][0][LP][0][0][SIM][MD1][MD0] 00456 // I2C_DISABLE - Disable I2C interace (0:enable, 1:disable) 00457 // LP - Low-power mode cofiguration (1:enable) 00458 // SIM - SPI mode selection (0:write-only, 1:read/write enable) 00459 // MD[1:0] - Operating mode 00460 // 00:continuous conversion, 01:single-conversion, 00461 // 10,11: Power-down 00462 tempRegValue = 0; 00463 if (settings.mag.lowPowerEnable) tempRegValue |= (1<<5); 00464 tempRegValue |= (settings.mag.operatingMode & 0x3); 00465 mWriteByte(CTRL_REG3_M, tempRegValue); // Continuous conversion mode 00466 00467 // CTRL_REG4_M (Default value: 0x00) 00468 // [0][0][0][0][OMZ1][OMZ0][BLE][0] 00469 // OMZ[1:0] - Z-axis operative mode selection 00470 // 00:low-power mode, 01:medium performance 00471 // 10:high performance, 10:ultra-high performance 00472 // BLE - Big/little endian data 00473 tempRegValue = 0; 00474 tempRegValue = (settings.mag.ZPerformance & 0x3) << 2; 00475 mWriteByte(CTRL_REG4_M, tempRegValue); 00476 00477 // CTRL_REG5_M (Default value: 0x00) 00478 // [0][BDU][0][0][0][0][0][0] 00479 // BDU - Block data update for magnetic data 00480 // 0:continuous, 1:not updated until MSB/LSB are read 00481 tempRegValue = 0; 00482 mWriteByte(CTRL_REG5_M, tempRegValue); 00483 } 00484 00485 uint8_t LSM9DS1::accelAvailable() 00486 { 00487 uint8_t status = xgReadByte(STATUS_REG_1); 00488 00489 return (status & (1<<0)); 00490 } 00491 00492 uint8_t LSM9DS1::gyroAvailable() 00493 { 00494 uint8_t status = xgReadByte(STATUS_REG_1); 00495 00496 return ((status & (1<<1)) >> 1); 00497 } 00498 00499 uint8_t LSM9DS1::tempAvailable() 00500 { 00501 uint8_t status = xgReadByte(STATUS_REG_1); 00502 00503 return ((status & (1<<2)) >> 2); 00504 } 00505 00506 uint8_t LSM9DS1::magAvailable(lsm9ds1_axis axis) 00507 { 00508 uint8_t status; 00509 status = mReadByte(STATUS_REG_M); 00510 00511 return ((status & (1<<axis)) >> axis); 00512 } 00513 00514 void LSM9DS1::readAccel() 00515 { 00516 uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp 00517 xgReadBytes(OUT_X_L_XL, temp, 6); // Read 6 bytes, beginning at OUT_X_L_XL 00518 ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax 00519 ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay 00520 az = (temp[5] << 8) | temp[4]; // Store z-axis values into az 00521 if (_autoCalc) 00522 { 00523 ax -= aBiasRaw[X_AXIS]; 00524 ay -= aBiasRaw[Y_AXIS]; 00525 az -= aBiasRaw[Z_AXIS]; 00526 } 00527 } 00528 00529 int16_t LSM9DS1::readAccel(lsm9ds1_axis axis) 00530 { 00531 uint8_t temp[2]; 00532 int16_t value; 00533 xgReadBytes(OUT_X_L_XL + (2 * axis), temp, 2); 00534 value = (temp[1] << 8) | temp[0]; 00535 00536 if (_autoCalc) 00537 value -= aBiasRaw[axis]; 00538 00539 return value; 00540 } 00541 00542 void LSM9DS1::readMag() 00543 { 00544 uint8_t temp[6]; // We'll read six bytes from the mag into temp 00545 mReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M 00546 mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx 00547 my = (temp[3] << 8) | temp[2]; // Store y-axis values into my 00548 mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz 00549 } 00550 00551 int16_t LSM9DS1::readMag(lsm9ds1_axis axis) 00552 { 00553 uint8_t temp[2]; 00554 mReadBytes(OUT_X_L_M + (2 * axis), temp, 2); 00555 return (temp[1] << 8) | temp[0]; 00556 } 00557 00558 void LSM9DS1::readTemp() 00559 { 00560 uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp 00561 xgReadBytes(OUT_TEMP_L, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L 00562 temperature = ((int16_t)temp[1] << 8) | temp[0]; 00563 } 00564 00565 void LSM9DS1::readGyro() 00566 { 00567 uint8_t temp[6]; // We'll read six bytes from the gyro into temp 00568 xgReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G 00569 gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx 00570 gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy 00571 gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz 00572 if (_autoCalc) 00573 { 00574 gx -= gBiasRaw[X_AXIS]; 00575 gy -= gBiasRaw[Y_AXIS]; 00576 gz -= gBiasRaw[Z_AXIS]; 00577 } 00578 } 00579 00580 int16_t LSM9DS1::readGyro(lsm9ds1_axis axis) 00581 { 00582 uint8_t temp[2]; 00583 int16_t value; 00584 00585 xgReadBytes(OUT_X_L_G + (2 * axis), temp, 2); 00586 00587 value = (temp[1] << 8) | temp[0]; 00588 00589 if (_autoCalc) 00590 value -= gBiasRaw[axis]; 00591 00592 return value; 00593 } 00594 00595 float LSM9DS1::calcGyro(int16_t gyro) 00596 { 00597 // Return the gyro raw reading times our pre-calculated DPS / (ADC tick): 00598 return gRes * gyro; 00599 } 00600 00601 float LSM9DS1::calcAccel(int16_t accel) 00602 { 00603 // Return the accel raw reading times our pre-calculated g's / (ADC tick): 00604 return aRes * accel; 00605 } 00606 00607 float LSM9DS1::calcMag(int16_t mag) 00608 { 00609 // Return the mag raw reading times our pre-calculated Gs / (ADC tick): 00610 return mRes * mag; 00611 } 00612 00613 void LSM9DS1::setGyroScale(uint16_t gScl) 00614 { 00615 // Read current value of CTRL_REG1_G: 00616 uint8_t ctrl1RegValue = xgReadByte(CTRL_REG1_G); 00617 // Mask out scale bits (3 & 4): 00618 ctrl1RegValue &= 0xE7; 00619 switch (gScl) 00620 { 00621 case 500: 00622 ctrl1RegValue |= (0x1 << 3); 00623 settings.gyro.scale = 500; 00624 break; 00625 case 2000: 00626 ctrl1RegValue |= (0x3 << 3); 00627 settings.gyro.scale = 2000; 00628 break; 00629 default: // Otherwise we'll set it to 245 dps (0x0 << 4) 00630 settings.gyro.scale = 245; 00631 break; 00632 } 00633 xgWriteByte(CTRL_REG1_G, ctrl1RegValue); 00634 00635 calcgRes(); 00636 } 00637 00638 void LSM9DS1::setAccelScale(uint8_t aScl) 00639 { 00640 // We need to preserve the other bytes in CTRL_REG6_XL. So, first read it: 00641 uint8_t tempRegValue = xgReadByte(CTRL_REG6_XL); 00642 // Mask out accel scale bits: 00643 tempRegValue &= 0xE7; 00644 00645 switch (aScl) 00646 { 00647 case 4: 00648 tempRegValue |= (0x2 << 3); 00649 settings.accel.scale = 4; 00650 break; 00651 case 8: 00652 tempRegValue |= (0x3 << 3); 00653 settings.accel.scale = 8; 00654 break; 00655 case 16: 00656 tempRegValue |= (0x1 << 3); 00657 settings.accel.scale = 16; 00658 break; 00659 default: // Otherwise it'll be set to 2g (0x0 << 3) 00660 settings.accel.scale = 2; 00661 break; 00662 } 00663 xgWriteByte(CTRL_REG6_XL, tempRegValue); 00664 00665 // Then calculate a new aRes, which relies on aScale being set correctly: 00666 calcaRes(); 00667 } 00668 00669 void LSM9DS1::setMagScale(uint8_t mScl) 00670 { 00671 // We need to preserve the other bytes in CTRL_REG6_XM. So, first read it: 00672 uint8_t temp = mReadByte(CTRL_REG2_M); 00673 // Then mask out the mag scale bits: 00674 temp &= 0xFF^(0x3 << 5); 00675 00676 switch (mScl) 00677 { 00678 case 8: 00679 temp |= (0x1 << 5); 00680 settings.mag.scale = 8; 00681 break; 00682 case 12: 00683 temp |= (0x2 << 5); 00684 settings.mag.scale = 12; 00685 break; 00686 case 16: 00687 temp |= (0x3 << 5); 00688 settings.mag.scale = 16; 00689 break; 00690 default: // Otherwise we'll default to 4 gauss (00) 00691 settings.mag.scale = 4; 00692 break; 00693 } 00694 00695 // And write the new register value back into CTRL_REG6_XM: 00696 mWriteByte(CTRL_REG2_M, temp); 00697 00698 // We've updated the sensor, but we also need to update our class variables 00699 // First update mScale: 00700 //mScale = mScl; 00701 // Then calculate a new mRes, which relies on mScale being set correctly: 00702 calcmRes(); 00703 } 00704 00705 void LSM9DS1::setGyroODR(uint8_t gRate) 00706 { 00707 // Only do this if gRate is not 0 (which would disable the gyro) 00708 if ((gRate & 0x07) != 0) 00709 { 00710 // We need to preserve the other bytes in CTRL_REG1_G. So, first read it: 00711 uint8_t temp = xgReadByte(CTRL_REG1_G); 00712 // Then mask out the gyro ODR bits: 00713 temp &= 0xFF^(0x7 << 5); 00714 temp |= (gRate & 0x07) << 5; 00715 // Update our settings struct 00716 settings.gyro.sampleRate = gRate & 0x07; 00717 // And write the new register value back into CTRL_REG1_G: 00718 xgWriteByte(CTRL_REG1_G, temp); 00719 } 00720 } 00721 00722 void LSM9DS1::setAccelODR(uint8_t aRate) 00723 { 00724 // Only do this if aRate is not 0 (which would disable the accel) 00725 if ((aRate & 0x07) != 0) 00726 { 00727 // We need to preserve the other bytes in CTRL_REG1_XM. So, first read it: 00728 uint8_t temp = xgReadByte(CTRL_REG6_XL); 00729 // Then mask out the accel ODR bits: 00730 temp &= 0x1F; 00731 // Then shift in our new ODR bits: 00732 temp |= ((aRate & 0x07) << 5); 00733 settings.accel.sampleRate = aRate & 0x07; 00734 // And write the new register value back into CTRL_REG1_XM: 00735 xgWriteByte(CTRL_REG6_XL, temp); 00736 } 00737 } 00738 00739 void LSM9DS1::setMagODR(uint8_t mRate) 00740 { 00741 // We need to preserve the other bytes in CTRL_REG5_XM. So, first read it: 00742 uint8_t temp = mReadByte(CTRL_REG1_M); 00743 // Then mask out the mag ODR bits: 00744 temp &= 0xFF^(0x7 << 2); 00745 // Then shift in our new ODR bits: 00746 temp |= ((mRate & 0x07) << 2); 00747 settings.mag.sampleRate = mRate & 0x07; 00748 // And write the new register value back into CTRL_REG5_XM: 00749 mWriteByte(CTRL_REG1_M, temp); 00750 } 00751 00752 void LSM9DS1::calcgRes() 00753 { 00754 gRes = ((float) settings.gyro.scale) / 32768.0; 00755 } 00756 00757 void LSM9DS1::calcaRes() 00758 { 00759 aRes = ((float) settings.accel.scale) / 32768.0; 00760 } 00761 00762 void LSM9DS1::calcmRes() 00763 { 00764 //mRes = ((float) settings.mag.scale) / 32768.0; 00765 switch (settings.mag.scale) 00766 { 00767 case 4: 00768 mRes = magSensitivity[0]; 00769 break; 00770 case 8: 00771 mRes = magSensitivity[1]; 00772 break; 00773 case 12: 00774 mRes = magSensitivity[2]; 00775 break; 00776 case 16: 00777 mRes = magSensitivity[3]; 00778 break; 00779 } 00780 00781 } 00782 00783 void LSM9DS1::configInt(interrupt_select interrupt, uint8_t generator, 00784 h_lactive activeLow, pp_od pushPull) 00785 { 00786 // Write to INT1_CTRL or INT2_CTRL. [interupt] should already be one of 00787 // those two values. 00788 // [generator] should be an OR'd list of values from the interrupt_generators enum 00789 xgWriteByte(interrupt, generator); 00790 00791 // Configure CTRL_REG8 00792 uint8_t temp; 00793 temp = xgReadByte(CTRL_REG8); 00794 00795 if (activeLow) temp |= (1<<5); 00796 else temp &= ~(1<<5); 00797 00798 if (pushPull) temp &= ~(1<<4); 00799 else temp |= (1<<4); 00800 00801 xgWriteByte(CTRL_REG8, temp); 00802 } 00803 00804 void LSM9DS1::configInactivity(uint8_t duration, uint8_t threshold, bool sleepOn) 00805 { 00806 uint8_t temp = 0; 00807 00808 temp = threshold & 0x7F; 00809 if (sleepOn) temp |= (1<<7); 00810 xgWriteByte(ACT_THS, temp); 00811 00812 xgWriteByte(ACT_DUR, duration); 00813 } 00814 00815 uint8_t LSM9DS1::getInactivity() 00816 { 00817 uint8_t temp = xgReadByte(STATUS_REG_0); 00818 temp &= (0x10); 00819 return temp; 00820 } 00821 00822 void LSM9DS1::configAccelInt(uint8_t generator, bool andInterrupts) 00823 { 00824 // Use variables from accel_interrupt_generator, OR'd together to create 00825 // the [generator]value. 00826 uint8_t temp = generator; 00827 if (andInterrupts) temp |= 0x80; 00828 xgWriteByte(INT_GEN_CFG_XL, temp); 00829 } 00830 00831 void LSM9DS1::configAccelThs(uint8_t threshold, lsm9ds1_axis axis, uint8_t duration, bool wait) 00832 { 00833 // Write threshold value to INT_GEN_THS_?_XL. 00834 // axis will be 0, 1, or 2 (x, y, z respectively) 00835 xgWriteByte(INT_GEN_THS_X_XL + axis, threshold); 00836 00837 // Write duration and wait to INT_GEN_DUR_XL 00838 uint8_t temp; 00839 temp = (duration & 0x7F); 00840 if (wait) temp |= 0x80; 00841 xgWriteByte(INT_GEN_DUR_XL, temp); 00842 } 00843 00844 uint8_t LSM9DS1::getAccelIntSrc() 00845 { 00846 uint8_t intSrc = xgReadByte(INT_GEN_SRC_XL); 00847 00848 // Check if the IA_XL (interrupt active) bit is set 00849 if (intSrc & (1<<6)) 00850 { 00851 return (intSrc & 0x3F); 00852 } 00853 00854 return 0; 00855 } 00856 00857 void LSM9DS1::configGyroInt(uint8_t generator, bool aoi, bool latch) 00858 { 00859 // Use variables from accel_interrupt_generator, OR'd together to create 00860 // the [generator]value. 00861 uint8_t temp = generator; 00862 if (aoi) temp |= 0x80; 00863 if (latch) temp |= 0x40; 00864 xgWriteByte(INT_GEN_CFG_G, temp); 00865 } 00866 00867 void LSM9DS1::configGyroThs(int16_t threshold, lsm9ds1_axis axis, uint8_t duration, bool wait) 00868 { 00869 uint8_t buffer[2]; 00870 buffer[0] = (threshold & 0x7F00) >> 8; 00871 buffer[1] = (threshold & 0x00FF); 00872 // Write threshold value to INT_GEN_THS_?H_G and INT_GEN_THS_?L_G. 00873 // axis will be 0, 1, or 2 (x, y, z respectively) 00874 xgWriteByte(INT_GEN_THS_XH_G + (axis * 2), buffer[0]); 00875 xgWriteByte(INT_GEN_THS_XH_G + 1 + (axis * 2), buffer[1]); 00876 00877 // Write duration and wait to INT_GEN_DUR_XL 00878 uint8_t temp; 00879 temp = (duration & 0x7F); 00880 if (wait) temp |= 0x80; 00881 xgWriteByte(INT_GEN_DUR_G, temp); 00882 } 00883 00884 uint8_t LSM9DS1::getGyroIntSrc() 00885 { 00886 uint8_t intSrc = xgReadByte(INT_GEN_SRC_G); 00887 00888 // Check if the IA_G (interrupt active) bit is set 00889 if (intSrc & (1<<6)) 00890 { 00891 return (intSrc & 0x3F); 00892 } 00893 00894 return 0; 00895 } 00896 00897 void LSM9DS1::configMagInt(uint8_t generator, h_lactive activeLow, bool latch) 00898 { 00899 // Mask out non-generator bits (0-4) 00900 uint8_t config = (generator & 0xE0); 00901 // IEA bit is 0 for active-low, 1 for active-high. 00902 if (activeLow == INT_ACTIVE_HIGH) config |= (1<<2); 00903 // IEL bit is 0 for latched, 1 for not-latched 00904 if (!latch) config |= (1<<1); 00905 // As long as we have at least 1 generator, enable the interrupt 00906 if (generator != 0) config |= (1<<0); 00907 00908 mWriteByte(INT_CFG_M, config); 00909 } 00910 00911 void LSM9DS1::configMagThs(uint16_t threshold) 00912 { 00913 // Write high eight bits of [threshold] to INT_THS_H_M 00914 mWriteByte(INT_THS_H_M, uint8_t((threshold & 0x7F00) >> 8)); 00915 // Write low eight bits of [threshold] to INT_THS_L_M 00916 mWriteByte(INT_THS_L_M, uint8_t(threshold & 0x00FF)); 00917 } 00918 00919 uint8_t LSM9DS1::getMagIntSrc() 00920 { 00921 uint8_t intSrc = mReadByte(INT_SRC_M); 00922 00923 // Check if the INT (interrupt active) bit is set 00924 if (intSrc & (1<<0)) 00925 { 00926 return (intSrc & 0xFE); 00927 } 00928 00929 return 0; 00930 } 00931 00932 void LSM9DS1::sleepGyro(bool enable) 00933 { 00934 uint8_t temp = xgReadByte(CTRL_REG9); 00935 if (enable) temp |= (1<<6); 00936 else temp &= ~(1<<6); 00937 xgWriteByte(CTRL_REG9, temp); 00938 } 00939 00940 void LSM9DS1::enableFIFO(bool enable) 00941 { 00942 uint8_t temp = xgReadByte(CTRL_REG9); 00943 if (enable) temp |= (1<<1); 00944 else temp &= ~(1<<1); 00945 xgWriteByte(CTRL_REG9, temp); 00946 } 00947 00948 void LSM9DS1::setFIFO(fifoMode_type fifoMode, uint8_t fifoThs) 00949 { 00950 // Limit threshold - 0x1F (31) is the maximum. If more than that was asked 00951 // limit it to the maximum. 00952 uint8_t threshold = fifoThs <= 0x1F ? fifoThs : 0x1F; 00953 xgWriteByte(FIFO_CTRL, ((fifoMode & 0x7) << 5) | (threshold & 0x1F)); 00954 } 00955 00956 uint8_t LSM9DS1::getFIFOSamples() 00957 { 00958 return (xgReadByte(FIFO_SRC) & 0x3F); 00959 } 00960 00961 void LSM9DS1::constrainScales() 00962 { 00963 if ((settings.gyro.scale != 245) && (settings.gyro.scale != 500) && 00964 (settings.gyro.scale != 2000)) 00965 { 00966 settings.gyro.scale = 245; 00967 } 00968 00969 if ((settings.accel.scale != 2) && (settings.accel.scale != 4) && 00970 (settings.accel.scale != 8) && (settings.accel.scale != 16)) 00971 { 00972 settings.accel.scale = 2; 00973 } 00974 00975 if ((settings.mag.scale != 4) && (settings.mag.scale != 8) && 00976 (settings.mag.scale != 12) && (settings.mag.scale != 16)) 00977 { 00978 settings.mag.scale = 4; 00979 } 00980 } 00981 00982 void LSM9DS1::xgWriteByte(uint8_t subAddress, uint8_t data) 00983 { 00984 // Whether we're using I2C or SPI, write a byte using the 00985 // gyro-specific I2C address or SPI CS pin. 00986 if (settings.device.commInterface == IMU_MODE_I2C) { 00987 printf("yo"); 00988 I2CwriteByte(_xgAddress, subAddress, data); 00989 } else if (settings.device.commInterface == IMU_MODE_SPI) { 00990 SPIwriteByte(_xgAddress, subAddress, data); 00991 } 00992 } 00993 00994 void LSM9DS1::mWriteByte(uint8_t subAddress, uint8_t data) 00995 { 00996 // Whether we're using I2C or SPI, write a byte using the 00997 // accelerometer-specific I2C address or SPI CS pin. 00998 if (settings.device.commInterface == IMU_MODE_I2C) 00999 return I2CwriteByte(_mAddress, subAddress, data); 01000 else if (settings.device.commInterface == IMU_MODE_SPI) 01001 return SPIwriteByte(_mAddress, subAddress, data); 01002 } 01003 01004 uint8_t LSM9DS1::xgReadByte(uint8_t subAddress) 01005 { 01006 // Whether we're using I2C or SPI, read a byte using the 01007 // gyro-specific I2C address or SPI CS pin. 01008 if (settings.device.commInterface == IMU_MODE_I2C) 01009 return I2CreadByte(_xgAddress, subAddress); 01010 else if (settings.device.commInterface == IMU_MODE_SPI) 01011 return SPIreadByte(_xgAddress, subAddress); 01012 } 01013 01014 void LSM9DS1::xgReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count) 01015 { 01016 // Whether we're using I2C or SPI, read multiple bytes using the 01017 // gyro-specific I2C address or SPI CS pin. 01018 if (settings.device.commInterface == IMU_MODE_I2C) { 01019 I2CreadBytes(_xgAddress, subAddress, dest, count); 01020 } else if (settings.device.commInterface == IMU_MODE_SPI) { 01021 SPIreadBytes(_xgAddress, subAddress, dest, count); 01022 } 01023 } 01024 01025 uint8_t LSM9DS1::mReadByte(uint8_t subAddress) 01026 { 01027 // Whether we're using I2C or SPI, read a byte using the 01028 // accelerometer-specific I2C address or SPI CS pin. 01029 if (settings.device.commInterface == IMU_MODE_I2C) 01030 return I2CreadByte(_mAddress, subAddress); 01031 else if (settings.device.commInterface == IMU_MODE_SPI) 01032 return SPIreadByte(_mAddress, subAddress); 01033 } 01034 01035 void LSM9DS1::mReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count) 01036 { 01037 // Whether we're using I2C or SPI, read multiple bytes using the 01038 // accelerometer-specific I2C address or SPI CS pin. 01039 if (settings.device.commInterface == IMU_MODE_I2C) 01040 I2CreadBytes(_mAddress, subAddress, dest, count); 01041 else if (settings.device.commInterface == IMU_MODE_SPI) 01042 SPIreadBytes(_mAddress, subAddress, dest, count); 01043 } 01044 01045 void LSM9DS1::initSPI() 01046 { 01047 /* 01048 pinMode(_xgAddress, OUTPUT); 01049 digitalWrite(_xgAddress, HIGH); 01050 pinMode(_mAddress, OUTPUT); 01051 digitalWrite(_mAddress, HIGH); 01052 01053 SPI.begin(); 01054 // Maximum SPI frequency is 10MHz, could divide by 2 here: 01055 SPI.setClockDivider(SPI_CLOCK_DIV2); 01056 // Data is read and written MSb first. 01057 SPI.setBitOrder(MSBFIRST); 01058 // Data is captured on rising edge of clock (CPHA = 0) 01059 // Base value of the clock is HIGH (CPOL = 1) 01060 SPI.setDataMode(SPI_MODE0); 01061 */ 01062 } 01063 01064 void LSM9DS1::SPIwriteByte(uint8_t csPin, uint8_t subAddress, uint8_t data) 01065 { 01066 /* 01067 digitalWrite(csPin, LOW); // Initiate communication 01068 01069 // If write, bit 0 (MSB) should be 0 01070 // If single write, bit 1 should be 0 01071 SPI.transfer(subAddress & 0x3F); // Send Address 01072 SPI.transfer(data); // Send data 01073 01074 digitalWrite(csPin, HIGH); // Close communication 01075 */ 01076 } 01077 01078 uint8_t LSM9DS1::SPIreadByte(uint8_t csPin, uint8_t subAddress) 01079 { 01080 uint8_t temp; 01081 // Use the multiple read function to read 1 byte. 01082 // Value is returned to `temp`. 01083 SPIreadBytes(csPin, subAddress, &temp, 1); 01084 return temp; 01085 } 01086 01087 void LSM9DS1::SPIreadBytes(uint8_t csPin, uint8_t subAddress, 01088 uint8_t * dest, uint8_t count) 01089 { 01090 // To indicate a read, set bit 0 (msb) of first byte to 1 01091 uint8_t rAddress = 0x80 | (subAddress & 0x3F); 01092 // Mag SPI port is different. If we're reading multiple bytes, 01093 // set bit 1 to 1. The remaining six bytes are the address to be read 01094 if ((csPin == _mAddress) && count > 1) 01095 rAddress |= 0x40; 01096 01097 /* 01098 digitalWrite(csPin, LOW); // Initiate communication 01099 SPI.transfer(rAddress); 01100 for (int i=0; i<count; i++) 01101 { 01102 dest[i] = SPI.transfer(0x00); // Read into destination array 01103 } 01104 digitalWrite(csPin, HIGH); // Close communication 01105 */ 01106 } 01107 01108 void LSM9DS1::initI2C() 01109 { 01110 /* 01111 Wire.begin(); // Initialize I2C library 01112 */ 01113 01114 //already initialized in constructor! 01115 } 01116 01117 // Wire.h read and write protocols 01118 void LSM9DS1::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data) 01119 { 01120 /* 01121 Wire.beginTransmission(address); // Initialize the Tx buffer 01122 Wire.write(subAddress); // Put slave register address in Tx buffer 01123 Wire.write(data); // Put data in Tx buffer 01124 Wire.endTransmission(); // Send the Tx buffer 01125 */ 01126 char temp_data[2] = {subAddress, data}; 01127 i2c.write(address, temp_data, 2); 01128 } 01129 01130 uint8_t LSM9DS1::I2CreadByte(uint8_t address, uint8_t subAddress) 01131 { 01132 /* 01133 int timeout = LSM9DS1_COMMUNICATION_TIMEOUT; 01134 uint8_t data; // `data` will store the register data 01135 01136 Wire.beginTransmission(address); // Initialize the Tx buffer 01137 Wire.write(subAddress); // Put slave register address in Tx buffer 01138 Wire.endTransmission(true); // Send the Tx buffer, but send a restart to keep connection alive 01139 Wire.requestFrom(address, (uint8_t) 1); // Read one byte from slave register address 01140 while ((Wire.available() < 1) && (timeout-- > 0)) 01141 delay(1); 01142 01143 if (timeout <= 0) 01144 return 255; //! Bad! 255 will be misinterpreted as a good value. 01145 01146 data = Wire.read(); // Fill Rx buffer with result 01147 return data; // Return data read from slave register 01148 */ 01149 char data; 01150 char temp[1] = {subAddress}; 01151 01152 i2c.write(address, temp, 1); 01153 //i2c.write(address & 0xFE); 01154 temp[1] = 0x00; 01155 i2c.write(address, temp, 1); 01156 //i2c.write( address | 0x01); 01157 int a = i2c.read(address, &data, 1); 01158 return data; 01159 } 01160 01161 uint8_t LSM9DS1::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count) 01162 { 01163 /* 01164 int timeout = LSM9DS1_COMMUNICATION_TIMEOUT; 01165 Wire.beginTransmission(address); // Initialize the Tx buffer 01166 // Next send the register to be read. OR with 0x80 to indicate multi-read. 01167 Wire.write(subAddress | 0x80); // Put slave register address in Tx buffer 01168 01169 Wire.endTransmission(true); // Send the Tx buffer, but send a restart to keep connection alive 01170 uint8_t i = 0; 01171 Wire.requestFrom(address, count); // Read bytes from slave register address 01172 while ((Wire.available() < count) && (timeout-- > 0)) 01173 delay(1); 01174 if (timeout <= 0) 01175 return -1; 01176 01177 for (int i=0; i<count;) 01178 { 01179 if (Wire.available()) 01180 { 01181 dest[i++] = Wire.read(); 01182 } 01183 } 01184 return count; 01185 */ 01186 int i; 01187 char temp_dest[count]; 01188 char temp[1] = {subAddress}; 01189 i2c.write(address, temp, 1); 01190 i2c.read(address, temp_dest, count); 01191 01192 //i2c doesn't take uint8_ts, but rather chars so do this nasty af conversion 01193 for (i=0; i < count; i++) { 01194 dest[i] = temp_dest[i]; 01195 } 01196 return count; 01197 }
Generated on Sun Jul 24 2022 15:36:09 by
1.7.2