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