bool RTIMULSM9DS1::IMURead()
{
unsigned char status;
unsigned char gyroData[6];
unsigned char accelData[6];
unsigned char compassData[6];
if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_STATUS, 1, &status, "Failed to read LSM9DS1 status"))
return false;
if ((status & 0x3) == 0)
return false;
for (int i = 0; i<6; i++){
if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_OUT_X_L_G + i, 1, &gyroData[i], "Failed to read LSM9DS1 gyro data"))
return false;
if (!m_settings->HALRead(m_accelGyroSlaveAddr, LSM9DS1_OUT_X_L_XL + i, 1, &accelData[i], "Failed to read LSM9DS1 accel data"))
return false;
if (!m_settings->HALRead(m_magSlaveAddr, LSM9DS1_MAG_OUT_X_L + i, 1, &compassData[i], "Failed to read LSM9DS1 compass data"))
return false;
}
m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
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.setZ(-m_imuData.gyro.z());
// sort out accel data;
m_imuData.accel.setX(-m_imuData.accel.x());
m_imuData.accel.setY(-m_imuData.accel.y());
// sort out compass axes
m_imuData.compass.setX(-m_imuData.compass.x());
m_imuData.compass.setZ(-m_imuData.compass.z());
// now do standard processing
handleGyroBias();
calibrateAverageCompass();
calibrateAccel();
// now update the filter
updateFusion();
return true;
}
// Update Data
void Kinect::update()
{
// Update Color
updateColor();
// Update Depth
updateDepth();
// Update Fusion
updateFusion();
}
void RTIMU::setExtIMUData(RTFLOAT gx, RTFLOAT gy, RTFLOAT gz, RTFLOAT ax, RTFLOAT ay, RTFLOAT az,
RTFLOAT mx, RTFLOAT my, RTFLOAT mz, uint64_t timestamp)
{
m_imuData.gyro.setX(gx);
m_imuData.gyro.setY(gy);
m_imuData.gyro.setZ(gz);
m_imuData.accel.setX(ax);
m_imuData.accel.setY(ay);
m_imuData.accel.setZ(az);
m_imuData.compass.setX(mx);
m_imuData.compass.setY(my);
m_imuData.compass.setZ(mz);
m_imuData.timestamp = timestamp;
updateFusion();
}
bool RTIMUMPU9150::IMURead()
{
unsigned char fifoCount[2];
unsigned int count;
unsigned char fifoData[12];
unsigned char compassData[8];
if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_COUNT_H, 2, fifoCount, "Failed to read fifo count"))
return false;
count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1];
if (count == 1024) {
HAL_INFO("MPU9150 fifo has overflowed");
resetFifo();
m_imuData.timestamp += m_sampleInterval * (1024 / MPU9150_FIFO_CHUNK_SIZE + 1); // try to fix timestamp
return false;
}
#ifdef MPU9150_CACHE_MODE
if ((m_cacheCount == 0) && (count >= MPU9150_FIFO_CHUNK_SIZE) && (count < (MPU9150_CACHE_SIZE * MPU9150_FIFO_CHUNK_SIZE))) {
// special case of a small fifo and nothing cached - just handle as simple read
if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
return false;
if (!m_settings->HALRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, m_compassDataLength, compassData, "Failed to read compass data"))
return false;
} else {
if (count >= (MPU9150_CACHE_SIZE * MPU9150_FIFO_CHUNK_SIZE)) {
if (m_cacheCount == MPU9150_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 == MPU9150_CACHE_BLOCK_COUNT)
m_cacheOut = 0;
m_cacheCount--;
}
int blockCount = count / MPU9150_FIFO_CHUNK_SIZE; // number of chunks in fifo
if (blockCount > MPU9150_CACHE_SIZE)
blockCount = MPU9150_CACHE_SIZE;
if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE * blockCount,
m_cache[m_cacheIn].data, "Failed to read fifo data"))
return false;
if (!m_settings->HALRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, 8, m_cache[m_cacheIn].compass, "Failed to read compass data"))
return false;
m_cache[m_cacheIn].count = blockCount;
m_cache[m_cacheIn].index = 0;
m_cacheCount++;
if (++m_cacheIn == MPU9150_CACHE_BLOCK_COUNT)
m_cacheIn = 0;
}
// now fifo has been read if necessary, get something to process
if (m_cacheCount == 0)
return false;
memcpy(fifoData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, MPU9150_FIFO_CHUNK_SIZE);
memcpy(compassData, m_cache[m_cacheOut].compass, 8);
m_cache[m_cacheOut].index += MPU9150_FIFO_CHUNK_SIZE;
if (--m_cache[m_cacheOut].count == 0) {
// this cache block is now empty
if (++m_cacheOut == MPU9150_CACHE_BLOCK_COUNT)
m_cacheOut = 0;
m_cacheCount--;
}
}
#else
if (count > MPU9150_FIFO_CHUNK_SIZE * 40) {
// more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly
while (count >= MPU9150_FIFO_CHUNK_SIZE * 10) {
if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
return false;
count -= MPU9150_FIFO_CHUNK_SIZE;
m_imuData.timestamp += m_sampleInterval;
}
}
if (count < MPU9150_FIFO_CHUNK_SIZE)
return false;
if (!m_settings->HALRead(m_slaveAddr, MPU9150_FIFO_R_W, MPU9150_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
return false;
if (!m_settings->HALRead(m_slaveAddr, MPU9150_EXT_SENS_DATA_00, m_compassDataLength, compassData, "Failed to read compass data"))
return false;
#endif
RTMath::convertToVector(fifoData, m_imuData.accel, m_accelScale, true);
RTMath::convertToVector(fifoData + 6, m_imuData.gyro, m_gyroScale, true);
if (m_compassIs5883)
RTMath::convertToVector(compassData, m_imuData.compass, 0.092f, true);
else
RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.3f, false);
// 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());
if (m_compassIs5883) {
// sort out compass axes
float temp;
temp = m_imuData.compass.y();
m_imuData.compass.setY(-m_imuData.compass.z());
m_imuData.compass.setZ(-temp);
} else {
// use the compass fuse data adjustments
m_imuData.compass.setX(m_imuData.compass.x() * m_compassAdjust[0]);
m_imuData.compass.setY(m_imuData.compass.y() * m_compassAdjust[1]);
m_imuData.compass.setZ(m_imuData.compass.z() * m_compassAdjust[2]);
// sort out compass axes
float temp;
temp = m_imuData.compass.x();
m_imuData.compass.setX(m_imuData.compass.y());
m_imuData.compass.setY(-temp);
}
// now do standard processing
handleGyroBias();
calibrateAverageCompass();
calibrateAccel();
if (m_firstTime)
m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
else
m_imuData.timestamp += m_sampleInterval;
m_firstTime = false;
// now update the filter
updateFusion();
return true;
}
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 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;
}
bool RTIMUMPU9255::IMURead()
{
unsigned char fifoCount[2];
unsigned int count;
unsigned char fifoData[MPU9255_FIFO_CHUNK_SIZE];
#if MPU9255_FIFO_WITH_COMPASS == 0
unsigned char compassData[8]; // compass data goes here if it is not coming in through FIFO
#endif
#if MPU9255_FIFO_WITH_TEMP == 0
unsigned char temperatureData[2]; // if temperature data is not coming in through FIFO
#endif
if (!m_settings->HALRead(m_slaveAddr, MPU9255_FIFO_COUNT_H, 2, fifoCount, "Failed to read fifo count"))
return false;
count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1];
if (count == 512) {
HAL_INFO("MPU-9255 fifo has overflowed");
resetFifo();
m_imuData.timestamp += m_sampleInterval * (512 / MPU9255_FIFO_CHUNK_SIZE + 1); // try to fix timestamp
return false;
}
#ifdef MPU9255_CACHE_MODE
if ( (m_cacheCount == 0) && (count < MPU9255_FIFO_CHUNK_SIZE) )
return false; // no new set of data available
if ((m_cacheCount == 0) && (count >= MPU9255_FIFO_CHUNK_SIZE) && (count < (MPU9255_CACHE_SIZE * MPU9255_FIFO_CHUNK_SIZE))) {
// special case of a small fifo and nothing cached - just handle as simple read
if (!m_settings->HALRead(m_slaveAddr, MPU9255_FIFO_R_W, MPU9255_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
return false;
#if MPU9255_FIFO_WITH_TEMP == 0 // read temp from registers
if (!m_settings->HALRead(m_slaveAddr, MPU9255_TEMP_OUT_H, 2,
temperatureData, "Failed to read temperature data"))
return false;
#endif
#if MPU9255_FIFO_WITH_COMPASS == 0 // read compass without fifo
if (!m_settings->HALRead(m_slaveAddr, MPU9255_EXT_SENS_DATA_00, 8, compassData, "Failed to read compass data"))
return false;
#endif
} else {
if (count >= (MPU9255_CACHE_SIZE * MPU9255_FIFO_CHUNK_SIZE)) {
if (m_cacheCount == MPU9255_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 == MPU9255_CACHE_BLOCK_COUNT)
m_cacheOut = 0;
m_cacheCount--;
}
int blockCount = count / MPU9255_FIFO_CHUNK_SIZE; // number of chunks in fifo
if (blockCount > MPU9255_CACHE_SIZE)
blockCount = MPU9255_CACHE_SIZE;
if (!m_settings->HALRead(m_slaveAddr, MPU9255_FIFO_R_W, MPU9255_FIFO_CHUNK_SIZE * blockCount,
m_cache[m_cacheIn].data, "Failed to read fifo data"))
return false;
#if MPU9255_FIFO_WITH_TEMP == 0 // read temp from registers
if (!m_settings->HALRead(m_slaveAddr, MPU9255_TEMP_OUT_H, 2,
m_cache[m_cacheIn].temperature, "Failed to read temperature data"))
return false;
#endif
#if MPU9255_FIFO_WITH_COMPASS == 0 // read compass from register
if (!m_settings->HALRead(m_slaveAddr, MPU9255_EXT_SENS_DATA_00, 8, m_cache[m_cacheIn].compass, "Failed to read compass data"))
return false;
#endif
m_cache[m_cacheIn].count = blockCount;
m_cache[m_cacheIn].index = 0;
m_cacheCount++;
if (++m_cacheIn == MPU9255_CACHE_BLOCK_COUNT)
m_cacheIn = 0;
}
// now fifo has been read if necessary, get something to process
if (m_cacheCount == 0)
return false;
memcpy(fifoData, m_cache[m_cacheOut].data + m_cache[m_cacheOut].index, MPU9255_FIFO_CHUNK_SIZE);
#if MPU9255_FIFO_WITH_COMPASS == 0
memcpy(compassData, m_cache[m_cacheOut].compass, 8);
#endif
#if MPU9255_FIFO_WITH_TEMP == 0
memcpy(temperatureData, m_cache[m_cacheOut].temperature, 2);
#endif
m_cache[m_cacheOut].index += MPU9255_FIFO_CHUNK_SIZE;
if (--m_cache[m_cacheOut].count == 0) {
// this cache block is now empty
if (++m_cacheOut == MPU9255_CACHE_BLOCK_COUNT)
m_cacheOut = 0;
m_cacheCount--;
}
}
#else
if (count > MPU9255_FIFO_CHUNK_SIZE * 40) {
// more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly
while (count >= MPU9255_FIFO_CHUNK_SIZE * 10) {
if (!m_settings->HALRead(m_slaveAddr, MPU9255_FIFO_R_W, MPU9255_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
return false;
count -= MPU9255_FIFO_CHUNK_SIZE;
m_imuData.timestamp += m_sampleInterval;
}
}
if (count < MPU9255_FIFO_CHUNK_SIZE)
return false;
if (!m_settings->HALRead(m_slaveAddr, MPU9255_FIFO_R_W, MPU9255_FIFO_CHUNK_SIZE, fifoData, "Failed to read fifo data"))
return false;
#if MPU9255_FIFO_WITH_TEMP == 0
if (!m_settings->HALRead(m_slaveAddr, MPU9255_TEMP_OUT_H, 2, temperatureData, "Failed to read temperature data"))
return false;
#endif
#if MPU9255_FIFO_WITH_COMPASS == 0
if (!m_settings->HALRead(m_slaveAddr, MPU9255_EXT_SENS_DATA_00, 8, compassData, "Failed to read compass data"))
return false;
#endif
#endif
// Accelerometer
RTMath::convertToVector(fifoData, m_imuData.accel, m_accelScale, true);
#if MPU9255_FIFO_WITH_TEMP == 1
// Temperature
m_imuData.IMUtemperature = ((RTFLOAT)((int16_t)(((uint16_t)fifoData[6] << 8) | (uint16_t)fifoData[7])) / 333.87f ) + 21.0f; // combined registers and convert to temperature
m_imuData.IMUtemperatureValid = true;
// Gyroscope
RTMath::convertToVector(fifoData + 8, m_imuData.gyro, m_gyroScale, true);
// Compass
#if MPU9255_FIFO_WITH_COMPASS == 1
RTMath::convertToVector(fifoData + 14 + 1, m_imuData.compass, 0.6f, false);
#else
RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.6f, false);
#endif
#else // no temp in fifo
// Temperature
m_imuData.IMUtemperature = ((RTFLOAT)((int16_t)(((uint16_t)fifoData[6] << 8) | (uint16_t)fifoData[7])) / 333.87f ) + 21.0f; // combined registers and convert to temperature
m_imuData.IMUtemperatureValid = true;
// ((TEMP_OUT – RoomTemp_Offset)/Temp_Sensitivity) + 21degC
// 333.87 = sensitivity
// 0 = room temp offset at 21
// Gyroscope
RTMath::convertToVector(fifoData + 6, m_imuData.gyro, m_gyroScale, true);
// Compass
#if MPU9255_FIFO_WITH_COMPASS == 1 // without temp but with compass in FIFO
RTMath::convertToVector(fifoData + 12 + 1, m_imuData.compass, 0.6f, false);
#else
RTMath::convertToVector(compassData + 1, m_imuData.compass, 0.6f, false);
#endif
#endif
// 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());
// use the compass fuse data adjustments
m_imuData.compass.setX(m_imuData.compass.x() * m_compassAdjust[0]);
m_imuData.compass.setY(m_imuData.compass.y() * m_compassAdjust[1]);
m_imuData.compass.setZ(m_imuData.compass.z() * m_compassAdjust[2]);
// sort out compass axes
float temp;
temp = m_imuData.compass.x();
m_imuData.compass.setX(m_imuData.compass.y());
m_imuData.compass.setY(-temp);
if (m_firstTime)
m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
else
m_imuData.timestamp += m_sampleInterval;
m_firstTime = false;
// now do standard processing
if (m_imuData.IMUtemperatureValid == true) {
// Check if temperature changed
if (fabs(m_imuData.IMUtemperature - m_IMUtemperature_previous) >= TEMPERATURE_DELTA) {
// If yes, update bias
updateTempBias(m_imuData.IMUtemperature);
m_IMUtemperature_previous = m_imuData.IMUtemperature;
}
// Then do
handleTempBias(); // temperature Correction
}
handleGyroBias();
calibrateAverageCompass();
calibrateAccel();
// now update the filter
updateFusion();
return true;
}
bool RTIMUBMX055::IMURead()
{
unsigned char status;
unsigned char gyroData[6];
unsigned char accelData[6];
unsigned char magData[8];
if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_FIFO_STATUS, 1, &status, "Failed to read BMX055 gyro fifo status"))
return false;
if (status & 0x80) {
// fifo overflowed
HAL_ERROR("BMX055 fifo overflow\n");
// this should clear it
if (!m_settings->HALWrite(m_gyroSlaveAddr, BMX055_GYRO_FIFO_CONFIG_1, 0x40, "Failed to set BMX055 FIFO config"))
return false;
m_imuData.timestamp = RTMath::currentUSecsSinceEpoch(); // try to fix timestamp
return false;
}
if (status == 0)
return false;
if (!m_settings->HALRead(m_gyroSlaveAddr, BMX055_GYRO_FIFO_DATA, 6, gyroData, "Failed to read BMX055 gyro data"))
return false;
if (!m_settings->HALRead(m_accelSlaveAddr, BMX055_ACCEL_X_LSB, 6, accelData, "Failed to read BMX055 accel data"))
return false;
if (!m_settings->HALRead(m_magSlaveAddr, BMX055_MAG_X_LSB, 8, magData, "Failed to read BMX055 mag data"))
return false;
RTMath::convertToVector(gyroData, m_imuData.gyro, m_gyroScale, false);
// need to prepare accel data
accelData[0] &= 0xf0;
accelData[2] &= 0xf0;
accelData[4] &= 0xf0;
RTMath::convertToVector(accelData, m_imuData.accel, m_accelScale, false);
float mx, my, mz;
processMagData(magData, mx, my, mz);
m_imuData.compass.setX(mx);
m_imuData.compass.setY(my);
m_imuData.compass.setZ(mz);
// sort out gyro axes
m_imuData.gyro.setY(-m_imuData.gyro.y());
m_imuData.gyro.setZ(-m_imuData.gyro.z());
// sort out accel axes
m_imuData.accel.setX(-m_imuData.accel.x());
// sort out mag axes
#ifdef BMX055_REMAP
m_imuData.compass.setY(-m_imuData.compass.y());
m_imuData.compass.setZ(-m_imuData.compass.z());
#else
RTFLOAT temp = m_imuData.compass.x();
m_imuData.compass.setX(-m_imuData.compass.y());
m_imuData.compass.setY(-temp);
m_imuData.compass.setZ(-m_imuData.compass.z());
#endif
// now do standard processing
handleGyroBias();
calibrateAverageCompass();
calibrateAccel();
if (m_firstTime)
m_imuData.timestamp = RTMath::currentUSecsSinceEpoch();
else
m_imuData.timestamp += m_sampleInterval;
m_firstTime = false;
// now update the filter
updateFusion();
return true;
}
bool RTIMUNull::IMURead()
{
updateFusion();
return true;
}
