bool RTIMULSM9DS0::IMURead()
{
unsigned char status;
unsigned char gyroData[6];
unsigned char accelData[6];
unsigned char compassData[6];
#ifdef LSM9DS0_CACHE_MODE
int count;
if (!I2CRead(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_SRC, 1, &status, "Failed to read LSM9DS0 gyro fifo status"))
return false;
if ((status & 0x40) != 0) {
HAL_INFO("LSM9DS0 gyro fifo overrun\n");
if (!I2CWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x10, "Failed to set LSM9DS0 gyro CTRL5"))
return false;
if (!I2CWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x0, "Failed to set LSM9DS0 gyro FIFO mode"))
return false;
if (!I2CWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x3f, "Failed to set LSM9DS0 gyro FIFO mode"))
return false;
if (!setGyroCTRL5())
return false;
m_imuData.timestamp += m_sampleInterval * 32;
return false;
}
// get count of samples in fifo
count = status & 0x1f;
if ((m_cacheCount == 0) && (count > 0) && (count < LSM9DS0_FIFO_THRESH)) {
// special case of a small fifo and nothing cached - just handle as simple read
if (!I2CRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, 6, gyroData, "Failed to read LSM9DS0 gyro data"))
return false;
if (!I2CRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6, accelData, "Failed to read LSM9DS0 accel data"))
return false;
if (!I2CRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6, compassData, "Failed to read LSM9DS0 compass data"))
return false;
if (m_firstTime)
m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
else
m_imuData.timestamp += m_sampleInterval;
m_firstTime = false;
} else {
if (count >= LSM9DS0_FIFO_THRESH) {
// need to create a cache block
if (m_cacheCount == LSM9DS0_CACHE_BLOCK_COUNT) {
// all cache blocks are full - discard oldest and update timestamp to account for lost samples
m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
if (++m_cacheOut == LSM9DS0_CACHE_BLOCK_COUNT)
m_cacheOut = 0;
m_cacheCount--;
}
if (!I2CRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, LSM9DS0_FIFO_CHUNK_SIZE * LSM9DS0_FIFO_THRESH,
m_cache[m_cacheIn].data, "Failed to read LSM9DS0 fifo data"))
return false;
if (!I2CRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6,
m_cache[m_cacheIn].accel, "Failed to read LSM9DS0 accel data"))
return false;
if (!I2CRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6,
m_cache[m_cacheIn].compass, "Failed to read LSM9DS0 compass data"))
return false;
m_cache[m_cacheIn].count = LSM9DS0_FIFO_THRESH;
m_cache[m_cacheIn].index = 0;
m_cacheCount++;
if (++m_cacheIn == LSM9DS0_CACHE_BLOCK_COUNT)
m_cacheIn = 0;
}
// now fifo has been read if necessary, get something to process
if (m_cacheCount == 0)
return false;
memcpy(gyroData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, LSM9DS0_FIFO_CHUNK_SIZE);
memcpy(accelData, m_cache[m_cacheOut].accel, 6);
memcpy(compassData, m_cache[m_cacheOut].compass, 6);
m_cache[m_cacheOut].index += LSM9DS0_FIFO_CHUNK_SIZE;
if (--m_cache[m_cacheOut].count == 0) {
// this cache block is now empty
if (++m_cacheOut == LSM9DS0_CACHE_BLOCK_COUNT)
m_cacheOut = 0;
m_cacheCount--;
}
if (m_firstTime)
m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
else
m_imuData.timestamp += m_sampleInterval;
m_firstTime = false;
}
#else
if (!I2CRead(m_gyroSlaveAddr, LSM9DS0_GYRO_STATUS, 1, &status, "Failed to read LSM9DS0 status"))
return false;
if ((status && 0x8) == 0)
return false;
if (!I2CRead(m_gyroSlaveAddr, 0x80 | LSM9DS0_GYRO_OUT_X_L, 6, gyroData, "Failed to read LSM9DS0 gyro data"))
return false;
m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
if (!I2CRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_A, 6, accelData, "Failed to read LSM9DS0 accel data"))
return false;
if (!I2CRead(m_accelCompassSlaveAddr, 0x80 | LSM9DS0_OUT_X_L_M, 6, compassData, "Failed to read LSM9DS0 compass data"))
return false;
#endif
RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
RTMath::convertToVector(compassData, m_imuData.compass, m_compassScale, false);
// sort out gyro axes and correct for bias
m_imuData.gyro.setX(m_imuData.gyro.x());
m_imuData.gyro.setY(-m_imuData.gyro.y());
m_imuData.gyro.setZ(-m_imuData.gyro.z());
// sort out accel data;
m_imuData.accel.setX(-m_imuData.accel.x());
// sort out compass axes
m_imuData.compass.setY(-m_imuData.compass.y());
// now do standard processing
handleGyroBias();
calibrateAverageCompass();
// now update the filter
updateFusion();
return true;
}
bool RTIMULSM9DS0::IMUInit()
{
unsigned char result;
#ifdef LSM9DS0_CACHE_MODE
m_firstTime = true;
m_cacheIn = m_cacheOut = m_cacheCount = 0;
#endif
// set validity flags
m_imuData.fusionPoseValid = false;
m_imuData.fusionQPoseValid = false;
m_imuData.gyroValid = true;
m_imuData.accelValid = true;
m_imuData.compassValid = true;
m_imuData.pressureValid = false;
m_imuData.temperatureValid = false;
m_imuData.humidityValid = false;
// configure IMU
m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
if (m_gyroSlaveAddr == LSM9DS0_GYRO_ADDRESS0)
m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS0;
else
m_accelCompassSlaveAddr = LSM9DS0_ACCELMAG_ADDRESS1;
m_bus = m_settings->m_I2CBus;
setCalibrationData(m_settings->m_compassCalValid, m_settings->m_compassCalMin,
m_settings->m_compassCalMax);
// enable the I2C bus
setI2CBus(m_bus);
if (!I2COpen())
return false;
// Set up the gyro
if (!I2CWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_CTRL5, 0x80, "Failed to boot LSM9DS0"))
return false;
if (!I2CRead(m_gyroSlaveAddr, LSM9DS0_GYRO_WHO_AM_I, 1, &result, "Failed to read LSM9DS0 gyro id"))
return false;
if (result != LSM9DS0_GYRO_ID) {
HAL_ERROR1("Incorrect LSM9DS0 gyro id %d\n", result);
return false;
}
if (!setGyroSampleRate())
return false;
if (!setGyroCTRL2())
return false;
if (!setGyroCTRL4())
return false;
// Set up the accel
if (!I2CRead(m_accelCompassSlaveAddr, LSM9DS0_WHO_AM_I, 1, &result, "Failed to read LSM9DS0 accel/mag id"))
return false;
if (result != LSM9DS0_ACCELMAG_ID) {
HAL_ERROR1("Incorrect LSM9DS0 accel/mag id %d\n", result);
return false;
}
if (!setAccelCTRL1())
return false;
if (!setAccelCTRL2())
return false;
if (!setCompassCTRL5())
return false;
if (!setCompassCTRL6())
return false;
if (!setCompassCTRL7())
return false;
#ifdef LSM9DS0_CACHE_MODE
// turn on gyro fifo
if (!I2CWrite(m_gyroSlaveAddr, LSM9DS0_GYRO_FIFO_CTRL, 0x3f, "Failed to set LSM9DS0 FIFO mode"))
return false;
#endif
if (!setGyroCTRL5())
return false;
gyroBiasInit();
HAL_INFO("LSM9DS0 init complete\n");
return true;
}
int RTIMUGD20HM303DLHC::IMUInit()
{
unsigned char result;
// configure IMU
m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
m_accelSlaveAddr = LSM303DLHC_ACCEL_ADDRESS;
m_compassSlaveAddr = LSM303DLHC_COMPASS_ADDRESS;
setCalibrationData();
// Set up the gyro
if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04))
return -1;
if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80))
return -2;
if (!I2CRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result))
return -3;
if (result != L3GD20H_ID) {
return -4;
}
if (!setGyroSampleRate())
return -5;
if (!setGyroCTRL2())
return -6;
if (!setGyroCTRL4())
return -7;
// Set up the accel
if (!setAccelCTRL1())
return -8;
if (!setAccelCTRL4())
return -9;
// Set up the compass
if (!setCompassCRA())
return -10;
if (!setCompassCRB())
return -11;
if (!setCompassCRM())
return -12;
if (!setGyroCTRL5())
return -13;
gyroBiasInit();
return true;
}
bool RTIMUGD20HM303DLHC::IMUInit()
{
unsigned char result;
#ifdef GD20HM303DLHC_CACHE_MODE
m_firstTime = true;
m_cacheIn = m_cacheOut = m_cacheCount = 0;
#endif
// set validity flags
m_imuData.fusionPoseValid = false;
m_imuData.fusionQPoseValid = false;
m_imuData.gyroValid = true;
m_imuData.accelValid = true;
m_imuData.compassValid = true;
m_imuData.pressureValid = false;
m_imuData.temperatureValid = false;
m_imuData.humidityValid = false;
// configure IMU
m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
m_accelSlaveAddr = LSM303DLHC_ACCEL_ADDRESS;
m_compassSlaveAddr = LSM303DLHC_COMPASS_ADDRESS;
setCalibrationData();
// enable the I2C bus
if (!m_settings->HALOpen())
return false;
// Set up the gyro
if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04, "Failed to reset L3GD20H"))
return false;
if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80, "Failed to boot L3GD20H"))
return false;
if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result, "Failed to read L3GD20H id"))
return false;
if (result != L3GD20H_ID) {
HAL_ERROR1("Incorrect L3GD20H id %d\n", result);
return false;
}
if (!setGyroSampleRate())
return false;
if (!setGyroCTRL2())
return false;
if (!setGyroCTRL4())
return false;
// Set up the accel
if (!setAccelCTRL1())
return false;
if (!setAccelCTRL4())
return false;
// Set up the compass
if (!setCompassCRA())
return false;
if (!setCompassCRB())
return false;
if (!setCompassCRM())
return false;
#ifdef GD20HM303DLHC_CACHE_MODE
// turn on gyro fifo
if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20_FIFO_CTRL, 0x3f, "Failed to set L3GD20 FIFO mode"))
return false;
#endif
if (!setGyroCTRL5())
return false;
gyroBiasInit();
HAL_INFO("GD20HM303DLHC init complete\n");
return true;
}
bool RTIMUGD20HM303DLHC::IMURead()
{
unsigned char status;
unsigned char gyroData[6];
unsigned char accelData[6];
unsigned char compassData[6];
#ifdef GD20HM303DLHC_CACHE_MODE
int count;
if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_FIFO_SRC, 1, &status, "Failed to read L3GD20 fifo status"))
return false;
if ((status & 0x40) != 0) {
HAL_INFO("L3GD20 fifo overrun\n");
if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x10, "Failed to set L3GD20 CTRL5"))
return false;
if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_FIFO_CTRL, 0x0, "Failed to set L3GD20 FIFO mode"))
return false;
if (!m_settings->HALWrite(m_gyroSlaveAddr, L3GD20H_FIFO_CTRL, 0x3f, "Failed to set L3GD20 FIFO mode"))
return false;
if (!setGyroCTRL5())
return false;
m_imuData.timestamp += m_sampleInterval * 32;
return false;
}
// get count of samples in fifo
count = status & 0x1f;
if ((m_cacheCount == 0) && (count > 0) && (count < GD20HM303DLHC_FIFO_THRESH)) {
// special case of a small fifo and nothing cached - just handle as simple read
if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, 6, gyroData, "Failed to read L3GD20 data"))
return false;
if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6, accelData, "Failed to read LSM303DLHC accel data"))
return false;
if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6, compassData, "Failed to read LSM303DLHC compass data"))
return false;
if (m_firstTime)
m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
else
m_imuData.timestamp += m_sampleInterval;
m_firstTime = false;
} else {
if (count >= GD20HM303DLHC_FIFO_THRESH) {
// need to create a cache block
if (m_cacheCount == GD20HM303DLHC_CACHE_BLOCK_COUNT) {
// all cache blocks are full - discard oldest and update timestamp to account for lost samples
m_imuData.timestamp += m_sampleInterval * m_cache[m_cacheOut].count;
if (++m_cacheOut == GD20HM303DLHC_CACHE_BLOCK_COUNT)
m_cacheOut = 0;
m_cacheCount--;
}
if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, GD20HM303DLHC_FIFO_CHUNK_SIZE * GD20HM303DLHC_FIFO_THRESH,
m_cache[m_cacheIn].data, "Failed to read L3GD20 fifo data"))
return false;
if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6,
m_cache[m_cacheIn].accel, "Failed to read LSM303DLHC accel data"))
return false;
if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6,
m_cache[m_cacheIn].compass, "Failed to read LSM303DLHC compass data"))
return false;
m_cache[m_cacheIn].count = GD20HM303DLHC_FIFO_THRESH;
m_cache[m_cacheIn].index = 0;
m_cacheCount++;
if (++m_cacheIn == GD20HM303DLHC_CACHE_BLOCK_COUNT)
m_cacheIn = 0;
}
// now fifo has been read if necessary, get something to process
if (m_cacheCount == 0)
return false;
memcpy(gyroData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, GD20HM303DLHC_FIFO_CHUNK_SIZE);
memcpy(accelData, m_cache[m_cacheOut].accel, 6);
memcpy(compassData, m_cache[m_cacheOut].compass, 6);
m_cache[m_cacheOut].index += GD20HM303DLHC_FIFO_CHUNK_SIZE;
if (--m_cache[m_cacheOut].count == 0) {
// this cache block is now empty
if (++m_cacheOut == GD20HM303DLHC_CACHE_BLOCK_COUNT)
m_cacheOut = 0;
m_cacheCount--;
}
if (m_firstTime)
m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
else
m_imuData.timestamp += m_sampleInterval;
m_firstTime = false;
}
#else
if (!m_settings->HALRead(m_gyroSlaveAddr, L3GD20H_STATUS, 1, &status, "Failed to read L3GD20H status"))
return false;
if ((status & 0x8) == 0)
return false;
if (!m_settings->HALRead(m_gyroSlaveAddr, 0x80 | L3GD20H_OUT_X_L, 6, gyroData, "Failed to read L3GD20H data"))
return false;
m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
if (!m_settings->HALRead(m_accelSlaveAddr, 0x80 | LSM303DLHC_OUT_X_L_A, 6, accelData, "Failed to read LSM303DLHC accel data"))
return false;
if (!m_settings->HALRead(m_compassSlaveAddr, 0x80 | LSM303DLHC_OUT_X_H_M, 6, compassData, "Failed to read LSM303DLHC compass data"))
return false;
#endif
RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
m_imuData.compass.setX((RTFLOAT)((int16_t)(((uint16_t)compassData[0] << 8) | (uint16_t)compassData[1])) * m_compassScaleXY);
m_imuData.compass.setY((RTFLOAT)((int16_t)(((uint16_t)compassData[2] << 8) | (uint16_t)compassData[3])) * m_compassScaleXY);
m_imuData.compass.setZ((RTFLOAT)((int16_t)(((uint16_t)compassData[4] << 8) | (uint16_t)compassData[5])) * m_compassScaleZ);
// sort out gyro axes
m_imuData.gyro.setX(m_imuData.gyro.x());
m_imuData.gyro.setY(-m_imuData.gyro.y());
m_imuData.gyro.setZ(-m_imuData.gyro.z());
// sort out accel data;
m_imuData.accel.setX(-m_imuData.accel.x());
// sort out compass axes
RTFLOAT temp;
temp = m_imuData.compass.z();
m_imuData.compass.setZ(-m_imuData.compass.y());
m_imuData.compass.setY(-temp);
// now do standard processing
handleGyroBias();
calibrateAverageCompass();
calibrateAccel();
// now update the filter
updateFusion();
return true;
}
int RTIMUGD20HM303D::IMUInit()
{
unsigned char result;
// configure IMU
m_gyroSlaveAddr = m_settings->m_I2CSlaveAddress;
if (m_gyroSlaveAddr == L3GD20H_ADDRESS0)
m_accelCompassSlaveAddr = LSM303D_ADDRESS0;
else
m_accelCompassSlaveAddr = LSM303D_ADDRESS1;
setCalibrationData();
// Set up the gyro
if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_LOW_ODR, 0x04))
return -1;
if (!I2CWrite(m_gyroSlaveAddr, L3GD20H_CTRL5, 0x80))
return -2;
if (!I2CRead(m_gyroSlaveAddr, L3GD20H_WHO_AM_I, 1, &result))
return -3;
if (result != L3GD20H_ID) {
return -4;
}
if (!setGyroSampleRate())
return -5;
if (!setGyroCTRL2())
return -6;
if (!setGyroCTRL4())
return -7;
// Set up the accel/compass
if (!I2CRead(m_accelCompassSlaveAddr, LSM303D_WHO_AM_I, 1, &result))
return -8;
if (result != LSM303D_ID) {
return -9;
}
if (!setAccelCTRL1())
return -10;
if (!setAccelCTRL2())
return -11;
if (!setCompassCTRL5())
return -12;
if (!setCompassCTRL6())
return -13;
if (!setCompassCTRL7())
return -14;
if (!setGyroCTRL5())
return -16;
gyroBiasInit();
return true;
}
