Zhihan Zhang
ECE 4180 Final Project
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 - (int16_t)(1./aRes); 00350 aBiasRawTemp[1] += ay; 00351 aBiasRawTemp[2] += az; 00352 //aBiasRawTemp[2] += az - (int16_t)(1./aRes); // Assumes sensor facing up! 00353 } 00354 for (ii = 0; ii < 3; ii++) { 00355 gBiasRaw[ii] = gBiasRawTemp[ii] / samples; 00356 gBias[ii] = calcGyro(gBiasRaw[ii]); 00357 aBiasRaw[ii] = aBiasRawTemp[ii] / samples; 00358 aBias[ii] = calcAccel(aBiasRaw[ii]); 00359 } 00360 00361 enableFIFO(false); 00362 setFIFO(FIFO_OFF, 0x00); 00363 00364 if (autoCalc) _autoCalc = true; 00365 } 00366 00367 void LSM9DS1::calibrateMag(bool loadIn) 00368 { 00369 int i, j; 00370 int16_t magMin[3] = {0, 0, 0}; 00371 int16_t magMax[3] = {0, 0, 0}; // The road warrior 00372 pc.printf("\n\n\r Rotate IMU device at least 360 in horizontal plane for magnetometer calibration\n\r"); 00373 wait(0.5); 00374 for (i=0; i<1000; i++) { 00375 while (!magAvailable(ALL_AXIS)); 00376 readMag(); 00377 int16_t magTemp[3] = {0, 0, 0}; 00378 magTemp[0] = mx; 00379 magTemp[1] = my; 00380 magTemp[2] = mz; 00381 for (j = 0; j < 3; j++) { 00382 if (magTemp[j] > magMax[j]) magMax[j] = magTemp[j]; 00383 if (magTemp[j] < magMin[j]) magMin[j] = magTemp[j]; 00384 } 00385 } 00386 for (j = 0; j < 3; j++) { 00387 mBiasRaw[j] = (magMax[j] + magMin[j]) / 2; 00388 mBias[j] = calcMag(mBiasRaw[j]); 00389 pc.printf("%f ",mBias[j]); 00390 if (loadIn) 00391 magOffset(j, mBiasRaw[j]); 00392 } 00393 pc.printf("\n\rMAG calibration done\n\r"); 00394 } 00395 void LSM9DS1::magOffset(uint8_t axis, int16_t offset) 00396 { 00397 if (axis > 2) 00398 return; 00399 uint8_t msb, lsb; 00400 msb = (offset & 0xFF00) >> 8; 00401 lsb = offset & 0x00FF; 00402 mWriteByte(OFFSET_X_REG_L_M + (2 * axis), lsb); 00403 mWriteByte(OFFSET_X_REG_H_M + (2 * axis), msb); 00404 } 00405 00406 void LSM9DS1::initMag() 00407 { 00408 uint8_t tempRegValue = 0; 00409 00410 // CTRL_REG1_M (Default value: 0x10) 00411 // [TEMP_COMP][OM1][OM0][DO2][DO1][DO0][0][ST] 00412 // TEMP_COMP - Temperature compensation 00413 // OM[1:0] - X & Y axes op mode selection 00414 // 00:low-power, 01:medium performance 00415 // 10: high performance, 11:ultra-high performance 00416 // DO[2:0] - Output data rate selection 00417 // ST - Self-test enable 00418 if (settings.mag.tempCompensationEnable) tempRegValue |= (1<<7); 00419 tempRegValue |= (settings.mag.XYPerformance & 0x3) << 5; 00420 tempRegValue |= (settings.mag.sampleRate & 0x7) << 2; 00421 mWriteByte(CTRL_REG1_M, tempRegValue); 00422 00423 // CTRL_REG2_M (Default value 0x00) 00424 // [0][FS1][FS0][0][REBOOT][SOFT_RST][0][0] 00425 // FS[1:0] - Full-scale configuration 00426 // REBOOT - Reboot memory content (0:normal, 1:reboot) 00427 // SOFT_RST - Reset config and user registers (0:default, 1:reset) 00428 tempRegValue = 0; 00429 switch (settings.mag.scale) { 00430 case 8: 00431 tempRegValue |= (0x1 << 5); 00432 break; 00433 case 12: 00434 tempRegValue |= (0x2 << 5); 00435 break; 00436 case 16: 00437 tempRegValue |= (0x3 << 5); 00438 break; 00439 // Otherwise we'll default to 4 gauss (00) 00440 } 00441 mWriteByte(CTRL_REG2_M, tempRegValue); // +/-4Gauss 00442 00443 // CTRL_REG3_M (Default value: 0x03) 00444 // [I2C_DISABLE][0][LP][0][0][SIM][MD1][MD0] 00445 // I2C_DISABLE - Disable I2C interace (0:enable, 1:disable) 00446 // LP - Low-power mode cofiguration (1:enable) 00447 // SIM - SPI mode selection (0:write-only, 1:read/write enable) 00448 // MD[1:0] - Operating mode 00449 // 00:continuous conversion, 01:single-conversion, 00450 // 10,11: Power-down 00451 tempRegValue = 0; 00452 if (settings.mag.lowPowerEnable) tempRegValue |= (1<<5); 00453 tempRegValue |= (settings.mag.operatingMode & 0x3); 00454 mWriteByte(CTRL_REG3_M, tempRegValue); // Continuous conversion mode 00455 00456 // CTRL_REG4_M (Default value: 0x00) 00457 // [0][0][0][0][OMZ1][OMZ0][BLE][0] 00458 // OMZ[1:0] - Z-axis operative mode selection 00459 // 00:low-power mode, 01:medium performance 00460 // 10:high performance, 10:ultra-high performance 00461 // BLE - Big/little endian data 00462 tempRegValue = 0; 00463 tempRegValue = (settings.mag.ZPerformance & 0x3) << 2; 00464 mWriteByte(CTRL_REG4_M, tempRegValue); 00465 00466 // CTRL_REG5_M (Default value: 0x00) 00467 // [0][BDU][0][0][0][0][0][0] 00468 // BDU - Block data update for magnetic data 00469 // 0:continuous, 1:not updated until MSB/LSB are read 00470 tempRegValue = 0; 00471 mWriteByte(CTRL_REG5_M, tempRegValue); 00472 } 00473 00474 uint8_t LSM9DS1::accelAvailable() 00475 { 00476 uint8_t status = xgReadByte(STATUS_REG_1); 00477 00478 return (status & (1<<0)); 00479 } 00480 00481 uint8_t LSM9DS1::gyroAvailable() 00482 { 00483 uint8_t status = xgReadByte(STATUS_REG_1); 00484 00485 return ((status & (1<<1)) >> 1); 00486 } 00487 00488 uint8_t LSM9DS1::tempAvailable() 00489 { 00490 uint8_t status = xgReadByte(STATUS_REG_1); 00491 00492 return ((status & (1<<2)) >> 2); 00493 } 00494 00495 uint8_t LSM9DS1::magAvailable(lsm9ds1_axis axis) 00496 { 00497 uint8_t status; 00498 status = mReadByte(STATUS_REG_M); 00499 00500 return ((status & (1<<axis)) >> axis); 00501 } 00502 00503 void LSM9DS1::readAccel() 00504 { 00505 uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp 00506 xgReadBytes(OUT_X_L_XL, temp, 6); // Read 6 bytes, beginning at OUT_X_L_XL 00507 ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax 00508 ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay 00509 az = (temp[5] << 8) | temp[4]; // Store z-axis values into az 00510 if (_autoCalc) { 00511 ax -= aBiasRaw[X_AXIS]; 00512 ay -= aBiasRaw[Y_AXIS]; 00513 az -= aBiasRaw[Z_AXIS]; 00514 } 00515 } 00516 00517 int16_t LSM9DS1::readAccel(lsm9ds1_axis axis) 00518 { 00519 uint8_t temp[2]; 00520 int16_t value; 00521 xgReadBytes(OUT_X_L_XL + (2 * axis), temp, 2); 00522 value = (temp[1] << 8) | temp[0]; 00523 00524 if (_autoCalc) 00525 value -= aBiasRaw[axis]; 00526 00527 return value; 00528 } 00529 00530 void LSM9DS1::readMag() 00531 { 00532 uint8_t temp[6]; // We'll read six bytes from the mag into temp 00533 mReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M 00534 mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx 00535 my = (temp[3] << 8) | temp[2]; // Store y-axis values into my 00536 mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz 00537 mx = mx - mBiasRaw[0]; 00538 my = my - mBiasRaw[1]; 00539 mz = mz - mBiasRaw[2]; 00540 } 00541 00542 int16_t LSM9DS1::readMag(lsm9ds1_axis axis) 00543 { 00544 uint8_t temp[2]; 00545 mReadBytes(OUT_X_L_M + (2 * axis), temp, 2); 00546 return (temp[1] << 8) | temp[0]; 00547 } 00548 00549 void LSM9DS1::readTemp() 00550 { 00551 uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp 00552 xgReadBytes(OUT_TEMP_L, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L 00553 temperature = (int16_t)((temp[1] << 8) | temp[0]); 00554 } 00555 00556 void LSM9DS1::readGyro() 00557 { 00558 uint8_t temp[6]; // We'll read six bytes from the gyro into temp 00559 xgReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G 00560 gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx 00561 gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy 00562 gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz 00563 if (_autoCalc) { 00564 gx -= gBiasRaw[X_AXIS]; 00565 gy -= gBiasRaw[Y_AXIS]; 00566 gz -= gBiasRaw[Z_AXIS]; 00567 } 00568 } 00569 00570 int16_t LSM9DS1::readGyro(lsm9ds1_axis axis) 00571 { 00572 uint8_t temp[2]; 00573 int16_t value; 00574 00575 xgReadBytes(OUT_X_L_G + (2 * axis), temp, 2); 00576 00577 value = (temp[1] << 8) | temp[0]; 00578 00579 if (_autoCalc) 00580 value -= gBiasRaw[axis]; 00581 00582 return value; 00583 } 00584 00585 float LSM9DS1::calcGyro(int16_t gyro) 00586 { 00587 // Return the gyro raw reading times our pre-calculated DPS / (ADC tick): 00588 return gRes * gyro; 00589 } 00590 00591 float LSM9DS1::calcAccel(int16_t accel) 00592 { 00593 // Return the accel raw reading times our pre-calculated g's / (ADC tick): 00594 return aRes * accel; 00595 } 00596 00597 float LSM9DS1::calcMag(int16_t mag) 00598 { 00599 // Return the mag raw reading times our pre-calculated Gs / (ADC tick): 00600 return mRes * mag; 00601 } 00602 00603 void LSM9DS1::setGyroScale(uint16_t gScl) 00604 { 00605 // Read current value of CTRL_REG1_G: 00606 uint8_t ctrl1RegValue = xgReadByte(CTRL_REG1_G); 00607 // Mask out scale bits (3 & 4): 00608 ctrl1RegValue &= 0xE7; 00609 switch (gScl) { 00610 case 500: 00611 ctrl1RegValue |= (0x1 << 3); 00612 settings.gyro.scale = 500; 00613 break; 00614 case 2000: 00615 ctrl1RegValue |= (0x3 << 3); 00616 settings.gyro.scale = 2000; 00617 break; 00618 default: // Otherwise we'll set it to 245 dps (0x0 << 4) 00619 settings.gyro.scale = 245; 00620 break; 00621 } 00622 xgWriteByte(CTRL_REG1_G, ctrl1RegValue); 00623 00624 calcgRes(); 00625 } 00626 00627 void LSM9DS1::setAccelScale(uint8_t aScl) 00628 { 00629 // We need to preserve the other bytes in CTRL_REG6_XL. So, first read it: 00630 uint8_t tempRegValue = xgReadByte(CTRL_REG6_XL); 00631 // Mask out accel scale bits: 00632 tempRegValue &= 0xE7; 00633 00634 switch (aScl) { 00635 case 4: 00636 tempRegValue |= (0x2 << 3); 00637 settings.accel.scale = 4; 00638 break; 00639 case 8: 00640 tempRegValue |= (0x3 << 3); 00641 settings.accel.scale = 8; 00642 break; 00643 case 16: 00644 tempRegValue |= (0x1 << 3); 00645 settings.accel.scale = 16; 00646 break; 00647 default: // Otherwise it'll be set to 2g (0x0 << 3) 00648 settings.accel.scale = 2; 00649 break; 00650 } 00651 xgWriteByte(CTRL_REG6_XL, tempRegValue); 00652 00653 // Then calculate a new aRes, which relies on aScale being set correctly: 00654 calcaRes(); 00655 } 00656 00657 void LSM9DS1::setMagScale(uint8_t mScl) 00658 { 00659 // We need to preserve the other bytes in CTRL_REG6_XM. So, first read it: 00660 uint8_t temp = mReadByte(CTRL_REG2_M); 00661 // Then mask out the mag scale bits: 00662 temp &= 0xFF^(0x3 << 5); 00663 00664 switch (mScl) { 00665 case 8: 00666 temp |= (0x1 << 5); 00667 settings.mag.scale = 8; 00668 break; 00669 case 12: 00670 temp |= (0x2 << 5); 00671 settings.mag.scale = 12; 00672 break; 00673 case 16: 00674 temp |= (0x3 << 5); 00675 settings.mag.scale = 16; 00676 break; 00677 default: // Otherwise we'll default to 4 gauss (00) 00678 settings.mag.scale = 4; 00679 break; 00680 } 00681 00682 // And write the new register value back into CTRL_REG6_XM: 00683 mWriteByte(CTRL_REG2_M, temp); 00684 00685 // We've updated the sensor, but we also need to update our class variables 00686 // First update mScale: 00687 //mScale = mScl; 00688 // Then calculate a new mRes, which relies on mScale being set correctly: 00689 calcmRes(); 00690 } 00691 00692 void LSM9DS1::setGyroODR(uint8_t gRate) 00693 { 00694 // Only do this if gRate is not 0 (which would disable the gyro) 00695 if ((gRate & 0x07) != 0) { 00696 // We need to preserve the other bytes in CTRL_REG1_G. So, first read it: 00697 uint8_t temp = xgReadByte(CTRL_REG1_G); 00698 // Then mask out the gyro ODR bits: 00699 temp &= 0xFF^(0x7 << 5); 00700 temp |= (gRate & 0x07) << 5; 00701 // Update our settings struct 00702 settings.gyro.sampleRate = gRate & 0x07; 00703 // And write the new register value back into CTRL_REG1_G: 00704 xgWriteByte(CTRL_REG1_G, temp); 00705 } 00706 } 00707 00708 void LSM9DS1::setAccelODR(uint8_t aRate) 00709 { 00710 // Only do this if aRate is not 0 (which would disable the accel) 00711 if ((aRate & 0x07) != 0) { 00712 // We need to preserve the other bytes in CTRL_REG1_XM. So, first read it: 00713 uint8_t temp = xgReadByte(CTRL_REG6_XL); 00714 // Then mask out the accel ODR bits: 00715 temp &= 0x1F; 00716 // Then shift in our new ODR bits: 00717 temp |= ((aRate & 0x07) << 5); 00718 settings.accel.sampleRate = aRate & 0x07; 00719 // And write the new register value back into CTRL_REG1_XM: 00720 xgWriteByte(CTRL_REG6_XL, temp); 00721 } 00722 } 00723 00724 void LSM9DS1::setMagODR(uint8_t mRate) 00725 { 00726 // We need to preserve the other bytes in CTRL_REG5_XM. So, first read it: 00727 uint8_t temp = mReadByte(CTRL_REG1_M); 00728 // Then mask out the mag ODR bits: 00729 temp &= 0xFF^(0x7 << 2); 00730 // Then shift in our new ODR bits: 00731 temp |= ((mRate & 0x07) << 2); 00732 settings.mag.sampleRate = mRate & 0x07; 00733 // And write the new register value back into CTRL_REG5_XM: 00734 mWriteByte(CTRL_REG1_M, temp); 00735 } 00736 00737 void LSM9DS1::calcgRes() 00738 { 00739 gRes = ((float) settings.gyro.scale) / 32768.0; 00740 } 00741 00742 void LSM9DS1::calcaRes() 00743 { 00744 aRes = ((float) settings.accel.scale) / 32768.0; 00745 } 00746 00747 void LSM9DS1::calcmRes() 00748 { 00749 //mRes = ((float) settings.mag.scale) / 32768.0; 00750 switch (settings.mag.scale) { 00751 case 4: 00752 mRes = magSensitivity[0]; 00753 break; 00754 case 8: 00755 mRes = magSensitivity[1]; 00756 break; 00757 case 12: 00758 mRes = magSensitivity[2]; 00759 break; 00760 case 16: 00761 mRes = magSensitivity[3]; 00762 break; 00763 } 00764 00765 } 00766 00767 void LSM9DS1::configInt(interrupt_select interrupt, uint8_t generator, 00768 h_lactive activeLow, pp_od pushPull) 00769 { 00770 // Write to INT1_CTRL or INT2_CTRL. [interupt] should already be one of 00771 // those two values. 00772 // [generator] should be an OR'd list of values from the interrupt_generators enum 00773 xgWriteByte(interrupt, generator); 00774 00775 // Configure CTRL_REG8 00776 uint8_t temp; 00777 temp = xgReadByte(CTRL_REG8); 00778 00779 if (activeLow) temp |= (1<<5); 00780 else temp &= ~(1<<5); 00781 00782 if (pushPull) temp &= ~(1<<4); 00783 else temp |= (1<<4); 00784 00785 xgWriteByte(CTRL_REG8, temp); 00786 } 00787 00788 void LSM9DS1::configInactivity(uint8_t duration, uint8_t threshold, bool sleepOn) 00789 { 00790 uint8_t temp = 0; 00791 00792 temp = threshold & 0x7F; 00793 if (sleepOn) temp |= (1<<7); 00794 xgWriteByte(ACT_THS, temp); 00795 00796 xgWriteByte(ACT_DUR, duration); 00797 } 00798 00799 uint8_t LSM9DS1::getInactivity() 00800 { 00801 uint8_t temp = xgReadByte(STATUS_REG_0); 00802 temp &= (0x10); 00803 return temp; 00804 } 00805 00806 void LSM9DS1::configAccelInt(uint8_t generator, bool andInterrupts) 00807 { 00808 // Use variables from accel_interrupt_generator, OR'd together to create 00809 // the [generator]value. 00810 uint8_t temp = generator; 00811 if (andInterrupts) temp |= 0x80; 00812 xgWriteByte(INT_GEN_CFG_XL, temp); 00813 } 00814 00815 void LSM9DS1::configAccelThs(uint8_t threshold, lsm9ds1_axis axis, uint8_t duration, bool wait) 00816 { 00817 // Write threshold value to INT_GEN_THS_?_XL. 00818 // axis will be 0, 1, or 2 (x, y, z respectively) 00819 xgWriteByte(INT_GEN_THS_X_XL + axis, threshold); 00820 00821 // Write duration and wait to INT_GEN_DUR_XL 00822 uint8_t temp; 00823 temp = (duration & 0x7F); 00824 if (wait) temp |= 0x80; 00825 xgWriteByte(INT_GEN_DUR_XL, temp); 00826 } 00827 00828 uint8_t LSM9DS1::getAccelIntSrc() 00829 { 00830 uint8_t intSrc = xgReadByte(INT_GEN_SRC_XL); 00831 00832 // Check if the IA_XL (interrupt active) bit is set 00833 if (intSrc & (1<<6)) { 00834 return (intSrc & 0x3F); 00835 } 00836 00837 return 0; 00838 } 00839 00840 void LSM9DS1::configGyroInt(uint8_t generator, bool aoi, bool latch) 00841 { 00842 // Use variables from accel_interrupt_generator, OR'd together to create 00843 // the [generator]value. 00844 uint8_t temp = generator; 00845 if (aoi) temp |= 0x80; 00846 if (latch) temp |= 0x40; 00847 xgWriteByte(INT_GEN_CFG_G, temp); 00848 } 00849 00850 void LSM9DS1::configGyroThs(int16_t threshold, lsm9ds1_axis axis, uint8_t duration, bool wait) 00851 { 00852 uint8_t buffer[2]; 00853 buffer[0] = (threshold & 0x7F00) >> 8; 00854 buffer[1] = (threshold & 0x00FF); 00855 // Write threshold value to INT_GEN_THS_?H_G and INT_GEN_THS_?L_G. 00856 // axis will be 0, 1, or 2 (x, y, z respectively) 00857 xgWriteByte(INT_GEN_THS_XH_G + (axis * 2), buffer[0]); 00858 xgWriteByte(INT_GEN_THS_XH_G + 1 + (axis * 2), buffer[1]); 00859 00860 // Write duration and wait to INT_GEN_DUR_XL 00861 uint8_t temp; 00862 temp = (duration & 0x7F); 00863 if (wait) temp |= 0x80; 00864 xgWriteByte(INT_GEN_DUR_G, temp); 00865 } 00866 00867 uint8_t LSM9DS1::getGyroIntSrc() 00868 { 00869 uint8_t intSrc = xgReadByte(INT_GEN_SRC_G); 00870 00871 // Check if the IA_G (interrupt active) bit is set 00872 if (intSrc & (1<<6)) { 00873 return (intSrc & 0x3F); 00874 } 00875 00876 return 0; 00877 } 00878 00879 void LSM9DS1::configMagInt(uint8_t generator, h_lactive activeLow, bool latch) 00880 { 00881 // Mask out non-generator bits (0-4) 00882 uint8_t config = (generator & 0xE0); 00883 // IEA bit is 0 for active-low, 1 for active-high. 00884 if (activeLow == INT_ACTIVE_HIGH) config |= (1<<2); 00885 // IEL bit is 0 for latched, 1 for not-latched 00886 if (!latch) config |= (1<<1); 00887 // As long as we have at least 1 generator, enable the interrupt 00888 if (generator != 0) config |= (1<<0); 00889 00890 mWriteByte(INT_CFG_M, config); 00891 } 00892 00893 void LSM9DS1::configMagThs(uint16_t threshold) 00894 { 00895 // Write high eight bits of [threshold] to INT_THS_H_M 00896 mWriteByte(INT_THS_H_M, uint8_t((threshold & 0x7F00) >> 8)); 00897 // Write low eight bits of [threshold] to INT_THS_L_M 00898 mWriteByte(INT_THS_L_M, uint8_t(threshold & 0x00FF)); 00899 } 00900 00901 uint8_t LSM9DS1::getMagIntSrc() 00902 { 00903 uint8_t intSrc = mReadByte(INT_SRC_M); 00904 00905 // Check if the INT (interrupt active) bit is set 00906 if (intSrc & (1<<0)) { 00907 return (intSrc & 0xFE); 00908 } 00909 00910 return 0; 00911 } 00912 00913 void LSM9DS1::sleepGyro(bool enable) 00914 { 00915 uint8_t temp = xgReadByte(CTRL_REG9); 00916 if (enable) temp |= (1<<6); 00917 else temp &= ~(1<<6); 00918 xgWriteByte(CTRL_REG9, temp); 00919 } 00920 00921 void LSM9DS1::enableFIFO(bool enable) 00922 { 00923 uint8_t temp = xgReadByte(CTRL_REG9); 00924 if (enable) temp |= (1<<1); 00925 else temp &= ~(1<<1); 00926 xgWriteByte(CTRL_REG9, temp); 00927 } 00928 00929 void LSM9DS1::setFIFO(fifoMode_type fifoMode, uint8_t fifoThs) 00930 { 00931 // Limit threshold - 0x1F (31) is the maximum. If more than that was asked 00932 // limit it to the maximum. 00933 uint8_t threshold = fifoThs <= 0x1F ? fifoThs : 0x1F; 00934 xgWriteByte(FIFO_CTRL, ((fifoMode & 0x7) << 5) | (threshold & 0x1F)); 00935 } 00936 00937 uint8_t LSM9DS1::getFIFOSamples() 00938 { 00939 return (xgReadByte(FIFO_SRC) & 0x3F); 00940 } 00941 00942 void LSM9DS1::constrainScales() 00943 { 00944 if ((settings.gyro.scale != 245) && (settings.gyro.scale != 500) && 00945 (settings.gyro.scale != 2000)) { 00946 settings.gyro.scale = 245; 00947 } 00948 00949 if ((settings.accel.scale != 2) && (settings.accel.scale != 4) && 00950 (settings.accel.scale != 8) && (settings.accel.scale != 16)) { 00951 settings.accel.scale = 2; 00952 } 00953 00954 if ((settings.mag.scale != 4) && (settings.mag.scale != 8) && 00955 (settings.mag.scale != 12) && (settings.mag.scale != 16)) { 00956 settings.mag.scale = 4; 00957 } 00958 } 00959 00960 void LSM9DS1::xgWriteByte(uint8_t subAddress, uint8_t data) 00961 { 00962 // Whether we're using I2C or SPI, write a byte using the 00963 // gyro-specific I2C address or SPI CS pin. 00964 if (settings.device.commInterface == IMU_MODE_I2C) { 00965 pc.printf("yo"); 00966 I2CwriteByte(_xgAddress, subAddress, data); 00967 } else if (settings.device.commInterface == IMU_MODE_SPI) { 00968 SPIwriteByte(_xgAddress, subAddress, data); 00969 } 00970 } 00971 00972 void LSM9DS1::mWriteByte(uint8_t subAddress, uint8_t data) 00973 { 00974 // Whether we're using I2C or SPI, write a byte using the 00975 // accelerometer-specific I2C address or SPI CS pin. 00976 if (settings.device.commInterface == IMU_MODE_I2C) { 00977 pc.printf("mo"); 00978 return I2CwriteByte(_mAddress, subAddress, data); 00979 } else if (settings.device.commInterface == IMU_MODE_SPI) 00980 return SPIwriteByte(_mAddress, subAddress, data); 00981 } 00982 00983 uint8_t LSM9DS1::xgReadByte(uint8_t subAddress) 00984 { 00985 // Whether we're using I2C or SPI, read a byte using the 00986 // gyro-specific I2C address or SPI CS pin. 00987 if (settings.device.commInterface == IMU_MODE_I2C) 00988 return I2CreadByte(_xgAddress, subAddress); 00989 else if (settings.device.commInterface == IMU_MODE_SPI) 00990 return SPIreadByte(_xgAddress, subAddress); 00991 } 00992 00993 void LSM9DS1::xgReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count) 00994 { 00995 // Whether we're using I2C or SPI, read multiple bytes using the 00996 // gyro-specific I2C address or SPI CS pin. 00997 if (settings.device.commInterface == IMU_MODE_I2C) { 00998 I2CreadBytes(_xgAddress, subAddress, dest, count); 00999 } else if (settings.device.commInterface == IMU_MODE_SPI) { 01000 SPIreadBytes(_xgAddress, subAddress, dest, count); 01001 } 01002 } 01003 01004 uint8_t LSM9DS1::mReadByte(uint8_t subAddress) 01005 { 01006 // Whether we're using I2C or SPI, read a byte using the 01007 // accelerometer-specific I2C address or SPI CS pin. 01008 if (settings.device.commInterface == IMU_MODE_I2C) 01009 return I2CreadByte(_mAddress, subAddress); 01010 else if (settings.device.commInterface == IMU_MODE_SPI) 01011 return SPIreadByte(_mAddress, subAddress); 01012 } 01013 01014 void LSM9DS1::mReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count) 01015 { 01016 // Whether we're using I2C or SPI, read multiple bytes using the 01017 // accelerometer-specific I2C address or SPI CS pin. 01018 if (settings.device.commInterface == IMU_MODE_I2C) 01019 I2CreadBytes(_mAddress, subAddress, dest, count); 01020 else if (settings.device.commInterface == IMU_MODE_SPI) 01021 SPIreadBytes(_mAddress, subAddress, dest, count); 01022 } 01023 01024 void LSM9DS1::initSPI() 01025 { 01026 /* 01027 pinMode(_xgAddress, OUTPUT); 01028 digitalWrite(_xgAddress, HIGH); 01029 pinMode(_mAddress, OUTPUT); 01030 digitalWrite(_mAddress, HIGH); 01031 01032 SPI.begin(); 01033 // Maximum SPI frequency is 10MHz, could divide by 2 here: 01034 SPI.setClockDivider(SPI_CLOCK_DIV2); 01035 // Data is read and written MSb first. 01036 SPI.setBitOrder(MSBFIRST); 01037 // Data is captured on rising edge of clock (CPHA = 0) 01038 // Base value of the clock is HIGH (CPOL = 1) 01039 SPI.setDataMode(SPI_MODE0); 01040 */ 01041 } 01042 01043 void LSM9DS1::SPIwriteByte(uint8_t csPin, uint8_t subAddress, uint8_t data) 01044 { 01045 /* 01046 digitalWrite(csPin, LOW); // Initiate communication 01047 01048 // If write, bit 0 (MSB) should be 0 01049 // If single write, bit 1 should be 0 01050 SPI.transfer(subAddress & 0x3F); // Send Address 01051 SPI.transfer(data); // Send data 01052 01053 digitalWrite(csPin, HIGH); // Close communication 01054 */ 01055 } 01056 01057 uint8_t LSM9DS1::SPIreadByte(uint8_t csPin, uint8_t subAddress) 01058 { 01059 uint8_t temp; 01060 // Use the multiple read function to read 1 byte. 01061 // Value is returned to `temp`. 01062 SPIreadBytes(csPin, subAddress, &temp, 1); 01063 return temp; 01064 } 01065 01066 void LSM9DS1::SPIreadBytes(uint8_t csPin, uint8_t subAddress, 01067 uint8_t * dest, uint8_t count) 01068 { 01069 // To indicate a read, set bit 0 (msb) of first byte to 1 01070 uint8_t rAddress = 0x80 | (subAddress & 0x3F); 01071 // Mag SPI port is different. If we're reading multiple bytes, 01072 // set bit 1 to 1. The remaining six bytes are the address to be read 01073 if ((csPin == _mAddress) && count > 1) 01074 rAddress |= 0x40; 01075 01076 /* 01077 digitalWrite(csPin, LOW); // Initiate communication 01078 SPI.transfer(rAddress); 01079 for (int i=0; i<count; i++) 01080 { 01081 dest[i] = SPI.transfer(0x00); // Read into destination array 01082 } 01083 digitalWrite(csPin, HIGH); // Close communication 01084 */ 01085 } 01086 01087 void LSM9DS1::initI2C() 01088 { 01089 /* 01090 Wire.begin(); // Initialize I2C library 01091 */ 01092 01093 //already initialized in constructor! 01094 } 01095 01096 // Wire.h read and write protocols 01097 void LSM9DS1::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data) 01098 { 01099 /* 01100 Wire.beginTransmission(address); // Initialize the Tx buffer 01101 Wire.write(subAddress); // Put slave register address in Tx buffer 01102 Wire.write(data); // Put data in Tx buffer 01103 Wire.endTransmission(); // Send the Tx buffer 01104 */ 01105 char temp_data[2] = {subAddress, data}; 01106 i2c.write(address, temp_data, 2); 01107 } 01108 01109 uint8_t LSM9DS1::I2CreadByte(uint8_t address, uint8_t subAddress) 01110 { 01111 /* 01112 int timeout = LSM9DS1_COMMUNICATION_TIMEOUT; 01113 uint8_t data; // `data` will store the register data 01114 01115 Wire.beginTransmission(address); // Initialize the Tx buffer 01116 Wire.write(subAddress); // Put slave register address in Tx buffer 01117 Wire.endTransmission(true); // Send the Tx buffer, but send a restart to keep connection alive 01118 Wire.requestFrom(address, (uint8_t) 1); // Read one byte from slave register address 01119 while ((Wire.available() < 1) && (timeout-- > 0)) 01120 delay(1); 01121 01122 if (timeout <= 0) 01123 return 255; //! Bad! 255 will be misinterpreted as a good value. 01124 01125 data = Wire.read(); // Fill Rx buffer with result 01126 return data; // Return data read from slave register 01127 */ 01128 char data; 01129 char temp[2] = {subAddress}; 01130 01131 i2c.write(address, temp, 1); 01132 //i2c.write(address & 0xFE); 01133 temp[1] = 0x00; 01134 i2c.write(address, temp, 1); 01135 //i2c.write( address | 0x01); 01136 int a = i2c.read(address, &data, 1); 01137 return data; 01138 } 01139 01140 uint8_t LSM9DS1::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count) 01141 { 01142 /* 01143 int timeout = LSM9DS1_COMMUNICATION_TIMEOUT; 01144 Wire.beginTransmission(address); // Initialize the Tx buffer 01145 // Next send the register to be read. OR with 0x80 to indicate multi-read. 01146 Wire.write(subAddress | 0x80); // Put slave register address in Tx buffer 01147 01148 Wire.endTransmission(true); // Send the Tx buffer, but send a restart to keep connection alive 01149 uint8_t i = 0; 01150 Wire.requestFrom(address, count); // Read bytes from slave register address 01151 while ((Wire.available() < count) && (timeout-- > 0)) 01152 delay(1); 01153 if (timeout <= 0) 01154 return -1; 01155 01156 for (int i=0; i<count;) 01157 { 01158 if (Wire.available()) 01159 { 01160 dest[i++] = Wire.read(); 01161 } 01162 } 01163 return count; 01164 */ 01165 int i; 01166 char temp_dest[count]; 01167 char temp[1] = {subAddress}; 01168 i2c.write(address, temp, 1); 01169 i2c.read(address, temp_dest, count); 01170 01171 //i2c doesn't take uint8_ts, but rather chars so do this nasty af conversion 01172 for (i=0; i < count; i++) { 01173 dest[i] = temp_dest[i]; 01174 } 01175 return count; 01176 }
Generated on Sun Jul 24 2022 01:04:11 by
1.7.2