This is a port from the library for Arduino provided by Sparkfun with their breakout board of the LSM9DS0. The original library can be found here: https://github.com/sparkfun/SparkFun_LSM9DS0_Arduino_Library/tree/V_1.0.1 It is also provided an AHRS example based on Madgwick, also a port from an Arduino example. All of this was tested on a Nucleo F411RE and a Sparkfun breakout board.
Diff: LSM9DS0_mbed.cpp
- Revision:
- 0:32b177f0030e
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM9DS0_mbed.cpp Sat Dec 05 16:23:36 2015 +0000 @@ -0,0 +1,427 @@ +/* +Code by @OlimexSmart - Luca Olivieri +This is a port from the Sparkfun library provided +with the breakout board of the LSM9DS0. +Visit their github for full comments: +https://github.com/sparkfun/SparkFun_LSM9DS0_Arduino_Library/tree/V_1.0.1 +*/ + +#include "LSM9DS0_mbed.h" + +LSM9DS0::LSM9DS0(PinName sdaP, PinName sclP, uint8_t gAddr, uint8_t xmAddr) +{ + // xmAddress and gAddress will store the 7-bit I2C address. + xmAddress = xmAddr; + gAddress = gAddr; + + i2c_ = new I2C(sdaP, sclP); //This is initI2C(); in the original library + i2c_->frequency(400000); + +} + +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 + // are used throughout to calculate the actual g's, DPS,and Gs's. + 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 + + + // 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. + + // Once everything is initialized, return the WHO_AM_I registers we read: + return (xmTest << 8) | gTest; +} + +void LSM9DS0::initGyro() +{ + + gWriteByte(CTRL_REG1_G, 0x0F); // Normal mode, enable all axes + 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 + +} + +void LSM9DS0::initAccel() +{ + 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 + xmWriteByte(CTRL_REG3_XM, 0x04); // Accelerometer data ready on INT1_XM (0x04) + +} + +void LSM9DS0::initMag() +{ + xmWriteByte(CTRL_REG5_XM, 0x94); // Mag data rate - 100 Hz, enable temperature sensor + xmWriteByte(CTRL_REG6_XM, 0x00); // Mag scale to +/- 2GS + xmWriteByte(CTRL_REG7_XM, 0x00); // Continuous conversion mode + xmWriteByte(CTRL_REG4_XM, 0x04); // Magnetometer data ready on INT2_XM (0x08) + xmWriteByte(INT_CTRL_REG_M, 0x09); // Enable interrupts for mag, active-low, push-pull +} + +// This is a function that uses the FIFO to accumulate sample of accelerometer and gyro data, average +// them, scales them to gs and deg/s, respectively, and then passes the biases to the main sketch +// for subtraction from all subsequent data. There are no gyro and accelerometer bias registers to store +// the data as there are in the ADXL345, a precursor to the LSM9DS0, or the MPU-9150, so we have to +// subtract the biases ourselves. This results in a more accurate measurement in general and can +// 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 + + 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; + + 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 + 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./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 + 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 + az = (temp[5] << 8) | temp[4]; // Store z-axis values into az + +} + +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 +} + +void LSM9DS0::readTemp() +{ + 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_XM + //temperature = (((int16_t) temp[1] << 12) | temp[0] << 4 ) >> 4; // Temperature is a 12-bit signed integer + + uint8_t xlo = temp[0]; + int16_t xhi = temp[1]; + xhi <<= 8; + xhi |= xlo; + temperature = xhi; +} + + +void LSM9DS0::readGyro() +{ + 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 +} + +float LSM9DS0::calcGyro(int16_t gyro) +{ + // Return the gyro raw reading times our pre-calculated DPS / (ADC tick): + return gRes * gyro; +} + +float LSM9DS0::calcAccel(int16_t accel) +{ + // Return the accel raw reading times our pre-calculated g's / (ADC tick): + return aRes * accel; +} + +float LSM9DS0::calcMag(int16_t mag) +{ + // Return the mag raw reading times our pre-calculated Gs / (ADC tick): + return mRes * mag; +} + +void LSM9DS0::setGyroScale(gyro_scale gScl) +{ + // We need to preserve the other bytes in CTRL_REG4_G. So, first read it: + uint8_t temp = gReadByte(CTRL_REG4_G); + // Then mask out the gyro scale bits: + temp &= 0xFF^(0x3 << 4); + // Then shift in our new scale bits: + 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; + // Then calculate a new gRes, which relies on gScale being set correctly: + calcgRes(); +} + +void LSM9DS0::setAccelScale(accel_scale aScl) +{ + // We need to preserve the other bytes in CTRL_REG2_XM. So, first read it: + uint8_t temp = xmReadByte(CTRL_REG2_XM); + // Then mask out the accel scale bits: + temp &= 0xFF^(0x3 << 3); + // Then shift in our new scale bits: + 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; + // Then calculate a new aRes, which relies on aScale being set correctly: + calcaRes(); +} + +void LSM9DS0::setMagScale(mag_scale mScl) +{ + // We need to preserve the other bytes in CTRL_REG6_XM. So, first read it: + uint8_t temp = xmReadByte(CTRL_REG6_XM); + // Then mask out the mag scale bits: + temp &= 0xFF^(0x3 << 5); + // Then shift in our new scale bits: + 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; + // Then calculate a new mRes, which relies on mScale being set correctly: + calcmRes(); +} + +void LSM9DS0::setGyroODR(gyro_odr gRate) +{ + // We need to preserve the other bytes in CTRL_REG1_G. So, first read it: + uint8_t temp = gReadByte(CTRL_REG1_G); + // Then mask out the gyro ODR bits: + temp &= 0xFF^(0xF << 4); + // Then shift in our new ODR bits: + temp |= (gRate << 4); + // And write the new register value back into CTRL_REG1_G: + gWriteByte(CTRL_REG1_G, temp); +} + +void LSM9DS0::setAccelODR(accel_odr aRate) +{ + // We need to preserve the other bytes in CTRL_REG1_XM. So, first read it: + uint8_t temp = xmReadByte(CTRL_REG1_XM); + // Then mask out the accel ODR bits: + temp &= 0xFF^(0xF << 4); + // Then shift in our new ODR bits: + temp |= (aRate << 4); + // And write the new register value back into CTRL_REG1_XM: + xmWriteByte(CTRL_REG1_XM, temp); +} + +void LSM9DS0::setMagODR(mag_odr mRate) +{ + // We need to preserve the other bytes in CTRL_REG5_XM. So, first read it: + uint8_t temp = xmReadByte(CTRL_REG5_XM); + // Then mask out the mag ODR bits: + temp &= 0xFF^(0x7 << 2); + // Then shift in our new ODR bits: + temp |= (mRate << 2); + // And write the new register value back into CTRL_REG5_XM: + xmWriteByte(CTRL_REG5_XM, temp); +} + +void LSM9DS0::setAccelABW(accel_abw abwRate) +{ + // We need to preserve the other bytes in CTRL_REG2_XM. So, first read it: + uint8_t temp = xmReadByte(CTRL_REG2_XM); + // Then mask out the accel ABW bits: + temp &= 0xFF^(0x3 << 7); + // Then shift in our new ODR bits: + temp |= (abwRate << 7); + // And write the new register value back into CTRL_REG2_XM: + xmWriteByte(CTRL_REG2_XM, temp); +} + +void LSM9DS0::calcgRes() +{ + // 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; + } +} + +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 + // algorithm to calculate g/(ADC tick) based on that 3-bit value: + aRes = aScale == A_SCALE_16G ? 16.0 / 32768.0 : + (((float) aScale + 1.0) * 2.0) / 32768.0; +} + +void LSM9DS0::calcmRes() +{ + // 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 : + (float) (mScale << 2) / 32768.0; +} + +void LSM9DS0::gWriteByte(uint8_t subAddress, uint8_t data) +{ + // Whether we're using I2C or SPI, write a byte using the + // gyro-specific I2C address or SPI CS pin. + I2CwriteByte(gAddress, subAddress, data); +} + +void LSM9DS0::xmWriteByte(uint8_t subAddress, uint8_t data) +{ + // Whether we're using I2C or SPI, write a byte using the + // accelerometer-specific I2C address or SPI CS pin. + return I2CwriteByte(xmAddress, subAddress, data); +} + +uint8_t LSM9DS0::gReadByte(uint8_t subAddress) +{ + return I2CreadByte(gAddress, subAddress); +} + +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. + 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. + return I2CreadByte(xmAddress, subAddress); +} + +void LSM9DS0::xmReadBytes(uint8_t subAddress, uint8_t * dest, uint8_t count) +{ + // read multiple bytes using the + // accelerometer-specific I2C address. + I2CreadBytes(xmAddress, subAddress, dest, count); +} + + +//I2C rewritten to accomodate i2cdev instead of Wire (Arduino) +void LSM9DS0::I2CwriteByte(uint8_t address, uint8_t subAddress, uint8_t data) +{ + char dt[2]; // Initialize the Tx buffer + dt[0] = subAddress; // Put slave register address in Tx buffer + dt[1] = data; // Put data in Tx buffer + i2c_->write(address << 1, dt, 2); // Send the Tx buffer +} + +uint8_t LSM9DS0::I2CreadByte(uint8_t address, uint8_t subAddress) +{ + i2c_->write(address << 1, (char*)&subAddress, 1, true); // Send request, but keep connection alive + char dt = 0; + i2c_->read(address << 1, &dt, 1); // Fill Rx buffer with result + + return dt; // Return data read from slave register + +} + +void LSM9DS0::I2CreadBytes(uint8_t address, uint8_t subAddress, uint8_t * dest, + uint8_t count) +{ + char sA = subAddress | 0x80; // Send the register to be read. OR with 0x80 to indicate multi-read. + i2c_->write(address << 1, &sA, 1, true); // Send the Tx buffer, but keep connection alive + i2c_->read(address << 1, (char*)dest, count); // Read bytes from slave register address +}