Diff: LSM9DS0.cpp

Revision:

4:bf8f4e7c9905

Parent:

0:1b975a6ae539

diff -r 7ede465deae1 -r bf8f4e7c9905 LSM9DS0.cpp
--- a/LSM9DS0.cpp	Sun Nov 23 17:46:45 2014 +0000
+++ b/LSM9DS0.cpp	Wed Dec 03 23:08:09 2014 +0000
@@ -2,15 +2,11 @@
 
 LSM9DS0::LSM9DS0(PinName sda, PinName scl, uint8_t gAddr, uint8_t xmAddr)
 {
-    
     // 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.
     xmAddress = xmAddr;
     gAddress = gAddr;
     
     i2c_ = new I2Cdev(sda, scl);
-    //100KHz, as specified by the datasheet.
-    //i2c_->frequency(100000);
 }
 
 uint16_t LSM9DS0::begin(gyro_scale gScl, accel_scale aScl, mag_scale mScl, 
@@ -41,8 +37,8 @@
     
     // 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.
+    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.
@@ -55,178 +51,115 @@
 
 void LSM9DS0::initGyro()
 {
-    /* CTRL_REG1_G sets output data rate, bandwidth, power-down and enables
-    Bits[7:0]: DR1 DR0 BW1 BW0 PD Zen Xen Yen
-    DR[1:0] - Output data rate selection
-        00=95Hz, 01=190Hz, 10=380Hz, 11=760Hz
-    BW[1:0] - Bandwidth selection (sets cutoff frequency)
-         Value depends on ODR. See datasheet table 21.
-    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, 0x0F); // 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
-        00=normal (reset reading HP_RESET_FILTER, 01=ref signal for filtering,
-        10=normal, 11=autoreset on interrupt
-    HPCF[3:0] - High pass filter cutoff frequency
-        Value depends on data rate. See datasheet table 26.
-    */
     gWriteByte(CTRL_REG2_G, 0x00); // Normal mode, high cutoff frequency
+    gWriteByte(CTRL_REG3_G, 0x88);  //Interrupt enabled on both INT_G  and I2_DRDY
+    gWriteByte(CTRL_REG4_G, 0x00); // Set scale to 245 dps
+    gWriteByte(CTRL_REG5_G, 0x00); //Init default values
     
-    /* 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)
-    I1_BOOT - Boot status available on INT_G (0=disable, 1=enable)
-    H_LACTIVE - Interrupt active configuration on INT_G (0:high, 1:low)
-    PP_OD - Push-pull/open-drain (0=push-pull, 1=open-drain)
-    I2_DRDY - Data ready on DRDY_G (0=disable, 1=enable)
-    I2_WTM - FIFO watermark interrupt on DRDY_G (0=disable 1=enable)
-    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, 0x88); 
-    
-    /* 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
-    BLE - Big/little endian (0=data LSB @ lower address, 1=LSB @ higher add)
-    FS[1:0] - Full-scale selection
-        00=245dps, 01=500dps, 10=2000dps, 11=2000dps
-    ST[1:0] - Self-test enable
-        00=disabled, 01=st 0 (x+, y-, z-), 10=undefined, 11=st 1 (x-, y+, z+)
-    SIM - SPI serial interface mode select
-        0=4 wire, 1=3 wire */
-    gWriteByte(CTRL_REG4_G, 0x00); // 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)
-    FIFO_EN - FIFO enable (0=disable, 1=enable)
-    HPen - HPF enable (0=disable, 1=enable)
-    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...
 }
 
 void LSM9DS0::initAccel()
 {
-    /* CTRL_REG0_XM (0x1F) (Default value: 0x00)
-    Bits (7-0): BOOT FIFO_EN WTM_EN 0 0 HP_CLICK HPIS1 HPIS2
-    BOOT - Reboot memory content (0: normal, 1: reboot)
-    FIFO_EN - Fifo enable (0: disable, 1: enable)
-    WTM_EN - FIFO watermark enable (0: disable, 1: enable)
-    HP_CLICK - HPF enabled for click (0: filter bypassed, 1: enabled)
-    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, 
-        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                                    */ 
-    xmWriteByte(CTRL_REG1_XM, 0x57); // 50Hz 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
-    ABW[1:0] - Accelerometer anti-alias filter bandwidth
-        00=773Hz, 01=194Hz, 10=362Hz, 11=50Hz
-    AFS[2:0] - Accel full-scale selection
-        000=+/-2g, 001=+/-4g, 010=+/-6g, 011=+/-8g, 100=+/-16g
-    AST[1:0] - Accel self-test enable
-        00=normal (no self-test), 01=positive st, 10=negative st, 11=not allowed
-    SIM - SPI mode selection
-        0=4-wire, 1=3-wire                                                   */
+    xmWriteByte(CTRL_REG0_XM, 0x00);  
+    xmWriteByte(CTRL_REG1_XM, 0x57); // 50Hz data rate, x/y/z all enabled                                                
     xmWriteByte(CTRL_REG2_XM, 0x00); // 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, 0x04); 
+    xmWriteByte(CTRL_REG3_XM, 0x04); // Accelerometer data ready on INT1_XM (0x04)
+
 }
 
 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)
-    M_RES[1:0] - Magnetometer resolution select (0=low, 3=high)
-    M_ODR[2:0] - Magnetometer data rate select
-        000=3.125Hz, 001=6.25Hz, 010=12.5Hz, 011=25Hz, 100=50Hz, 101=100Hz
-    LIR2 - Latch interrupt request on INT2_SRC (cleared by reading INT2_SRC)
-        0=interrupt request not latched, 1=interrupt request latched
-    LIR1 - Latch interrupt request on INT1_SRC (cleared by readging INT1_SRC)
-        0=irq not latched, 1=irq latched                                     */
-    xmWriteByte(CTRL_REG5_XM, 0x14); // Mag data rate - 100 Hz
-    
-    /* 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_REG5_XM, 0x94); // Mag data rate - 100 Hz, enable temperature sensor
     xmWriteByte(CTRL_REG6_XM, 0x00); // 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, 
-        10=normal, 11=autoreset on interrupt event
-    AFDS - Filtered acceleration data selection
-        0=internal filter bypassed, 1=data from internal filter sent to FIFO
-    MLP - Magnetic data low-power mode
-        0=data rate is set by M_ODR bits in CTRL_REG5
-        1=data rate is set to 3.125Hz
-    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, 0x04); // 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
-    PP_OD - Push-pull/open-drain interrupt configuration (0=push-pull, 1=od)
-    IEA - Interrupt polarity for accel and magneto
-        0=active-low, 1=active-high
-    IEL - Latch interrupt request for accel and magneto
-        0=irq not latched, 1=irq latched
-    4D - 4D enable. 4D detection is enabled when 6D bit in INT_GEN1_REG is set
-    MIEN - Enable interrupt generation for magnetic data
-        0=disable, 1=enable) */
     xmWriteByte(INT_CTRL_REG_M, 0x09); // Enable interrupts for mag, active-low, push-pull
 }
 
+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
+
+  for(ii = 0; ii < samples ; ii++) {            // Read the gyro data stored in the FIFO
+
+    data[0] = gReadByte(OUT_X_L_G);
+    data[1] = gReadByte(OUT_X_H_G);
+    data[2] = gReadByte(OUT_Y_L_G);
+    data[3] = gReadByte(OUT_Y_H_G);
+    data[4] = gReadByte(OUT_Z_L_G);
+    data[5] = gReadByte(OUT_Z_H_G);
+  
+    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;
+  
+  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
+
+  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
+   
+    data[0] = xmReadByte(OUT_X_L_A);
+    data[1] = xmReadByte(OUT_X_H_A);
+    data[2] = xmReadByte(OUT_Y_L_A);
+    data[3] = xmReadByte(OUT_Y_H_A);
+    data[4] = xmReadByte(OUT_Z_L_A);
+    data[5] = xmReadByte(OUT_Z_H_A);
+    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./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;
+
+  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
+  
+}
 void LSM9DS0::readAccel()
 {
-    /*uint8_t temp[6]; // We'll read six bytes from the accelerometer into temp   
-    //xmReadByte(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
-    az = (temp[5] << 8) | temp[4]; // Store z-axis values into az*/
-    
   uint16_t Temp = 0;
-  uint8_t  INTStatus = 0;
   
-  while(INTStatus == 0)
-  {
-      INTStatus = xmReadByte(STATUS_REG_A) & 0x08;
-  }
-
   //Get x
   Temp = xmReadByte(OUT_X_H_A);
   Temp = Temp<<8;
@@ -252,19 +185,7 @@
 
 void LSM9DS0::readMag()
 {
-    /*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
-    mz = (temp[5] << 8) | temp[4]; // Store z-axis values into mz*/
-    
- uint16_t Temp = 0;
-  uint8_t  INTStatus = 0;
-  
-  while(INTStatus == 0)
-  {
-      INTStatus = xmReadByte(STATUS_REG_M) & 0x08;
-  }
+  uint16_t Temp = 0;  
 
   //Get x
   Temp = xmReadByte(OUT_X_H_M);
@@ -288,41 +209,40 @@
   mz = Temp;
 }
 
-void LSM9DS0::readGyro()
+void LSM9DS0::readTemp()
 {
-    /*uint8_t temp[6]; // We'll read six bytes from the gyro into temp
-    gReadBytes(OUT_X_L_G, temp, 6); // Read 6 bytes, beginning at OUT_X_L_G
-    gx = (temp[1] << 8) | temp[0]; // Store x-axis values into gx
-    gy = (temp[3] << 8) | temp[2]; // Store y-axis values into gy
-    gz = (temp[5] << 8) | temp[4]; // Store z-axis values into gz*/
+    uint8_t temp[2]; // We'll read two bytes from the temperature sensor into temp  
+    
+    temp[0] = xmReadByte(OUT_TEMP_L_XM);
+    temp[1] = xmReadByte(OUT_TEMP_H_XM);
     
+    temperature = (((int16_t) temp[1] << 12) | temp[0] << 4 ) >> 4; // Temperature is a 12-bit signed integer
+}
+
+
+void LSM9DS0::readGyro()
+{   
   uint16_t Temp = 0;
-  uint8_t  INTStatus = 0;
-  
-  while(INTStatus == 0)
-  {
-      INTStatus = (xmReadByte(STATUS_REG_G)&0x08);
-  }
 
   //Get x
-  Temp = xmReadByte(OUT_X_H_G);
+  Temp = gReadByte(OUT_X_H_G);
   Temp = Temp<<8;
-  Temp |= xmReadByte(OUT_X_L_G);
+  Temp |= gReadByte(OUT_X_L_G);
   gx = Temp;
   
   
   //Get y
   Temp=0;
-  Temp = xmReadByte(OUT_Y_H_G);
+  Temp = gReadByte(OUT_Y_H_G);
   Temp = Temp<<8;
-  Temp |= xmReadByte(OUT_Y_L_G);
+  Temp |= gReadByte(OUT_Y_L_G);
   gy = Temp;
   
   //Get z
   Temp=0;
-  Temp = xmReadByte(OUT_Z_H_G);
+  Temp = gReadByte(OUT_Z_H_G);
   Temp = Temp<<8;
-  Temp |= xmReadByte(OUT_Z_L_G);
+  Temp |= gReadByte(OUT_Z_L_G);
   gz = Temp;
 }
 
@@ -336,7 +256,6 @@
 {
     // Return the accel raw reading times our pre-calculated g's / (ADC tick):
     return aRes * accel;
-    //return accel * (2/32768) - 2;
 }
 
 float LSM9DS0::calcMag(int16_t mag)
@@ -507,38 +426,27 @@
 void LSM9DS0::gReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
 {
     // Whether we're using I2C or SPI, read multiple bytes using the
-    // gyro-specific I2C address or SPI CS pin.
+    // gyro-specific I2C address.
         I2CreadBytes(gAddress, subAddress, dest, count);
 }
 
 uint8_t LSM9DS0::xmReadByte(uint8_t subAddress)
 {
     // Whether we're using I2C or SPI, read a byte using the
-    // accelerometer-specific I2C address or SPI CS pin.
+    // accelerometer-specific I2C address.
         return I2CreadByte(xmAddress, subAddress);
 }
 
 void LSM9DS0::xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count)
 {
-    // Whether we're using I2C or SPI, read multiple bytes using the
-    // accelerometer-specific I2C address or SPI CS pin.
-        I2CreadBytes(xmAddress, subAddress, dest, count);
+    // read multiple bytes using the
+    // accelerometer-specific I2C address.
+    I2CreadBytes(xmAddress, subAddress, dest, count);
 }
 
 
 void LSM9DS0::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data)
-{
-   /* i2c_->start();
-     wait_ms(1);
-    i2c_->write(address);
-    wait_ms(1);
-    i2c_->write(subAddress);
-    wait_ms(1);
-    
-    i2c_->write(data);
-    wait_ms(1);
-    i2c_->stop();*/
-    
+{   
     i2c_->writeByte(address,subAddress,data);
 }
 
@@ -546,14 +454,6 @@
 {
     char data[1]; // `data` will store the register data
     
-   /* data[0] = subAddress;
-    
-    i2c_->write(address, data, 1, true);
-    i2c_->read(address, data, 1, true);
-
-        i2c_->stop();
-    return (uint8_t)data[0]; // Return data from register*/
-    
     I2CreadBytes(address, subAddress,(uint8_t*)data, 1);
     return (uint8_t)data[0];
 
@@ -561,35 +461,6 @@
 
 void LSM9DS0::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest,
                             uint8_t count)
-{
-    /*char data[1]; // `data` will store the register data
-    data[0] = subAddress;
-    
-    
-    i2c_->write(address, data, 1, true);
-    i2c_->read(address, data, 1, true);
-    
-    dest[0] = data[0];
-    for (int i=1; i<count ;i++)
-    {
-        if(i == (count -1))
-        dest[i] = i2c_->read(0);
-        else
-        dest[i] = i2c_->read(1);
-    }
-    // End I2C Transmission
-    i2c_->stop();*/
-    /*char command[1];
-    command[0] = subAddress;
-    char *redData = (char*)malloc(count);
-    i2c_->write(address, command, 1, true);
-    
-    i2c_->read(address, redData, count);
-    for(int i =0; i < count; i++) {
-        dest[i] = redData[i];
-    }
-    
-    free(redData);*/
-    
+{   
     i2c_->readBytes(address, subAddress, count, dest);
 }