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