Revision 5:56ff956c499e, committed 2016-07-28

Comitter:

adam_z

Date:

Thu Jul 28 08:16:54 2016 +0000

Parent:

4:90f024c6b406

Commit message:

slightly modified.

Changed in this revision

diff -r 90f024c6b406 -r 56ff956c499e LSM9DS0.cpp
--- a/LSM9DS0.cpp	Sat Jun 18 09:48:50 2016 +0000
+++ b/LSM9DS0.cpp	Thu Jul 28 08:16:54 2016 +0000
@@ -45,14 +45,14 @@
 {
     // interfaceMode will keep track of whether we're using SPI or I2C:
     interfaceMode = interface;
-    
+
     // xmAddress and gAddress will store the 7-bit I2C address, if using I2C.
     // If we're using SPI, these variables store the chip-select pins.
     gAddress = gAddr;
     xmAddress = xmAddr;
 }
 
-uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl, 
+uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl,
                         gyro_odr gODR, accel_odr aODR, mag_odr mODR)
 {
     // Store the given scales in class variables. These scale variables
@@ -60,45 +60,47 @@
     gScale = gScl;
     aScale = aScl;
     mScale = mScl;
-    
+
     // Once we have the scale values, we can calculate the resolution
     // of each sensor. That's what these functions are for. One for each sensor
     calcgRes(); // Calculate DPS / ADC tick, stored in gRes variable
     calcmRes(); // Calculate Gs / ADC tick, stored in mRes variable
     calcaRes(); // Calculate g / ADC tick, stored in aRes variable
-    
+
     // Now, initialize our hardware interface.
     if (interfaceMode == I2C_MODE)  // If we're using I2C
         initI2C();                  // Initialize I2C
     else if (interfaceMode == SPI_MODE)     // else, if we're using SPI
         initSPI();                          // Initialize SPI
-    
+
     // To verify communication, we can read from the WHO_AM_I register of
     // each device. Store those in a variable so we can return them.
     uint8_t gTest = gReadByte(WHO_AM_I_G);      // Read the gyro WHO_AM_I
     uint8_t xmTest = xmReadByte(WHO_AM_I_XM);   // Read the accel/mag WHO_AM_I
-    
+
     // Gyro initialization stuff:
     initGyro(); // This will "turn on" the gyro. Setting up interrupts, etc.
     setGyroODR(gODR); // Set the gyro output data rate and bandwidth.
     setGyroScale(gScale); // Set the gyro range
-    
+
     // Accelerometer initialization stuff:
     initAccel(); // "Turn on" all axes of the accel. Set up interrupts, etc.
     setAccelODR(aODR); // Set the accel data rate.
     setAccelScale(aScale); // Set the accel range.
-    
+
     // Magnetometer initialization stuff:
     initMag(); // "Turn on" all axes of the mag. Set up interrupts, etc.
     setMagODR(mODR); // Set the magnetometer output data rate.
     setMagScale(mScale); // Set the magnetometer's range.
-    
+
     setGyroOffset(0,0,0);
     setAccelOffset(0,0,0);
     setMagOffset(0,0,0);
-    
+
     // Once everything is initialized, return the WHO_AM_I registers we read:
     return (xmTest << 8) | gTest;
+    
+    init = 0;
 }
 
 void LSM9DS0::initGyro()
@@ -112,7 +114,7 @@
     PD - Power down enable (0=power down mode, 1=normal or sleep mode)
     Zen, Xen, Yen - Axis enable (o=disabled, 1=enabled) */
     gWriteByte(CTRL_REG1_G, 0xFF); // Normal mode, enable all axes
-    
+
     /* CTRL_REG2_G sets up the HPF
     Bits[7:0]: 0 0 HPM1 HPM0 HPCF3 HPCF2 HPCF1 HPCF0
     HPM[1:0] - High pass filter mode selection
@@ -122,7 +124,7 @@
         Value depends on data rate. See datasheet table 26.
     */
     gWriteByte(CTRL_REG2_G, 0x09); // Normal mode, high cutoff frequency
-    
+
     /* CTRL_REG3_G sets up interrupt and DRDY_G pins
     Bits[7:0]: I1_IINT1 I1_BOOT H_LACTIVE PP_OD I2_DRDY I2_WTM I2_ORUN I2_EMPTY
     I1_INT1 - Interrupt enable on INT_G pin (0=disable, 1=enable)
@@ -134,8 +136,8 @@
     I2_ORUN - FIFO overrun interrupt on DRDY_G (0=disable 1=enable)
     I2_EMPTY - FIFO empty interrupt on DRDY_G (0=disable 1=enable) */
     // Int1 enabled (pp, active low), data read on DRDY_G:
-    gWriteByte(CTRL_REG3_G, 0x00); 
-    
+    gWriteByte(CTRL_REG3_G, 0x00);
+
     /* CTRL_REG4_G sets the scale, update mode
     Bits[7:0] - BDU BLE FS1 FS0 - ST1 ST0 SIM
     BDU - Block data update (0=continuous, 1=output not updated until read
@@ -147,7 +149,7 @@
     SIM - SPI serial interface mode select
         0=4 wire, 1=3 wire */
     gWriteByte(CTRL_REG4_G, 0x30); // Set scale to 245 dps
-    
+
     /* CTRL_REG5_G sets up the FIFO, HPF, and INT1
     Bits[7:0] - BOOT FIFO_EN - HPen INT1_Sel1 INT1_Sel0 Out_Sel1 Out_Sel0
     BOOT - Reboot memory content (0=normal, 1=reboot)
@@ -156,7 +158,7 @@
     INT1_Sel[1:0] - Int 1 selection configuration
     Out_Sel[1:0] - Out selection configuration */
     gWriteByte(CTRL_REG5_G, 0x00);
-    
+
     // Temporary !!! For testing !!! Remove !!! Or make useful !!!
     configGyroInt(0x2A, 0, 0, 0, 0); // Trigger interrupt when above 0 DPS...
 }
@@ -172,20 +174,20 @@
     HPIS1 - HPF enabled for interrupt generator 1 (0: bypassed, 1: enabled)
     HPIS2 - HPF enabled for interrupt generator 2 (0: bypassed, 1 enabled)   */
     xmWriteByte(CTRL_REG0_XM, 0x00);
-    
+
     /* CTRL_REG1_XM (0x20) (Default value: 0x07)
     Bits (7-0): AODR3 AODR2 AODR1 AODR0 BDU AZEN AYEN AXEN
     AODR[3:0] - select the acceleration data rate:
-        0000=power down, 0001=3.125Hz, 0010=6.25Hz, 0011=12.5Hz, 
+        0000=power down, 0001=3.125Hz, 0010=6.25Hz, 0011=12.5Hz,
         0100=25Hz, 0101=50Hz, 0110=100Hz, 0111=200Hz, 1000=400Hz,
         1001=800Hz, 1010=1600Hz, (remaining combinations undefined).
     BDU - block data update for accel AND mag
         0: Continuous update
         1: Output registers aren't updated until MSB and LSB have been read.
     AZEN, AYEN, and AXEN - Acceleration x/y/z-axis enabled.
-        0: Axis disabled, 1: Axis enabled                                    */ 
+        0: Axis disabled, 1: Axis enabled                                    */
     xmWriteByte(CTRL_REG1_XM, 0x97); // 100Hz data rate, x/y/z all enabled
-    
+
     //Serial.println(xmReadByte(CTRL_REG1_XM));
     /* CTRL_REG2_XM (0x21) (Default value: 0x00)
     Bits (7-0): ABW1 ABW0 AFS2 AFS1 AFS0 AST1 AST0 SIM
@@ -198,16 +200,16 @@
     SIM - SPI mode selection
         0=4-wire, 1=3-wire                                                   */
     xmWriteByte(CTRL_REG2_XM, 0xD8); // Set scale to 2g
-    
+
     /* CTRL_REG3_XM is used to set interrupt generators on INT1_XM
     Bits (7-0): P1_BOOT P1_TAP P1_INT1 P1_INT2 P1_INTM P1_DRDYA P1_DRDYM P1_EMPTY
     */
     // Accelerometer data ready on INT1_XM (0x04)
-    xmWriteByte(CTRL_REG3_XM, 0x00); 
+    xmWriteByte(CTRL_REG3_XM, 0x00);
 }
 
 void LSM9DS0::initMag()
-{   
+{
     /* CTRL_REG5_XM enables temp sensor, sets mag resolution and data rate
     Bits (7-0): TEMP_EN M_RES1 M_RES0 M_ODR2 M_ODR1 M_ODR0 LIR2 LIR1
     TEMP_EN - Enable temperature sensor (0=disabled, 1=enabled)
@@ -219,17 +221,17 @@
     LIR1 - Latch interrupt request on INT1_SRC (cleared by readging INT1_SRC)
         0=irq not latched, 1=irq latched                                     */
     xmWriteByte(CTRL_REG5_XM, 0x74); // Mag data rate - 100 Hz, disable temperature sensor
-    
+
     /* CTRL_REG6_XM sets the magnetometer full-scale
     Bits (7-0): 0 MFS1 MFS0 0 0 0 0 0
     MFS[1:0] - Magnetic full-scale selection
     00:+/-2Gauss, 01:+/-4Gs, 10:+/-8Gs, 11:+/-12Gs                           */
     xmWriteByte(CTRL_REG6_XM, 0x40); // Mag scale to +/- 2GS
-    
+
     /* CTRL_REG7_XM sets magnetic sensor mode, low power mode, and filters
     AHPM1 AHPM0 AFDS 0 0 MLP MD1 MD0
     AHPM[1:0] - HPF mode selection
-        00=normal (resets reference registers), 01=reference signal for filtering, 
+        00=normal (resets reference registers), 01=reference signal for filtering,
         10=normal, 11=autoreset on interrupt event
     AFDS - Filtered acceleration data selection
         0=internal filter bypassed, 1=data from internal filter sent to FIFO
@@ -239,12 +241,12 @@
     MD[1:0] - Magnetic sensor mode selection (default 10)
         00=continuous-conversion, 01=single-conversion, 10 and 11=power-down */
     xmWriteByte(CTRL_REG7_XM, 0x00); // Continuous conversion mode
-    
+
     /* CTRL_REG4_XM is used to set interrupt generators on INT2_XM
     Bits (7-0): P2_TAP P2_INT1 P2_INT2 P2_INTM P2_DRDYA P2_DRDYM P2_Overrun P2_WTM
     */
     xmWriteByte(CTRL_REG4_XM, 0x00); // Magnetometer data ready on INT2_XM (0x08)
-    
+
     /* INT_CTRL_REG_M to set push-pull/open drain, and active-low/high
     Bits[7:0] - XMIEN YMIEN ZMIEN PP_OD IEA IEL 4D MIEN
     XMIEN, YMIEN, ZMIEN - Enable interrupt recognition on axis for mag data
@@ -267,69 +269,69 @@
 // remove errors due to imprecise or varying initial placement. Calibration of sensor data in this manner
 // is good practice.
 void LSM9DS0::calLSM9DS0(float * gbias, float * abias)
-{  
-  uint8_t data[6] = {0, 0, 0, 0, 0, 0};
-  int16_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
-  int samples, ii;
-  
-  // First get gyro bias
-  uint8_t c = gReadByte(CTRL_REG5_G);
-  gWriteByte(CTRL_REG5_G, c | 0x40);         // Enable gyro FIFO  
-  wait_ms(20);                                 // Wait for change to take effect
-  gWriteByte(FIFO_CTRL_REG_G, 0x20 | 0x1F);  // Enable gyro FIFO stream mode and set watermark at 32 samples
-  wait_ms(1000);  // delay 1000 milliseconds to collect FIFO samples
-  
-  samples = (gReadByte(FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples
+{
+    uint8_t data[6] = {0, 0, 0, 0, 0, 0};
+    int16_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
+    int samples, ii;
+
+    // First get gyro bias
+    uint8_t c = gReadByte(CTRL_REG5_G);
+    gWriteByte(CTRL_REG5_G, c | 0x40);         // Enable gyro FIFO
+    wait_ms(20);                                 // Wait for change to take effect
+    gWriteByte(FIFO_CTRL_REG_G, 0x20 | 0x1F);  // Enable gyro FIFO stream mode and set watermark at 32 samples
+    wait_ms(1000);  // delay 1000 milliseconds to collect FIFO samples
+
+    samples = (gReadByte(FIFO_SRC_REG_G) & 0x1F); // Read number of stored samples
 
-  for(ii = 0; ii < samples ; ii++) {            // Read the gyro data stored in the FIFO
-    gReadBytes(OUT_X_L_G,  &data[0], 6);
-    gyro_bias[0] += (((int16_t)data[1] << 8) | data[0]);
-    gyro_bias[1] += (((int16_t)data[3] << 8) | data[2]);
-    gyro_bias[2] += (((int16_t)data[5] << 8) | data[4]);
-  }  
+    for(ii = 0; ii < samples ; ii++) {            // Read the gyro data stored in the FIFO
+        gReadBytes(OUT_X_L_G,  &data[0], 6);
+        gyro_bias[0] += (((int16_t)data[1] << 8) | data[0]);
+        gyro_bias[1] += (((int16_t)data[3] << 8) | data[2]);
+        gyro_bias[2] += (((int16_t)data[5] << 8) | data[4]);
+    }
+
+    gyro_bias[0] /= samples; // average the data
+    gyro_bias[1] /= samples;
+    gyro_bias[2] /= samples;
+
+    gbias[0] = (float)gyro_bias[0]*gRes;  // Properly scale the data to get deg/s
+    gbias[1] = (float)gyro_bias[1]*gRes;
+    gbias[2] = (float)gyro_bias[2]*gRes;
 
-  gyro_bias[0] /= samples; // average the data
-  gyro_bias[1] /= samples; 
-  gyro_bias[2] /= samples; 
-  
-  gbias[0] = (float)gyro_bias[0]*gRes;  // Properly scale the data to get deg/s
-  gbias[1] = (float)gyro_bias[1]*gRes;
-  gbias[2] = (float)gyro_bias[2]*gRes;
-  
-  c = gReadByte(CTRL_REG5_G);
-  gWriteByte(CTRL_REG5_G, c & ~0x40);  // Disable gyro FIFO  
-  wait_ms(20);
-  gWriteByte(FIFO_CTRL_REG_G, 0x00);   // Enable gyro bypass mode
-  
+    c = gReadByte(CTRL_REG5_G);
+    gWriteByte(CTRL_REG5_G, c & ~0x40);  // Disable gyro FIFO
+    wait_ms(20);
+    gWriteByte(FIFO_CTRL_REG_G, 0x00);   // Enable gyro bypass mode
+
 
-  //  Now get the accelerometer biases
-  c = xmReadByte(CTRL_REG0_XM);
-  xmWriteByte(CTRL_REG0_XM, c | 0x40);      // Enable accelerometer FIFO  
-  wait_ms(20);                                // Wait for change to take effect
-  xmWriteByte(FIFO_CTRL_REG, 0x20 | 0x1F);  // Enable accelerometer FIFO stream mode and set watermark at 32 samples
-  wait_ms(1000);  // delay 1000 milliseconds to collect FIFO samples
+    //  Now get the accelerometer biases
+    c = xmReadByte(CTRL_REG0_XM);
+    xmWriteByte(CTRL_REG0_XM, c | 0x40);      // Enable accelerometer FIFO
+    wait_ms(20);                                // Wait for change to take effect
+    xmWriteByte(FIFO_CTRL_REG, 0x20 | 0x1F);  // Enable accelerometer FIFO stream mode and set watermark at 32 samples
+    wait_ms(1000);  // delay 1000 milliseconds to collect FIFO samples
+
+    samples = (xmReadByte(FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples
 
-  samples = (xmReadByte(FIFO_SRC_REG) & 0x1F); // Read number of stored accelerometer samples
-
-   for(ii = 0; ii < samples ; ii++) {          // Read the accelerometer data stored in the FIFO
-    xmReadBytes(OUT_X_L_A, &data[0], 6);
-    accel_bias[0] += (((int16_t)data[1] << 8) | data[0]);
-    accel_bias[1] += (((int16_t)data[3] << 8) | data[2]);
-    accel_bias[2] += (((int16_t)data[5] << 8) | data[4]) - (int16_t)(1.0f/aRes); // Assumes sensor facing up!
-  }  
+    for(ii = 0; ii < samples ; ii++) {          // Read the accelerometer data stored in the FIFO
+        xmReadBytes(OUT_X_L_A, &data[0], 6);
+        accel_bias[0] += (((int16_t)data[1] << 8) | data[0]);
+        accel_bias[1] += (((int16_t)data[3] << 8) | data[2]);
+        accel_bias[2] += (((int16_t)data[5] << 8) | data[4]) - (int16_t)(1.0f/aRes); // Assumes sensor facing up!
+    }
 
-  accel_bias[0] /= samples; // average the data
-  accel_bias[1] /= samples; 
-  accel_bias[2] /= samples; 
-  
-  abias[0] = (float)accel_bias[0]*aRes; // Properly scale data to get gs
-  abias[1] = (float)accel_bias[1]*aRes;
-  abias[2] = (float)accel_bias[2]*aRes;
+    accel_bias[0] /= samples; // average the data
+    accel_bias[1] /= samples;
+    accel_bias[2] /= samples;
 
-  c = xmReadByte(CTRL_REG0_XM);
-  xmWriteByte(CTRL_REG0_XM, c & ~0x40);    // Disable accelerometer FIFO  
-  wait_ms(20);
-  xmWriteByte(FIFO_CTRL_REG, 0x00);       // Enable accelerometer bypass mode
+    abias[0] = (float)accel_bias[0]*aRes; // Properly scale data to get gs
+    abias[1] = (float)accel_bias[1]*aRes;
+    abias[2] = (float)accel_bias[2]*aRes;
+
+    c = xmReadByte(CTRL_REG0_XM);
+    xmWriteByte(CTRL_REG0_XM, c & ~0x40);    // Disable accelerometer FIFO
+    wait_ms(20);
+    xmWriteByte(FIFO_CTRL_REG, 0x00);       // Enable accelerometer bypass mode
 }
 
 //**********************
@@ -398,7 +400,7 @@
 //**********************
 void LSM9DS0::readAccel()
 {
-    uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp   
+    uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp
     xmReadBytes(OUT_X_L_A, temp, 6); // Read 6 bytes, beginning at OUT_X_L_A
     ax = (temp[1] << 8) | temp[0]; // Store x-axis values into ax
     ay = (temp[3] << 8) | temp[2]; // Store y-axis values into ay
@@ -459,7 +461,7 @@
 //**********************
 void LSM9DS0::readMag()
 {
-    uint8_t temp[6]; // We'll read six bytes from the mag into temp 
+    uint8_t temp[6]; // We'll read six bytes from the mag into temp
     xmReadBytes(OUT_X_L_M, temp, 6); // Read 6 bytes, beginning at OUT_X_L_M
     mx = (temp[1] << 8) | temp[0]; // Store x-axis values into mx
     my = (temp[3] << 8) | temp[2]; // Store y-axis values into my
@@ -520,7 +522,7 @@
 //**********************
 void LSM9DS0::readTemp()
 {
-    uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp  
+    uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp
     xmReadBytes(OUT_TEMP_L_XM, temp, 2); // Read 2 bytes, beginning at OUT_TEMP_L_M
     temperature = (((int16_t) temp[1] << 12) | temp[0] << 4 ) >> 4; // Temperature is a 12-bit signed integer
 }
@@ -528,7 +530,7 @@
 float LSM9DS0::calcGyro(int16_t gyro)
 {
     // Return the gyro raw reading times our pre-calculated DPS / (ADC tick):
-    return gRes * gyro; 
+    return gRes * gyro;
 }
 
 float LSM9DS0::calcAccel(int16_t accel)
@@ -553,7 +555,7 @@
     temp |= gScl << 4;
     // And write the new register value back into CTRL_REG4_G:
     gWriteByte(CTRL_REG4_G, temp);
-    
+
     // We've updated the sensor, but we also need to update our class variables
     // First update gScale:
     gScale = gScl;
@@ -571,7 +573,7 @@
     temp |= aScl << 3;
     // And write the new register value back into CTRL_REG2_XM:
     xmWriteByte(CTRL_REG2_XM, temp);
-    
+
     // We've updated the sensor, but we also need to update our class variables
     // First update aScale:
     aScale = aScl;
@@ -589,7 +591,7 @@
     temp |= mScl << 5;
     // And write the new register value back into CTRL_REG6_XM:
     xmWriteByte(CTRL_REG6_XM, temp);
-    
+
     // We've updated the sensor, but we also need to update our class variables
     // First update mScale:
     mScale = mScl;
@@ -662,26 +664,25 @@
     // Possible gyro scales (and their register bit settings) are:
     // 245 DPS (00), 500 DPS (01), 2000 DPS (10). Here's a bit of an algorithm
     // to calculate DPS/(ADC tick) based on that 2-bit value:
-    switch (gScale)
-    {
-    case G_SCALE_245DPS:
-        gRes = 245.0 / 32768.0;
-        break;
-    case G_SCALE_500DPS:
-        gRes = 500.0 / 32768.0;
-        break;
-    case G_SCALE_2000DPS:
-        gRes = 2000.0 / 32768.0;
-        break;
+    switch (gScale) {
+        case G_SCALE_245DPS:
+            gRes = 245.0 / 32768.0;
+            break;
+        case G_SCALE_500DPS:
+            gRes = 500.0 / 32768.0;
+            break;
+        case G_SCALE_2000DPS:
+            gRes = 2000.0 / 32768.0;
+            break;
     }
 }
 
 void LSM9DS0::calcaRes()
 {
     // Possible accelerometer scales (and their register bit settings) are:
-    // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an 
+    // 2 g (000), 4g (001), 6g (010) 8g (011), 16g (100). Here's a bit of an
     // algorithm to calculate g/(ADC tick) based on that 3-bit value:
-    aRes = aScale == A_SCALE_16G ? 16.0 / 32768.0 : 
+    aRes = aScale == A_SCALE_16G ? 16.0 / 32768.0 :
            (((float) aScale + 1.0f) * 2.0f) / 32768.0f;
 }
 
@@ -690,10 +691,10 @@
     // Possible magnetometer scales (and their register bit settings) are:
     // 2 Gs (00), 4 Gs (01), 8 Gs (10) 12 Gs (11). Here's a bit of an algorithm
     // to calculate Gs/(ADC tick) based on that 2-bit value:
-    mRes = mScale == M_SCALE_2GS ? 2.0 / 32768.0 : 
+    mRes = mScale == M_SCALE_2GS ? 2.0 / 32768.0 :
            (float) (mScale << 2) / 32768.0f;
 }
-    
+
 void LSM9DS0::gWriteByte(uint8_t subAddress, uint8_t data)
 {
     // Whether we're using I2C or SPI, write a byte using the
@@ -762,7 +763,7 @@
 {
     csG_ = 1;
     csXM_= 1;
-    
+
     // Maximum SPI frequency is 10MHz:
 //    spi_.frequency(1000000);
     spi_.format(8,0b11);
@@ -775,12 +776,12 @@
         csG_ = 0;
     else if(csPin == xmAddress)
         csXM_= 0;
-    
+
     // If write, bit 0 (MSB) should be 0
     // If single write, bit 1 should be 0
     spi_.write(subAddress & 0x3F); // Send Address
     spi_.write(data); // Send data
-    
+
     csG_ = 1; // Close communication
     csXM_= 1;
 }
@@ -788,14 +789,14 @@
 uint8_t LSM9DS0::SPIreadByte(uint8_t csPin, uint8_t subAddress)
 {
     uint8_t temp;
-    // Use the multiple read function to read 1 byte. 
+    // Use the multiple read function to read 1 byte.
     // Value is returned to `temp`.
     SPIreadBytes(csPin, subAddress, &temp, 1);
     return temp;
 }
 
 void LSM9DS0::SPIreadBytes(uint8_t csPin, uint8_t subAddress,
-                            uint8_t * dest, uint8_t count)
+                           uint8_t * dest, uint8_t count)
 {
     // Initiate communication
     if(csPin == gAddress)
@@ -809,8 +810,7 @@
         spi_.write(0xC0 | (subAddress & 0x3F));
     else
         spi_.write(0x80 | (subAddress & 0x3F));
-    for (int i=0; i<count; i++)
-    {
+    for (int i=0; i<count; i++) {
         dest[i] = spi_.write(0x00); // Read into destination array
     }
     csG_ = 1; // Close communication
@@ -836,11 +836,11 @@
 uint8_t LSM9DS0::I2CreadByte(uint8_t address, uint8_t subAddress)
 {
     return 0;
-//    uint8_t data; // `data` will store the register data     
+//    uint8_t data; // `data` will store the register data
 //    Wire.beginTransmission(address);         // Initialize the Tx buffer
 //    Wire.write(subAddress);                  // Put slave register address in Tx buffer
 //    Wire.endTransmission(false);             // Send the Tx buffer, but send a restart to keep connection alive
-//    Wire.requestFrom(address, (uint8_t) 1);  // Read one byte from slave register address 
+//    Wire.requestFrom(address, (uint8_t) 1);  // Read one byte from slave register address
 //    data = Wire.read();                      // Fill Rx buffer with result
 //    return data;                             // Return data read from slave register
 }
@@ -853,8 +853,8 @@
 //    Wire.write(subAddress | 0x80);     // Put slave register address in Tx buffer
 //    Wire.endTransmission(false);       // Send the Tx buffer, but send a restart to keep connection alive
 //    uint8_t i = 0;
-//    Wire.requestFrom(address, count);  // Read bytes from slave register address 
-//    while (Wire.available()) 
+//    Wire.requestFrom(address, count);  // Read bytes from slave register address
+//    while (Wire.available())
 //    {
 //        dest[i++] = Wire.read(); // Put read results in the Rx buffer
 //    }
@@ -862,19 +862,23 @@
 
 void LSM9DS0::complementaryFilter(float * data, float dt)
 {
-    
-    float pitchAcc, rollAcc;
- 
-    /* Integrate the gyro data(deg/s) over time to get angle */
-    pitch += data[5] * dt;  // Angle around the Z-axis
+    if(init == 0) {
+        pitch = (float)atan2f(-data[0], -data[1])*180.0f/PI;
+        init = 1;
+    } else {
+        float pitchAcc, rollAcc;
+
+        /* Integrate the gyro data(deg/s) over time to get angle */
+        pitch += data[5] * dt;  // Angle around the Z-axis
 //    roll +=  data[3] * dt;  // Angle around the X-axis
-    
-    /* Turning around the X-axis results in a vector on the Y-axis
-    whereas turning around the Y-axis results in a vector on the X-axis. */
-    pitchAcc = (float)atan2f(-data[0], -data[1])*180.0f/PI;
+
+        /* Turning around the X-axis results in a vector on the Y-axis
+        whereas turning around the Y-axis results in a vector on the X-axis. */
+        pitchAcc = (float)atan2f(-data[0], -data[1])*180.0f/PI;
 //    rollAcc  = (float)atan2f(data[2], -data[1])*180.0f/PI;
-  
-    /* Apply Complementary Filter */
-    pitch = pitch * 0.999 + pitchAcc * 0.001;
+
+        /* Apply Complementary Filter */
+        pitch = pitch * 0.9999 + pitchAcc * 0.0001;
 //    roll  = roll  * 0.999 + rollAcc  * 0.001;
+    }
 }
\ No newline at end of file

diff -r 90f024c6b406 -r 56ff956c499e LSM9DS0.h
--- a/LSM9DS0.h	Sat Jun 18 09:48:50 2016 +0000
+++ b/LSM9DS0.h	Thu Jul 28 08:16:54 2016 +0000
@@ -395,6 +395,7 @@
     DigitalOut csXM_;
     
     float pitch, roll;
+    bool init;
     void complementaryFilter(float * data, float dt);
 
 private: