Allows for reading accelerometer, gyroscope, and magnetometer data from an LSM9DS0 IMU device
Dependents: uVGA_4180 uLCD_4180_mini ECE4781_Project
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); }