LSM9DS0.cpp

00001 //Original author
00002 /******************************************************************************
00003 SFE_LSM9DS0.cpp
00004 SFE_LSM9DS0 Library Source File
00005 Jim Lindblom @ SparkFun Electronics
00006 Original Creation Date: February 14, 2014 (Happy Valentines Day!)
00007 https://github.com/sparkfun/LSM9DS0_Breakout
00008 
00009 This file implements all functions of the LSM9DS0 class. Functions here range
00010 from higher level stuff, like reading/writing LSM9DS0 registers to low-level,
00011 hardware reads and writes. Both SPI and I2C handler functions can be found
00012 towards the bottom of this file.
00013 
00014 Development environment specifics:
00015     IDE: Arduino 1.0.5
00016     Hardware Platform: Arduino Pro 3.3V/8MHz
00017     LSM9DS0 Breakout Version: 1.0
00018 
00019 This code is beerware; if you see me (or any other SparkFun employee) at the
00020 local, and you've found our code helpful, please buy us a round!
00021 
00022 Distributed as-is; no warranty is given.
00023 ******************************************************************************/
00024 
00025 #include "LSM9DS0.h"
00026 #include "mbed.h"
00027 
00028 //I2C i2c(D14,D15);
00029 //SPI spi(D4,D5,D3);
00030 //****************************************************************************//
00031 //
00032 //  LSM9DS0 functions.
00033 //
00034 //  Construction arguments:
00035 //  (interface_mode interface, uint8_t gAddr, uint8_t xmAddr ),
00036 //
00037 //    where gAddr and xmAddr are addresses for I2C_MODE and chip select pin
00038 //    number for SPI_MODE
00039 //
00040 //  For SPI, construct LSM6DS3 myIMU(SPI_MODE, D9, D6);
00041 //
00042 //=================================
00043 
00044 LSM9DS0::LSM9DS0(interface_mode interface, uint8_t gAddr, uint8_t xmAddr) : interfaceMode(SPI_MODE), spi_(D4,D5,D3), i2c_(I2C_SDA,I2C_SCL), csG_(D9), csXM_(D6)
00045 {
00046     // interfaceMode will keep track of whether we're using SPI or I2C:
00047     interfaceMode = interface;
00048     
00049     // xmAddress and gAddress will store the 7-bit I2C address, if using I2C.
00050     // If we're using SPI, these variables store the chip-select pins.
00051     gAddress = gAddr;
00052     xmAddress = xmAddr;
00053 }
00054 
00055 uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl, 
00056                         gyro_odr gODR, accel_odr aODR, mag_odr mODR)
00057 {
00058     // Store the given scales in class variables. These scale variables
00059     // are used throughout to calculate the actual g's, DPS,and Gs's.
00060     gScale = gScl;
00061     aScale = aScl;
00062     mScale = mScl;
00063     
00064     // Once we have the scale values, we can calculate the resolution
00065     // of each sensor. That's what these functions are for. One for each sensor
00066     calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
00067     calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
00068     calcaRes(); // Calculate g / ADC tick, stored in aRes variable
00069     
00070     // Now, initialize our hardware interface.
00071     if (interfaceMode == I2C_MODE)  // If we're using I2C
00072         initI2C();                  // Initialize I2C
00073     else if (interfaceMode == SPI_MODE)     // else, if we're using SPI
00074         initSPI();                          // Initialize SPI
00075     
00076     // To verify communication, we can read from the WHO_AM_I register of
00077     // each device. Store those in a variable so we can return them.
00078     uint8_t gTest = gReadByte(WHO_AM_I_G);      // Read the gyro WHO_AM_I
00079     uint8_t xmTest = xmReadByte(WHO_AM_I_XM);   // Read the accel/mag WHO_AM_I
00080     
00081     // Gyro initialization stuff:
00082     initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
00083     setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
00084     setGyroScale(gScale); // Set the gyro range
00085     
00086     // Accelerometer initialization stuff:
00087     initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
00088     setAccelODR(aODR); // Set the accel data rate.
00089     setAccelScale(aScale); // Set the accel range.
00090     
00091     // Magnetometer initialization stuff:
00092     initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
00093     setMagODR(mODR); // Set the magnetometer output data rate.
00094     setMagScale(mScale); // Set the magnetometer's range.
00095     
00096     setGyroOffset(0,0,0);
00097     setAccelOffset(0,0,0);
00098     setMagOffset(0,0,0);
00099     
00100     // Once everything is initialized, return the WHO_AM_I registers we read:
00101     return (xmTest << 8) | gTest;
00102 }
00103 
00104 void LSM9DS0::initGyro()
00105 {
00106     /* CTRL_REG1_G sets output data rate, bandwidth, power-down and enables
00107     Bits[7:0]: DR1 DR0 BW1 BW0 PD Zen Xen Yen
00108     DR[1:0] - Output data rate selection
00109         00=95Hz, 01=190Hz, 10=380Hz, 11=760Hz
00110     BW[1:0] - Bandwidth selection (sets cutoff frequency)
00111          Value depends on ODR. See datasheet table 21.
00112     PD - Power down enable (0=power down mode, 1=normal or sleep mode)
00113     Zen, Xen, Yen - Axis enable (o=disabled, 1=enabled) */
00114     gWriteByte(CTRL_REG1_G, 0xFF); // Normal mode, enable all axes
00115     
00116     /* CTRL_REG2_G sets up the HPF
00117     Bits[7:0]: 0 0 HPM1 HPM0 HPCF3 HPCF2 HPCF1 HPCF0
00118     HPM[1:0] - High pass filter mode selection
00119         00=normal (reset reading HP_RESET_FILTER, 01=ref signal for filtering,
00120         10=normal, 11=autoreset on interrupt
00121     HPCF[3:0] - High pass filter cutoff frequency
00122         Value depends on data rate. See datasheet table 26.
00123     */
00124     gWriteByte(CTRL_REG2_G, 0x09); // Normal mode, high cutoff frequency
00125     
00126     /* CTRL_REG3_G sets up interrupt and DRDY_G pins
00127     Bits[7:0]: I1_IINT1 I1_BOOT H_LACTIVE PP_OD I2_DRDY I2_WTM I2_ORUN I2_EMPTY
00128     I1_INT1 - Interrupt enable on INT_G pin (0=disable, 1=enable)
00129     I1_BOOT - Boot status available on INT_G (0=disable, 1=enable)
00130     H_LACTIVE - Interrupt active configuration on INT_G (0:high, 1:low)
00131     PP_OD - Push-pull/open-drain (0=push-pull, 1=open-drain)
00132     I2_DRDY - Data ready on DRDY_G (0=disable, 1=enable)
00133     I2_WTM - FIFO watermark interrupt on DRDY_G (0=disable 1=enable)
00134     I2_ORUN - FIFO overrun interrupt on DRDY_G (0=disable 1=enable)
00135     I2_EMPTY - FIFO empty interrupt on DRDY_G (0=disable 1=enable) */
00136     // Int1 enabled (pp, active low), data read on DRDY_G:
00137     gWriteByte(CTRL_REG3_G, 0x00); 
00138     
00139     /* CTRL_REG4_G sets the scale, update mode
00140     Bits[7:0] - BDU BLE FS1 FS0 - ST1 ST0 SIM
00141     BDU - Block data update (0=continuous, 1=output not updated until read
00142     BLE - Big/little endian (0=data LSB @ lower address, 1=LSB @ higher add)
00143     FS[1:0] - Full-scale selection
00144         00=245dps, 01=500dps, 10=2000dps, 11=2000dps
00145     ST[1:0] - Self-test enable
00146         00=disabled, 01=st 0 (x+, y-, z-), 10=undefined, 11=st 1 (x-, y+, z+)
00147     SIM - SPI serial interface mode select
00148         0=4 wire, 1=3 wire */
00149     gWriteByte(CTRL_REG4_G, 0x30); // Set scale to 245 dps
00150     
00151     /* CTRL_REG5_G sets up the FIFO, HPF, and INT1
00152     Bits[7:0] - BOOT FIFO_EN - HPen INT1_Sel1 INT1_Sel0 Out_Sel1 Out_Sel0
00153     BOOT - Reboot memory content (0=normal, 1=reboot)
00154     FIFO_EN - FIFO enable (0=disable, 1=enable)
00155     HPen - HPF enable (0=disable, 1=enable)
00156     INT1_Sel[1:0] - Int 1 selection configuration
00157     Out_Sel[1:0] - Out selection configuration */
00158     gWriteByte(CTRL_REG5_G, 0x00);
00159     
00160     // Temporary !!! For testing !!! Remove !!! Or make useful !!!
00161     configGyroInt(0x2A, 0, 0, 0, 0); // Trigger interrupt when above 0 DPS...
00162 }
00163 
00164 void LSM9DS0::initAccel()
00165 {
00166     /* CTRL_REG0_XM (0x1F) (Default value: 0x00)
00167     Bits (7-0): BOOT FIFO_EN WTM_EN 0 0 HP_CLICK HPIS1 HPIS2
00168     BOOT - Reboot memory content (0: normal, 1: reboot)
00169     FIFO_EN - Fifo enable (0: disable, 1: enable)
00170     WTM_EN - FIFO watermark enable (0: disable, 1: enable)
00171     HP_CLICK - HPF enabled for click (0: filter bypassed, 1: enabled)
00172     HPIS1 - HPF enabled for interrupt generator 1 (0: bypassed, 1: enabled)
00173     HPIS2 - HPF enabled for interrupt generator 2 (0: bypassed, 1 enabled)   */
00174     xmWriteByte(CTRL_REG0_XM, 0x00);
00175     
00176     /* CTRL_REG1_XM (0x20) (Default value: 0x07)
00177     Bits (7-0): AODR3 AODR2 AODR1 AODR0 BDU AZEN AYEN AXEN
00178     AODR[3:0] - select the acceleration data rate:
00179         0000=power down, 0001=3.125Hz, 0010=6.25Hz, 0011=12.5Hz, 
00180         0100=25Hz, 0101=50Hz, 0110=100Hz, 0111=200Hz, 1000=400Hz,
00181         1001=800Hz, 1010=1600Hz, (remaining combinations undefined).
00182     BDU - block data update for accel AND mag
00183         0: Continuous update
00184         1: Output registers aren't updated until MSB and LSB have been read.
00185     AZEN, AYEN, and AXEN - Acceleration x/y/z-axis enabled.
00186         0: Axis disabled, 1: Axis enabled                                    */ 
00187     xmWriteByte(CTRL_REG1_XM, 0x97); // 100Hz data rate, x/y/z all enabled
00188     
00189     //Serial.println(xmReadByte(CTRL_REG1_XM));
00190     /* CTRL_REG2_XM (0x21) (Default value: 0x00)
00191     Bits (7-0): ABW1 ABW0 AFS2 AFS1 AFS0 AST1 AST0 SIM
00192     ABW[1:0] - Accelerometer anti-alias filter bandwidth
00193         00=773Hz, 01=194Hz, 10=362Hz, 11=50Hz
00194     AFS[2:0] - Accel full-scale selection
00195         000=+/-2g, 001=+/-4g, 010=+/-6g, 011=+/-8g, 100=+/-16g
00196     AST[1:0] - Accel self-test enable
00197         00=normal (no self-test), 01=positive st, 10=negative st, 11=not allowed
00198     SIM - SPI mode selection
00199         0=4-wire, 1=3-wire                                                   */
00200     xmWriteByte(CTRL_REG2_XM, 0xD8); // Set scale to 2g
00201     
00202     /* CTRL_REG3_XM is used to set interrupt generators on INT1_XM
00203     Bits (7-0): P1_BOOT P1_TAP P1_INT1 P1_INT2 P1_INTM P1_DRDYA P1_DRDYM P1_EMPTY
00204     */
00205     // Accelerometer data ready on INT1_XM (0x04)
00206     xmWriteByte(CTRL_REG3_XM, 0x00); 
00207 }
00208 
00209 void LSM9DS0::initMag()
00210 {   
00211     /* CTRL_REG5_XM enables temp sensor, sets mag resolution and data rate
00212     Bits (7-0): TEMP_EN M_RES1 M_RES0 M_ODR2 M_ODR1 M_ODR0 LIR2 LIR1
00213     TEMP_EN - Enable temperature sensor (0=disabled, 1=enabled)
00214     M_RES[1:0] - Magnetometer resolution select (0=low, 3=high)
00215     M_ODR[2:0] - Magnetometer data rate select
00216         000=3.125Hz, 001=6.25Hz, 010=12.5Hz, 011=25Hz, 100=50Hz, 101=100Hz
00217     LIR2 - Latch interrupt request on INT2_SRC (cleared by reading INT2_SRC)
00218         0=interrupt request not latched, 1=interrupt request latched
00219     LIR1 - Latch interrupt request on INT1_SRC (cleared by readging INT1_SRC)
00220         0=irq not latched, 1=irq latched                                     */
00221     xmWriteByte(CTRL_REG5_XM, 0x74); // Mag data rate - 100 Hz, disable temperature sensor
00222     
00223     /* CTRL_REG6_XM sets the magnetometer full-scale
00224     Bits (7-0): 0 MFS1 MFS0 0 0 0 0 0
00225     MFS[1:0] - Magnetic full-scale selection
00226     00:+/-2Gauss, 01:+/-4Gs, 10:+/-8Gs, 11:+/-12Gs                           */
00227     xmWriteByte(CTRL_REG6_XM, 0x40); // Mag scale to +/- 2GS
00228     
00229     /* CTRL_REG7_XM sets magnetic sensor mode, low power mode, and filters
00230     AHPM1 AHPM0 AFDS 0 0 MLP MD1 MD0
00231     AHPM[1:0] - HPF mode selection
00232         00=normal (resets reference registers), 01=reference signal for filtering, 
00233         10=normal, 11=autoreset on interrupt event
00234     AFDS - Filtered acceleration data selection
00235         0=internal filter bypassed, 1=data from internal filter sent to FIFO
00236     MLP - Magnetic data low-power mode
00237         0=data rate is set by M_ODR bits in CTRL_REG5
00238         1=data rate is set to 3.125Hz
00239     MD[1:0] - Magnetic sensor mode selection (default 10)
00240         00=continuous-conversion, 01=single-conversion, 10 and 11=power-down */
00241     xmWriteByte(CTRL_REG7_XM, 0x00); // Continuous conversion mode
00242     
00243     /* CTRL_REG4_XM is used to set interrupt generators on INT2_XM
00244     Bits (7-0): P2_TAP P2_INT1 P2_INT2 P2_INTM P2_DRDYA P2_DRDYM P2_Overrun P2_WTM
00245     */
00246     xmWriteByte(CTRL_REG4_XM, 0x00); // Magnetometer data ready on INT2_XM (0x08)
00247     
00248     /* INT_CTRL_REG_M to set push-pull/open drain, and active-low/high
00249     Bits[7:0] - XMIEN YMIEN ZMIEN PP_OD IEA IEL 4D MIEN
00250     XMIEN, YMIEN, ZMIEN - Enable interrupt recognition on axis for mag data
00251     PP_OD - Push-pull/open-drain interrupt configuration (0=push-pull, 1=od)
00252     IEA - Interrupt polarity for accel and magneto
00253         0=active-low, 1=active-high
00254     IEL - Latch interrupt request for accel and magneto
00255         0=irq not latched, 1=irq latched
00256     4D - 4D enable. 4D detection is enabled when 6D bit in INT_GEN1_REG is set
00257     MIEN - Enable interrupt generation for magnetic data
00258         0=disable, 1=enable) */
00259     xmWriteByte(INT_CTRL_REG_M, 0x09); // Enable interrupts for mag, active-low, push-pull
00260 }
00261 
00262 // This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average
00263 // them, scales them to  gs and deg/s, respectively, and then passes the biases to the main sketch
00264 // for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store
00265 // the data as there are in the ADXL345, a precursor to the LSM9DS0, or the MPU-9150, so we have to
00266 // subtract the biases ourselves. This results in a more accurate measurement in general and can
00267 // remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner
00268 // is good practice.
00269 void LSM9DS0::calLSM9DS0(float * gbias, float * abias)
00270 {  
00271   uint8_t data[6] = {0, 0, 0, 0, 0, 0};
00272   int16_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
00273   int samples, ii;
00274   
00275   // First get gyro bias
00276   uint8_t c = gReadByte(CTRL_REG5_G);
00277   gWriteByte(CTRL_REG5_G, c | 0x40);         // Enable gyro FIFO  
00278   wait_ms(20);                                 // Wait for change to take effect
00279   gWriteByte(FIFO_CTRL_REG_G, 0x20 | 0x1F);  // Enable gyro FIFO stream mode and set watermark at 32 samples
00280   wait_ms(1000);  // delay 1000 milliseconds to collect FIFO samples
00281   
00282   samples = (gReadByte(FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples
00283 
00284   for(ii = 0; ii < samples ; ii++) {            // Read the gyro data stored in the FIFO
00285     gReadBytes(OUT_X_L_G,  &data[0], 6);
00286     gyro_bias[0] += (((int16_t)data[1] << 8) | data[0]);
00287     gyro_bias[1] += (((int16_t)data[3] << 8) | data[2]);
00288     gyro_bias[2] += (((int16_t)data[5] << 8) | data[4]);
00289   }  
00290 
00291   gyro_bias[0] /= samples; // average the data
00292   gyro_bias[1] /= samples; 
00293   gyro_bias[2] /= samples; 
00294   
00295   gbias[0] = (float)gyro_bias[0]*gRes;  // Properly scale the data to get deg/s
00296   gbias[1] = (float)gyro_bias[1]*gRes;
00297   gbias[2] = (float)gyro_bias[2]*gRes;
00298   
00299   c = gReadByte(CTRL_REG5_G);
00300   gWriteByte(CTRL_REG5_G, c & ~0x40);  // Disable gyro FIFO  
00301   wait_ms(20);
00302   gWriteByte(FIFO_CTRL_REG_G, 0x00);   // Enable gyro bypass mode
00303   
00304 
00305   //  Now get the accelerometer biases
00306   c = xmReadByte(CTRL_REG0_XM);
00307   xmWriteByte(CTRL_REG0_XM, c | 0x40);      // Enable accelerometer FIFO  
00308   wait_ms(20);                                // Wait for change to take effect
00309   xmWriteByte(FIFO_CTRL_REG, 0x20 | 0x1F);  // Enable accelerometer FIFO stream mode and set watermark at 32 samples
00310   wait_ms(1000);  // delay 1000 milliseconds to collect FIFO samples
00311 
00312   samples = (xmReadByte(FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples
00313 
00314    for(ii = 0; ii < samples ; ii++) {          // Read the accelerometer data stored in the FIFO
00315     xmReadBytes(OUT_X_L_A, &data[0], 6);
00316     accel_bias[0] += (((int16_t)data[1] << 8) | data[0]);
00317     accel_bias[1] += (((int16_t)data[3] << 8) | data[2]);
00318     accel_bias[2] += (((int16_t)data[5] << 8) | data[4]) - (int16_t)(1.0f/aRes); // Assumes sensor facing up!
00319   }  
00320 
00321   accel_bias[0] /= samples; // average the data
00322   accel_bias[1] /= samples; 
00323   accel_bias[2] /= samples; 
00324   
00325   abias[0] = (float)accel_bias[0]*aRes; // Properly scale data to get gs
00326   abias[1] = (float)accel_bias[1]*aRes;
00327   abias[2] = (float)accel_bias[2]*aRes;
00328 
00329   c = xmReadByte(CTRL_REG0_XM);
00330   xmWriteByte(CTRL_REG0_XM, c & ~0x40);    // Disable accelerometer FIFO  
00331   wait_ms(20);
00332   xmWriteByte(FIFO_CTRL_REG, 0x00);       // Enable accelerometer bypass mode
00333 }
00334 
00335 //**********************
00336 //  Gyro section
00337 //**********************
00338 void LSM9DS0::readGyro()
00339 {
00340     uint8_t temp[6]; // We'll read six bytes from the gyro into temp
00341     gReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
00342     gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx
00343     gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
00344     gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz
00345 }
00346 
00347 void LSM9DS0::setGyroOffset(int16_t _gx, int16_t _gy, int16_t _gz)
00348 {
00349     gyroOffset[0] = _gx;
00350     gyroOffset[1] = _gy;
00351     gyroOffset[2] = _gz;
00352 }
00353 
00354 int16_t LSM9DS0::readRawGyroX( void )
00355 {
00356     uint8_t temp[2];
00357     gReadBytes(OUT_X_L_G, temp, 2);
00358     gx = (temp[1] << 8) | temp[0];
00359     return gx;
00360 }
00361 
00362 int16_t LSM9DS0::readRawGyroY( void )
00363 {
00364     uint8_t temp[2];
00365     gReadBytes(OUT_Y_L_G, temp, 2);
00366     gy = (temp[1] << 8) | temp[0];
00367     return gy;
00368 }
00369 
00370 int16_t LSM9DS0::readRawGyroZ( void )
00371 {
00372     uint8_t temp[2];
00373     gReadBytes(OUT_Z_L_G, temp, 2);
00374     gz = (temp[1] << 8) | temp[0];
00375     return gz;
00376 }
00377 
00378 float LSM9DS0::readFloatGyroX( void )
00379 {
00380     float output = calcGyro(readRawGyroX() - gyroOffset[0]);
00381     return output;
00382 }
00383 
00384 float LSM9DS0::readFloatGyroY( void )
00385 {
00386     float output = calcGyro(readRawGyroY() - gyroOffset[1]);
00387     return output;
00388 }
00389 
00390 float LSM9DS0::readFloatGyroZ( void )
00391 {
00392     float output = calcGyro(readRawGyroZ() - gyroOffset[2]);
00393     return output;
00394 }
00395 
00396 //**********************
00397 //  Accel section
00398 //**********************
00399 void LSM9DS0::readAccel()
00400 {
00401     uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp   
00402     xmReadBytes(OUT_X_L_A, temp, 6); // Read 6 bytes, beginning at OUT_X_L_A
00403     ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax
00404     ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay
00405     az = (temp[5] << 8) | temp[4]; // Store z-axis values into az
00406 }
00407 
00408 void LSM9DS0::setAccelOffset(int16_t _ax, int16_t _ay, int16_t _az)
00409 {
00410     accelOffset[0] = _ax;
00411     accelOffset[1] = _ay;
00412     accelOffset[2] = _az;
00413 }
00414 
00415 int16_t LSM9DS0::readRawAccelX( void )
00416 {
00417     uint8_t temp[2];
00418     xmReadBytes(OUT_X_L_A, temp, 2);
00419     ax = (temp[1] << 8) | temp[0];
00420     return ax;
00421 }
00422 
00423 int16_t LSM9DS0::readRawAccelY( void )
00424 {
00425     uint8_t temp[2];
00426     xmReadBytes(OUT_Y_L_A, temp, 2);
00427     ay = (temp[1] << 8) | temp[0];
00428     return ay;
00429 }
00430 
00431 int16_t LSM9DS0::readRawAccelZ( void )
00432 {
00433     uint8_t temp[2];
00434     xmReadBytes(OUT_Z_L_A, temp, 2);
00435     az = (temp[1] << 8) | temp[0];
00436     return az;
00437 }
00438 
00439 float LSM9DS0::readFloatAccelX( void )
00440 {
00441     float output = calcAccel(readRawAccelX() - accelOffset[0]);
00442     return output;
00443 }
00444 
00445 float LSM9DS0::readFloatAccelY( void )
00446 {
00447     float output = calcAccel(readRawAccelY() - accelOffset[1]);
00448     return output;
00449 }
00450 
00451 float LSM9DS0::readFloatAccelZ( void )
00452 {
00453     float output = calcAccel(readRawAccelZ() - accelOffset[2]);
00454     return output;
00455 }
00456 
00457 //**********************
00458 //  Mag section
00459 //**********************
00460 void LSM9DS0::readMag()
00461 {
00462     uint8_t temp[6]; // We'll read six bytes from the mag into temp 
00463     xmReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M
00464     mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx
00465     my = (temp[3] << 8) | temp[2]; // Store y-axis values into my
00466     mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz
00467 }
00468 
00469 void LSM9DS0::setMagOffset(int16_t _mx, int16_t _my, int16_t _mz)
00470 {
00471     magOffset[0] = _mx;
00472     magOffset[1] = _my;
00473     magOffset[2] = _mz;
00474 }
00475 
00476 int16_t LSM9DS0::readRawMagX( void )
00477 {
00478     uint8_t temp[2];
00479     xmReadBytes(OUT_X_L_M, temp, 2);
00480     mx = (temp[1] << 8) | temp[0];
00481     return mx;
00482 }
00483 
00484 int16_t LSM9DS0::readRawMagY( void )
00485 {
00486     uint8_t temp[2];
00487     xmReadBytes(OUT_Y_L_M, temp, 2);
00488     my = (temp[1] << 8) | temp[0];
00489     return my;
00490 }
00491 
00492 int16_t LSM9DS0::readRawMagZ( void )
00493 {
00494     uint8_t temp[2];
00495     xmReadBytes(OUT_Z_L_M, temp, 2);
00496     mz = (temp[1] << 8) | temp[0];
00497     return mz;
00498 }
00499 
00500 float LSM9DS0::readFloatMagX( void )
00501 {
00502     float output = calcMag(readRawMagX() - magOffset[0]);
00503     return output;
00504 }
00505 
00506 float LSM9DS0::readFloatMagY( void )
00507 {
00508     float output = calcMag(readRawMagY() - magOffset[1]);
00509     return output;
00510 }
00511 
00512 float LSM9DS0::readFloatMagZ( void )
00513 {
00514     float output = calcMag(readRawMagZ() - magOffset[2]);
00515     return output;
00516 }
00517 
00518 //**********************
00519 //  Temp section
00520 //**********************
00521 void LSM9DS0::readTemp()
00522 {
00523     uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp  
00524     xmReadBytes(OUT_TEMP_L_XM, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L_M
00525     temperature = (((int16_t) temp[1] << 12) | temp[0] << 4 ) >> 4; // Temperature is a 12-bit signed integer
00526 }
00527 
00528 float LSM9DS0::calcGyro(int16_t gyro)
00529 {
00530     // Return the gyro raw reading times our pre-calculated DPS / (ADC tick):
00531     return gRes * gyro; 
00532 }
00533 
00534 float LSM9DS0::calcAccel(int16_t accel)
00535 {
00536     // Return the accel raw reading times our pre-calculated g's / (ADC tick):
00537     return aRes * accel;
00538 }
00539 
00540 float LSM9DS0::calcMag(int16_t mag)
00541 {
00542     // Return the mag raw reading times our pre-calculated Gs / (ADC tick):
00543     return mRes * mag;
00544 }
00545 
00546 void LSM9DS0::setGyroScale(gyro_scale gScl)
00547 {
00548     // We need to preserve the other bytes in CTRL_REG4_G. So, first read it:
00549     uint8_t temp = gReadByte(CTRL_REG4_G);
00550     // Then mask out the gyro scale bits:
00551     temp &= 0xFF^(0x3 << 4);
00552     // Then shift in our new scale bits:
00553     temp |= gScl << 4;
00554     // And write the new register value back into CTRL_REG4_G:
00555     gWriteByte(CTRL_REG4_G, temp);
00556     
00557     // We've updated the sensor, but we also need to update our class variables
00558     // First update gScale:
00559     gScale = gScl;
00560     // Then calculate a new gRes, which relies on gScale being set correctly:
00561     calcgRes();
00562 }
00563 
00564 void LSM9DS0::setAccelScale(accel_scale aScl)
00565 {
00566     // We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
00567     uint8_t temp = xmReadByte(CTRL_REG2_XM);
00568     // Then mask out the accel scale bits:
00569     temp &= 0xFF^(0x7 << 3);
00570     // Then shift in our new scale bits:
00571     temp |= aScl << 3;
00572     // And write the new register value back into CTRL_REG2_XM:
00573     xmWriteByte(CTRL_REG2_XM, temp);
00574     
00575     // We've updated the sensor, but we also need to update our class variables
00576     // First update aScale:
00577     aScale = aScl;
00578     // Then calculate a new aRes, which relies on aScale being set correctly:
00579     calcaRes();
00580 }
00581 
00582 void LSM9DS0::setMagScale(mag_scale mScl)
00583 {
00584     // We need to preserve the other bytes in CTRL_REG6_XM. So, first read it:
00585     uint8_t temp = xmReadByte(CTRL_REG6_XM);
00586     // Then mask out the mag scale bits:
00587     temp &= 0xFF^(0x3 << 5);
00588     // Then shift in our new scale bits:
00589     temp |= mScl << 5;
00590     // And write the new register value back into CTRL_REG6_XM:
00591     xmWriteByte(CTRL_REG6_XM, temp);
00592     
00593     // We've updated the sensor, but we also need to update our class variables
00594     // First update mScale:
00595     mScale = mScl;
00596     // Then calculate a new mRes, which relies on mScale being set correctly:
00597     calcmRes();
00598 }
00599 
00600 void LSM9DS0::setGyroODR(gyro_odr gRate)
00601 {
00602     // We need to preserve the other bytes in CTRL_REG1_G. So, first read it:
00603     uint8_t temp = gReadByte(CTRL_REG1_G);
00604     // Then mask out the gyro ODR bits:
00605     temp &= 0xFF^(0xF << 4);
00606     // Then shift in our new ODR bits:
00607     temp |= (gRate << 4);
00608     // And write the new register value back into CTRL_REG1_G:
00609     gWriteByte(CTRL_REG1_G, temp);
00610 }
00611 void LSM9DS0::setAccelODR(accel_odr aRate)
00612 {
00613     // We need to preserve the other bytes in CTRL_REG1_XM. So, first read it:
00614     uint8_t temp = xmReadByte(CTRL_REG1_XM);
00615     // Then mask out the accel ODR bits:
00616     temp &= 0xFF^(0xF << 4);
00617     // Then shift in our new ODR bits:
00618     temp |= (aRate << 4);
00619     // And write the new register value back into CTRL_REG1_XM:
00620     xmWriteByte(CTRL_REG1_XM, temp);
00621 }
00622 void LSM9DS0::setAccelABW(accel_abw abwRate)
00623 {
00624     // We need to preserve the other bytes in CTRL_REG2_XM. So, first read it:
00625     uint8_t temp = xmReadByte(CTRL_REG2_XM);
00626     // Then mask out the accel ABW bits:
00627     temp &= 0xFF^(0x3 << 6);
00628     // Then shift in our new ODR bits:
00629     temp |= (abwRate << 6);
00630     // And write the new register value back into CTRL_REG2_XM:
00631     xmWriteByte(CTRL_REG2_XM, temp);
00632 }
00633 void LSM9DS0::setMagODR(mag_odr mRate)
00634 {
00635     // We need to preserve the other bytes in CTRL_REG5_XM. So, first read it:
00636     uint8_t temp = xmReadByte(CTRL_REG5_XM);
00637     // Then mask out the mag ODR bits:
00638     temp &= 0xFF^(0x7 << 2);
00639     // Then shift in our new ODR bits:
00640     temp |= (mRate << 2);
00641     // And write the new register value back into CTRL_REG5_XM:
00642     xmWriteByte(CTRL_REG5_XM, temp);
00643 }
00644 
00645 void LSM9DS0::configGyroInt(uint8_t int1Cfg, uint16_t int1ThsX, uint16_t int1ThsY, uint16_t int1ThsZ, uint8_t duration)
00646 {
00647     gWriteByte(INT1_CFG_G, int1Cfg);
00648     gWriteByte(INT1_THS_XH_G, (int1ThsX & 0xFF00) >> 8);
00649     gWriteByte(INT1_THS_XL_G, (int1ThsX & 0xFF));
00650     gWriteByte(INT1_THS_YH_G, (int1ThsY & 0xFF00) >> 8);
00651     gWriteByte(INT1_THS_YL_G, (int1ThsY & 0xFF));
00652     gWriteByte(INT1_THS_ZH_G, (int1ThsZ & 0xFF00) >> 8);
00653     gWriteByte(INT1_THS_ZL_G, (int1ThsZ & 0xFF));
00654     if (duration)
00655         gWriteByte(INT1_DURATION_G, 0x80 | duration);
00656     else
00657         gWriteByte(INT1_DURATION_G, 0x00);
00658 }
00659 
00660 void LSM9DS0::calcgRes()
00661 {
00662     // Possible gyro scales (and their register bit settings) are:
00663     // 245 DPS (00), 500 DPS (01), 2000 DPS (10). Here's a bit of an algorithm
00664     // to calculate DPS/(ADC tick) based on that 2-bit value:
00665     switch (gScale)
00666     {
00667     case G_SCALE_245DPS:
00668         gRes = 245.0f / 32768.0f;
00669         break;
00670     case G_SCALE_500DPS:
00671         gRes = 500.0f / 32768.0f;
00672         break;
00673     case G_SCALE_2000DPS:
00674         gRes = 2000.0f / 32768.0f;
00675         break;
00676     }
00677 }
00678 
00679 void LSM9DS0::calcaRes()
00680 {
00681     // Possible accelerometer scales (and their register bit settings) are:
00682     // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an 
00683     // algorithm to calculate g/(ADC tick) based on that 3-bit value:
00684     aRes = aScale == A_SCALE_16G ? 16.0f / 32768.0f : 
00685            (((float) aScale + 1.0f) * 2.0f) / 32768.0f;
00686            
00687 //    debug = aRes;
00688 }
00689 
00690 void LSM9DS0::calcmRes()
00691 {
00692     // Possible magnetometer scales (and their register bit settings) are:
00693     // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11). Here's a bit of an algorithm
00694     // to calculate Gs/(ADC tick) based on that 2-bit value:
00695     mRes = mScale == M_SCALE_2GS ? 2.0f / 32768.0f : 
00696            (float) (mScale << 2) / 32768.0f;
00697 }
00698     
00699 void LSM9DS0::gWriteByte(uint8_t subAddress, uint8_t data)
00700 {
00701     // Whether we're using I2C or SPI, write a byte using the
00702     // gyro-specific I2C address or SPI CS pin.
00703     if (interfaceMode == I2C_MODE)
00704         I2CwriteByte(gAddress, subAddress, data);
00705     else if (interfaceMode == SPI_MODE)
00706         SPIwriteByte(gAddress, subAddress, data);
00707 }
00708 
00709 void LSM9DS0::xmWriteByte(uint8_t subAddress, uint8_t data)
00710 {
00711     // Whether we're using I2C or SPI, write a byte using the
00712     // accelerometer-specific I2C address or SPI CS pin.
00713     if (interfaceMode == I2C_MODE)
00714         return I2CwriteByte(xmAddress, subAddress, data);
00715     else if (interfaceMode == SPI_MODE)
00716         return SPIwriteByte(xmAddress, subAddress, data);
00717 }
00718 
00719 uint8_t LSM9DS0::gReadByte(uint8_t subAddress)
00720 {
00721     // Whether we're using I2C or SPI, read a byte using the
00722     // gyro-specific I2C address or SPI CS pin.
00723     if (interfaceMode == I2C_MODE)
00724         return I2CreadByte(gAddress, subAddress);
00725     else if (interfaceMode == SPI_MODE)
00726         return SPIreadByte(gAddress, subAddress);
00727     else
00728         return SPIreadByte(gAddress, subAddress);
00729 }
00730 
00731 void LSM9DS0::gReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
00732 {
00733     // Whether we're using I2C or SPI, read multiple bytes using the
00734     // gyro-specific I2C address or SPI CS pin.
00735     if (interfaceMode == I2C_MODE)
00736         I2CreadBytes(gAddress, subAddress, dest, count);
00737     else if (interfaceMode == SPI_MODE)
00738         SPIreadBytes(gAddress, subAddress, dest, count);
00739 }
00740 
00741 uint8_t LSM9DS0::xmReadByte(uint8_t subAddress)
00742 {
00743     // Whether we're using I2C or SPI, read a byte using the
00744     // accelerometer-specific I2C address or SPI CS pin.
00745     if (interfaceMode == I2C_MODE)
00746         return I2CreadByte(xmAddress, subAddress);
00747     else if (interfaceMode == SPI_MODE)
00748         return SPIreadByte(xmAddress, subAddress);
00749     else
00750         return SPIreadByte(xmAddress, subAddress);
00751 }
00752 
00753 void LSM9DS0::xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
00754 {
00755     // Whether we're using I2C or SPI, read multiple bytes using the
00756     // accelerometer-specific I2C address or SPI CS pin.
00757     if (interfaceMode == I2C_MODE)
00758         I2CreadBytes(xmAddress, subAddress, dest, count);
00759     else if (interfaceMode == SPI_MODE)
00760         SPIreadBytes(xmAddress, subAddress, dest, count);
00761 }
00762 
00763 void LSM9DS0::initSPI()
00764 {
00765     csG_ = 1;
00766     csXM_= 1;
00767     
00768     // Maximum SPI frequency is 10MHz:
00769 //    spi_.frequency(1000000);
00770     spi_.format(8,0b11);
00771 }
00772 
00773 void LSM9DS0::SPIwriteByte(uint8_t csPin, uint8_t subAddress, uint8_t data)
00774 {
00775     // Initiate communication
00776     if(csPin == gAddress)
00777         csG_ = 0;
00778     else if(csPin == xmAddress)
00779         csXM_= 0;
00780     
00781     // If write, bit 0 (MSB) should be 0
00782     // If single write, bit 1 should be 0
00783     spi_.write(subAddress & 0x3F); // Send Address
00784     spi_.write(data); // Send data
00785     
00786     csG_ = 1; // Close communication
00787     csXM_= 1;
00788 }
00789 
00790 uint8_t LSM9DS0::SPIreadByte(uint8_t csPin, uint8_t subAddress)
00791 {
00792     uint8_t temp;
00793     // Use the multiple read function to read 1 byte. 
00794     // Value is returned to `temp`.
00795     SPIreadBytes(csPin, subAddress, &temp, 1);
00796     return temp;
00797 }
00798 
00799 void LSM9DS0::SPIreadBytes(uint8_t csPin, uint8_t subAddress,
00800                             uint8_t * dest, uint8_t count)
00801 {
00802     // Initiate communication
00803     if(csPin == gAddress)
00804         csG_ = 0;
00805     else if(csPin == xmAddress)
00806         csXM_= 0;
00807     // To indicate a read, set bit 0 (msb) to 1
00808     // If we're reading multiple bytes, set bit 1 to 1
00809     // The remaining six bytes are the address to be read
00810     if (count > 1)
00811         spi_.write(0xC0 | (subAddress & 0x3F));
00812     else
00813         spi_.write(0x80 | (subAddress & 0x3F));
00814     for (int i=0; i<count; i++)
00815     {
00816         dest[i] = spi_.write(0x00); // Read into destination array
00817     }
00818     csG_ = 1; // Close communication
00819     csXM_= 1;
00820 }
00821 
00822 void LSM9DS0::initI2C()
00823 {
00824 //    Wire.begin();   // Initialize I2C library
00825     ;
00826 }
00827 
00828 // Wire.h read and write protocols
00829 void LSM9DS0::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data)
00830 {
00831     ;
00832 //    Wire.beginTransmission(address);  // Initialize the Tx buffer
00833 //    Wire.write(subAddress);           // Put slave register address in Tx buffer
00834 //    Wire.write(data);                 // Put data in Tx buffer
00835 //    Wire.endTransmission();           // Send the Tx buffer
00836 }
00837 
00838 uint8_t LSM9DS0::I2CreadByte(uint8_t address, uint8_t subAddress)
00839 {
00840     return 0;
00841 //    uint8_t data; // `data` will store the register data     
00842 //    Wire.beginTransmission(address);         // Initialize the Tx buffer
00843 //    Wire.write(subAddress);                  // Put slave register address in Tx buffer
00844 //    Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
00845 //    Wire.requestFrom(address, (uint8_t) 1);  // Read one byte from slave register address 
00846 //    data = Wire.read();                      // Fill Rx buffer with result
00847 //    return data;                             // Return data read from slave register
00848 }
00849 
00850 void LSM9DS0::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, uint8_t count)
00851 {
00852     ;
00853 //    Wire.beginTransmission(address);   // Initialize the Tx buffer
00854 //    // Next send the register to be read. OR with 0x80 to indicate multi-read.
00855 //    Wire.write(subAddress | 0x80);     // Put slave register address in Tx buffer
00856 //    Wire.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
00857 //    uint8_t i = 0;
00858 //    Wire.requestFrom(address, count);  // Read bytes from slave register address 
00859 //    while (Wire.available()) 
00860 //    {
00861 //        dest[i++] = Wire.read(); // Put read results in the Rx buffer
00862 //    }
00863 }
00864 
00865 void LSM9DS0::complementaryFilter(float data[6], float dt)
00866 {
00867     
00868     float pitchAcc, rollAcc;
00869  
00870     /* Integrate the gyro data(deg/s) over time to get angle */
00871     roll += data[4] * dt;  // Angle around the Y-axis of IMU
00872     pitch +=  data[3] * dt;  // Angle around the X-axis of IMU
00873     /* Turning around the X-axis results in a vector on the Y-axis
00874     whereas turning around the Y-axis results in a vector on the X-axis. */
00875     rollAcc = (float)atan2f(data[0], -data[2])*180.0f/PI;
00876     pitchAcc  = (float)atan2f(-data[1], -data[2])*180.0f/PI;//Let Z toggle to avoid pi to -pi transition.
00877   
00878     /* Apply Complementary Filter */
00879     roll = roll * 0.97f + rollAcc * 0.03f;
00880 //    pitch = pitch * 0.9f + pitchAcc * 0.1f;//0.95
00881 //    pitch = pitch * 0.94f + pitchAcc * 0.06f;//0.95
00882 //    if(pitch < 0)// = (pitch > 0 )? pitch:pitch;
00883 //        pitch = -1*pitch;
00884 //    roll  = roll  * 0.9f + rollAcc  * 0.1f;
00885 
00886     pitch  = pitch * 0.94f + pitchAcc  * 0.06f;
00887 
00888 //    roll = (roll > 0 )? roll:180.0f+roll;
00889 }