void Accelerometer::clearFifo()
{
uint8_t tmpBuffer[6] = {0};
uint8_t fifoSrc;
bool fifoFull = false;
do{
// FIFO overrun test
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_SRC_REG_A, &fifoSrc, 1);
fifoFull = fifoFull || (fifoSrc & FIFO_OVERRUN) != 0;
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, tmpBuffer, 6);
// FIFO empty test
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_SRC_REG_A, &fifoSrc, 1);
}while ((fifoSrc & FIFO_EMPTY) == 0);
/* FIFO needs reset after being full */
if (fifoFull)
resetFifo();
else
sleep(CHANGE_DELAY);
}
void Gyroscope::clearFifo()
{
uint8_t tmpbuffer[6] = {0};
uint8_t fifoSrc;
bool fifoFull = false;
do{
// FIFO overrun test
L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1);
fifoFull = fifoFull || (fifoSrc & 0x40) != 0;
L3GD20_Read(tmpbuffer, L3GD20_OUT_X_L_ADDR, 6);
// FIFO empty test
L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1);
}while ((fifoSrc & 0x20) == 0);
/* FIFO needs reset after being full */
if (fifoFull)
resetFifo();
else
sleep(CHANGE_DELAY);
}
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 RTIMUMPU9150::IMUInit()
{
unsigned char result;
m_firstTime = true;
#ifdef MPU9150_CACHE_MODE
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_slaveAddr = m_settings->m_I2CSlaveAddress;
setSampleRate(m_settings->m_MPU9150GyroAccelSampleRate);
setCompassRate(m_settings->m_MPU9150CompassSampleRate);
setLpf(m_settings->m_MPU9150GyroAccelLpf);
setGyroFsr(m_settings->m_MPU9150GyroFsr);
setAccelFsr(m_settings->m_MPU9150AccelFsr);
setCalibrationData();
// enable the I2C bus
if (!m_settings->HALOpen())
return false;
// reset the MPU9150
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x80, "Failed to initiate MPU9150 reset"))
return false;
m_settings->delayMs(100);
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x00, "Failed to stop MPU9150 reset"))
return false;
if (!m_settings->HALRead(m_slaveAddr, MPU9150_WHO_AM_I, 1, &result, "Failed to read MPU9150 id"))
return false;
if (result != MPU9150_ID) {
HAL_ERROR1("Incorrect MPU9150 id %d\n", result);
return false;
}
// now configure the various components
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_LPF_CONFIG, m_lpf, "Failed to set lpf"))
return false;
if (!setSampleRate())
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_GYRO_CONFIG, m_gyroFsr, "Failed to set gyro fsr"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_ACCEL_CONFIG, m_accelFsr, "Failed to set accel fsr"))
return false;
// now configure compass
if (!configureCompass())
return false;
// enable the sensors
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2"))
return false;
// select the data to go into the FIFO and enable
if (!resetFifo())
return false;
gyroBiasInit();
HAL_INFO("MPU9150 init complete\n");
return true;
}
bool RTIMUMPU9250::IMURead()
{
unsigned char fifoCount[2];
unsigned int count;
unsigned char fifoData[12];
unsigned char compassData[8];
if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_FIFO_COUNT_H, 2, fifoCount))
return false;
count = ((unsigned int)fifoCount[0] << 8) + fifoCount[1];
if (count == 1024) {
resetFifo();
m_timestamp += m_sampleInterval * (1024 / MPU9250_FIFO_CHUNK_SIZE + 1); // try to fix timestamp
return false;
}
if (count > MPU9250_FIFO_CHUNK_SIZE * 40) {
// more than 40 samples behind - going too slowly so discard some samples but maintain timestamp correctly
while (count >= MPU9250_FIFO_CHUNK_SIZE * 10) {
if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData))
return false;
count -= MPU9250_FIFO_CHUNK_SIZE;
m_timestamp += m_sampleInterval;
}
}
if (count < MPU9250_FIFO_CHUNK_SIZE)
return false;
if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_FIFO_R_W, MPU9250_FIFO_CHUNK_SIZE, fifoData))
return false;
if (!I2Cdev::readBytes(m_slaveAddr, MPU9250_EXT_SENS_DATA_00, 8, compassData))
return false;
RTMath::convertToVector(fifoData, m_accel, m_accelScale, true);
RTMath::convertToVector(fifoData + 6, m_gyro, m_gyroScale, true);
RTMath::convertToVector(compassData + 1, m_compass, 0.6f, false);
// sort out gyro axes
m_gyro.setY(-m_gyro.y());
m_gyro.setZ(-m_gyro.z());
// sort out accel data;
m_accel.setX(-m_accel.x());
// use the fuse data adjustments for compass
m_compass.setX(m_compass.x() * m_compassAdjust[0]);
m_compass.setY(m_compass.y() * m_compassAdjust[1]);
m_compass.setZ(m_compass.z() * m_compassAdjust[2]);
// sort out compass axes
float temp;
temp = m_compass.x();
m_compass.setX(m_compass.y());
m_compass.setY(-temp);
// now do standard processing
handleGyroBias();
calibrateAverageCompass();
if (m_firstTime)
m_timestamp = millis();
else
m_timestamp += m_sampleInterval;
m_firstTime = false;
return true;
}
int RTIMUMPU9250::IMUInit()
{
unsigned char result;
unsigned char asa[3];
m_firstTime = true;
#ifdef MPU9250_CACHE_MODE
m_cacheIn = m_cacheOut = m_cacheCount = 0;
#endif
// configure IMU
m_slaveAddr = m_settings->m_I2CSlaveAddress;
setSampleRate(m_settings->m_MPU9250GyroAccelSampleRate);
setCompassRate(m_settings->m_MPU9250CompassSampleRate);
setGyroLpf(m_settings->m_MPU9250GyroLpf);
setAccelLpf(m_settings->m_MPU9250AccelLpf);
setGyroFsr(m_settings->m_MPU9250GyroFsr);
setAccelFsr(m_settings->m_MPU9250AccelFsr);
setCalibrationData();
// reset the MPU9250
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_1, 0x80))
return -1;
delay(100);
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_1, 0x00))
return -4;
if (!I2Cdev::readByte(m_slaveAddr, MPU9250_WHO_AM_I, &result))
return -5;
if (result != MPU9250_ID) {
return -6;
}
// now configure the various components
if (!setGyroConfig())
return -7;
if (!setAccelConfig())
return -8;
if (!setSampleRate())
return -9;
// now configure compass
if (!bypassOn())
return -11;
// get fuse ROM data
if (!I2Cdev::writeByte(AK8963_ADDRESS, AK8963_CNTL, 0)) {
bypassOff();
return -12;
}
if (!I2Cdev::writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0f)) {
bypassOff();
return -13;
}
if (!I2Cdev::readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, asa)) {
bypassOff();
return -14;
}
// convert asa to usable scale factor
m_compassAdjust[0] = ((float)asa[0] - 128.0) / 256.0 + 1.0f;
m_compassAdjust[1] = ((float)asa[1] - 128.0) / 256.0 + 1.0f;
m_compassAdjust[2] = ((float)asa[2] - 128.0) / 256.0 + 1.0f;
if (!I2Cdev::writeByte(AK8963_ADDRESS, AK8963_CNTL, 0)) {
bypassOff();
return -15;
}
if (!bypassOff())
return -16;
// now set up MPU9250 to talk to the compass chip
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_MST_CTRL, 0x40))
return -17;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV0_ADDR, 0x80 | AK8963_ADDRESS))
return -18;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV0_REG, AK8963_ST1))
return -19;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV0_CTRL, 0x88))
return -20;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_ADDR, AK8963_ADDRESS))
return -21;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_REG, AK8963_CNTL))
return -22;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_CTRL, 0x81))
return -23;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_SLV1_DO, 0x1))
return -24;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_I2C_MST_DELAY_CTRL, 0x3))
return -25;
if (!setCompassRate())
return -27;
// enable the sensors
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_1, 1))
return -28;
if (!I2Cdev::writeByte(m_slaveAddr, MPU9250_PWR_MGMT_2, 0))
return -29;
// select the data to go into the FIFO and enable
if (!resetFifo())
return -30;
gyroBiasInit();
return 1;
}
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 RTIMUMPU9255::IMUInit()
{
unsigned char result;
m_firstTime = true;
#ifdef MPU9255_CACHE_MODE
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.motion = true;
m_imuData.IMUtemperatureValid = false;
m_imuData.IMUtemperature = 0.0;
m_imuData.humidityValid = false;
m_imuData.humidity = -1.0;
m_imuData.humidityTemperatureValid = false;
m_imuData.humidityTemperature = 0.0;
m_imuData.pressureValid = false;
m_imuData.pressure = 0.0;
m_imuData.pressureTemperatureValid = false;
m_imuData.pressureTemperature = 0.0;
// configure IMU
m_slaveAddr = m_settings->m_I2CSlaveAddress;
setSampleRate(m_settings->m_MPU9255GyroAccelSampleRate);
setCompassRate(m_settings->m_MPU9255CompassSampleRate);
setGyroLpf(m_settings->m_MPU9255GyroLpf);
setAccelLpf(m_settings->m_MPU9255AccelLpf);
setGyroFsr(m_settings->m_MPU9255GyroFsr);
setAccelFsr(m_settings->m_MPU9255AccelFsr);
setCalibrationData();
// enable the bus
if (!m_settings->HALOpen())
return false;
// reset the MPU9255
if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_1, 0x80, "Failed to initiate MPU9255 reset"))
return false;
m_settings->delayMs(100);
if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_1, 0x00, "Failed to stop MPU9255 reset"))
return false;
if (!m_settings->HALRead(m_slaveAddr, MPU9255_WHO_AM_I, 1, &result, "Failed to read MPU9255 id"))
return false;
if (result != MPU9255_ID) {
HAL_ERROR2("Incorrect %s id %d\n", IMUName(), result);
return false;
}
// now configure the various components
if (!setGyroConfig())
return false;
if (!setAccelConfig())
return false;
if (!setSampleRate())
return false;
if(!compassSetup()) {
return false;
}
if (!setCompassRate())
return false;
// enable the sensors
if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9255_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2"))
return false;
// select the data to go into the FIFO and enable
if (!resetFifo())
return false;
gyroBiasInit();
HAL_INFO1("%s init complete\n", IMUName());
return true;
}
void Accelerometer::retrieveValues()
{
const uint16_t shift = 16;
uint8_t tmpBuffer[6] = {0};
math3d::Vector3<int16_t> vec;
uint8_t ctrl4, fifoCtrl, fifoSrc;
int i = 0;
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_CTRL_REG_A, &fifoCtrl, 1);
bool fifoMode = (fifoCtrl & IS_FIFO) != 0;
bool fifoFull = false;
if (fifoMode){
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_SRC_REG_A, &fifoSrc, 1);
/* Test FIFO empty bit */
if ((fifoSrc & FIFO_EMPTY) != 0)
return;
}
if(_scaleBuffer.empty())
return;
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG4_A, &ctrl4, 1);
do{
// FIFO overrun test
if (fifoMode){
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_SRC_REG_A, &fifoSrc, 1);
fifoFull = fifoFull || (fifoSrc & FIFO_OVERRUN) != 0;
}
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, tmpBuffer, 6);
/* Check in the control register 4 the data alignment (Big Endian or Little Endian) */
if(ctrl4 & LSM303DLHC_BLE_MSB){
for(i = 0; i < 3; i++){
vec[i] = (int16_t)(((uint16_t)tmpBuffer[2*i] << 8) + tmpBuffer[2*i+1]);
}
}
else{
for(i = 0; i < 3; i++){
vec[i] = (int16_t)(((uint16_t)tmpBuffer[2*i+1] << 8) + tmpBuffer[2*i]);
}
}
// Check if buffer size isn't larger than maximum
if(_maxBufferSize > 0 && _dataBuffer.size() >= _maxBufferSize)
discard();
/* Place retrieved values in local buffer */
_dataBuffer.push_back(vec / shift);
_scaleBuffer.back().second++;
// FIFO empty test
if (fifoMode){
// Let FIFO update
sleep(10, microsecond);
LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_FIFO_SRC_REG_A, &fifoSrc, 1);
}
}while (fifoMode && (fifoSrc & FIFO_EMPTY) == 0);
/* FIFO needs reset after being full */
if (fifoMode && fifoFull)
resetFifo();
}
// TODO: Bypass to stream support
// Stream to FIFO support
void Gyroscope::retrieveValues()
{
uint8_t tmpbuffer[6] = {0};
math3d::Vector3<int16_t> vec;
uint8_t ctrl4, fifoCtrl, fifoSrc;
int i = 0;
L3GD20_Read(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1);
bool fifoMode = (fifoCtrl & 0x70) != 0;
bool fifoFull = false;
if (fifoMode){
L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1);
/* Test FIFO empty bit */
if ((fifoSrc & 0x20) != 0)
return;
}
if(_scaleBuffer.empty())
return;
L3GD20_Read(&ctrl4, L3GD20_CTRL_REG4_ADDR, 1);
do{
// FIFO overrun test
if (fifoMode){
L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1);
fifoFull = fifoFull || (fifoSrc & 0x40) != 0;
}
L3GD20_Read(tmpbuffer, L3GD20_OUT_X_L_ADDR, 6);
/* Check in the control register 4 the data alignment (Big Endian or Little Endian) */
if(ctrl4 & 0x40){
for(i = 0; i < 3; i++){
vec[i] = (int16_t)(((uint16_t)tmpbuffer[2*i] << 8) + tmpbuffer[2*i+1]);
}
}
else{
for(i = 0; i < 3; i++){
vec[i] = (int16_t)(((uint16_t)tmpbuffer[2*i+1] << 8) + tmpbuffer[2*i]);
}
}
// Check if buffer size isn't larger than maximum
if(_maxBufferSize > 0 && _dataBuffer.size() >= _maxBufferSize)
discard();
/* Place retrieved values in local buffer */
_dataBuffer.push_back(vec);
_scaleBuffer.back().second++;
// FIFO empty test
if (fifoMode){
// Let FIFO update
sleep(10, microsecond);
L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1);
}
}while (fifoMode && (fifoSrc & 0x20) == 0);
/* FIFO needs reset after being full */
if (fifoMode && fifoFull)
resetFifo();
}
bool RTIMUMPU9150::IMUInit()
{
unsigned char result;
unsigned char asa[3];
m_firstTime = true;
#ifdef MPU9150_CACHE_MODE
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_slaveAddr = m_settings->m_I2CSlaveAddress;
setSampleRate(m_settings->m_MPU9150GyroAccelSampleRate);
setCompassRate(m_settings->m_MPU9150CompassSampleRate);
setLpf(m_settings->m_MPU9150GyroAccelLpf);
setGyroFsr(m_settings->m_MPU9150GyroFsr);
setAccelFsr(m_settings->m_MPU9150AccelFsr);
setCalibrationData();
// enable the I2C bus
if (!m_settings->HALOpen())
return false;
// reset the MPU9150
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x80, "Failed to initiate MPU9150 reset"))
return false;
m_settings->delayMs(100);
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 0x00, "Failed to stop MPU9150 reset"))
return false;
if (!m_settings->HALRead(m_slaveAddr, MPU9150_WHO_AM_I, 1, &result, "Failed to read MPU9150 id"))
return false;
if (result != MPU9150_ID) {
HAL_ERROR1("Incorrect MPU9150 id %d\n", result);
return false;
}
// now configure the various components
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_LPF_CONFIG, m_lpf, "Failed to set lpf"))
return false;
if (!setSampleRate())
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_GYRO_CONFIG, m_gyroFsr, "Failed to set gyro fsr"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_ACCEL_CONFIG, m_accelFsr, "Failed to set accel fsr"))
return false;
// now configure compass
bypassOn();
// get fuse ROM data
if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 1")) {
bypassOff();
return false;
}
if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0x0f, "Failed to set compass in fuse ROM mode")) {
bypassOff();
return false;
}
if (!m_settings->HALRead(AK8975_ADDRESS, AK8975_ASAX, 3, asa, "Failed to read compass fuse ROM")) {
bypassOff();
return false;
}
// convert asa to usable scale factor
m_compassAdjust[0] = ((float)asa[0] - 128.0) / 256.0 + 1.0f;
m_compassAdjust[1] = ((float)asa[1] - 128.0) / 256.0 + 1.0f;
m_compassAdjust[2] = ((float)asa[2] - 128.0) / 256.0 + 1.0f;
if (!m_settings->HALWrite(AK8975_ADDRESS, AK8975_CNTL, 0, "Failed to set compass in power down mode 2")) {
bypassOff();
return false;
}
bypassOff();
// now set up MPU9150 to talk to the compass chip
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_CTRL, 0x40, "Failed to set I2C master mode"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_ADDR, 0x80 | AK8975_ADDRESS, "Failed to set slave 0 address"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_REG, AK8975_ST1, "Failed to set slave 0 reg"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV0_CTRL, 0x88, "Failed to set slave 0 ctrl"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_ADDR, AK8975_ADDRESS, "Failed to set slave 1 address"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_REG, AK8975_CNTL, "Failed to set slave 1 reg"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_CTRL, 0x81, "Failed to set slave 1 ctrl"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_SLV1_DO, 0x1, "Failed to set slave 1 DO"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_I2C_MST_DELAY_CTRL, 0x3, "Failed to set mst delay"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_YG_OFFS_TC, 0x80, "Failed to set yg offs tc"))
return false;
if (!setCompassRate())
return false;
// enable the sensors
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_1, 1, "Failed to set pwr_mgmt_1"))
return false;
if (!m_settings->HALWrite(m_slaveAddr, MPU9150_PWR_MGMT_2, 0, "Failed to set pwr_mgmt_2"))
return false;
// select the data to go into the FIFO and enable
if (!resetFifo())
return false;
gyroBiasInit();
HAL_INFO("MPU9150 init complete\n");
return true;
}
