RTIMU::RTIMU(RTIMUSettings *settings)
{
m_settings = settings;
m_compassCalibrationMode = false;
m_accelCalibrationMode = false;
m_runtimeMagCalValid = false;
for (int i = 0; i < 3; i++) {
m_runtimeMagCalMax[i] = -1000;
m_runtimeMagCalMin[i] = 1000;
}
switch (m_settings->m_fusionType) {
case RTFUSION_TYPE_KALMANSTATE4:
m_fusion = new RTFusionKalman4();
break;
case RTFUSION_TYPE_RTQF:
m_fusion = new RTFusionRTQF();
break;
default:
m_fusion = new RTFusion();
break;
}
HAL_INFO1("Using fusion algorithm %s\n", RTFusion::fusionName(m_settings->m_fusionType));
}
RTFusionAHRS::RTFusionAHRS()
{
float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
m_beta = beta;
m_zeta = zeta;
reset();
m_enableAccel = true;
m_enableCompass = true;
m_enableGyro = true;
if (m_debug) {
HAL_INFO("\n------\n");
HAL_INFO1("IMU beta %f\n", m_beta);
HAL_INFO1("IMU zeta %f\n", m_zeta);
}
}
RTIMU::RTIMU(RTIMUSettings *settings)
{
m_settings = settings;
m_compassCalibrationMode = false;
m_accelCalibrationMode = false;
m_runtimeMagCalValid = false;
m_runtimeMagCalMax = -1000.0f;
m_runtimeMagCalMin = 1000.0f;
//m_runtimeMagCalMax.setX(-1000);
//m_runtimeMagCalMax.setY(-1000);
//m_runtimeMagCalMax.setZ(-1000);
//m_runtimeMagCalMin.setX( 1000);
//m_runtimeMagCalMin.setY( 1000);
//m_runtimeMagCalMin.setZ( 1000);
m_gyroCalibrationMode = false;
m_temperatureCalibrationMode = false;
m_gyroRunTimeCalibrationEnable = true;
m_gyroManualCalibrationEnable = false;
m_accelRunTimeCalibrationEnable = false;
m_compassRunTimeCalibrationEnable = false;
switch (m_settings->m_fusionType) {
case RTFUSION_TYPE_KALMANSTATE4:
m_fusion = new RTFusionKalman4();
break;
case RTFUSION_TYPE_RTQF:
m_fusion = new RTFusionRTQF();
break;
case RTFUSION_TYPE_AHRS:
m_fusion = new RTFusionAHRS();
break;
default:
m_fusion = new RTFusion();
break;
}
HAL_INFO1("Using fusion algorithm %s\n", RTFusion::fusionName(m_settings->m_fusionType));
m_compassAverageX = new RunningAverage(20); // 0.1f * m_sampleRate;
m_compassAverageY = new RunningAverage(20);
m_compassAverageZ = new RunningAverage(20);
}
RTIMU::RTIMU(RTIMUSettings *settings)
{
m_settings = settings;
m_compassCalibrationMode = false;
m_accelCalibrationMode = false;
switch (m_settings->m_fusionType) {
case RTFUSION_TYPE_KALMANSTATE4:
m_fusion = new RTFusionKalman4();
break;
case RTFUSION_TYPE_RTQF:
m_fusion = new RTFusionRTQF();
break;
default:
m_fusion = new RTFusion();
break;
}
HAL_INFO1("Using fusion algorithm %s\n", RTFusion::fusionName(m_settings->m_fusionType));
}
void RTFusionKalman4::newIMUData(RTIMU_DATA& data, const RTIMUSettings *settings)
{
if (m_enableGyro)
m_gyro = data.gyro;
else
m_gyro = RTVector3();
m_accel = data.accel;
m_compass = data.compass;
m_compassValid = data.compassValid;
if (m_firstTime) {
m_lastFusionTime = data.timestamp;
calculatePose(m_accel, m_compass, settings->m_compassAdjDeclination);
m_Fk.fill(0);
// init covariance matrix to something
m_Pkk.fill(0);
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
m_Pkk.setVal(i,j, 0.5);
// initialize the observation model Hk
// Note: since the model is the state vector, this is an identity matrix so it won't be used
// initialize the poses
m_stateQ.fromEuler(m_measuredPose);
m_fusionQPose = m_stateQ;
m_fusionPose = m_measuredPose;
m_firstTime = false;
} else {
m_timeDelta = (RTFLOAT)(data.timestamp - m_lastFusionTime) / (RTFLOAT)1000000;
m_lastFusionTime = data.timestamp;
if (m_timeDelta <= 0)
return;
if (m_debug) {
HAL_INFO("\n------\n");
HAL_INFO1("IMU update delta time: %f\n", m_timeDelta);
}
calculatePose(data.accel, data.compass, settings->m_compassAdjDeclination);
predict();
update();
m_stateQ.toEuler(m_fusionPose);
m_fusionQPose = m_stateQ;
if (m_debug | settings->m_fusionDebug) {
HAL_INFO(RTMath::displayRadians("Measured pose", m_measuredPose));
HAL_INFO(RTMath::displayRadians("Kalman pose", m_fusionPose));
HAL_INFO(RTMath::displayRadians("Measured quat", m_measuredPose));
HAL_INFO(RTMath::display("Kalman quat", m_stateQ));
HAL_INFO(RTMath::display("Error quat", m_stateQError));
}
}
data.fusionPoseValid = true;
data.fusionQPoseValid = true;
data.fusionPose = m_fusionPose;
data.fusionQPose = m_fusionQPose;
}
bool RTIMUSettings::loadSettings()
{
char buf[200];
char key[200];
char val[200];
RTFLOAT ftemp;
// preset general defaults
m_imuType = RTIMU_TYPE_MPU6050;
m_I2CSlaveAddress = 0;
m_I2CBus = 1;
m_fusionType = RTFUSION_TYPE_RTQF;
m_compassCalValid = false;
// MPU6050 defaults
m_MPU6050GyroAccelSampleRate = 50;
m_MPU6050CompassSampleRate = 25;
m_MPU6050GyroAccelLpf = MPU6050_LPF_20;
m_MPU6050GyroFsr = MPU6050_GYROFSR_1000;
m_MPU6050AccelFsr = MPU6050_ACCELFSR_8;
// check to see if settings file exists
if (!(m_fd = fopen(m_filename, "r"))) {
HAL_INFO("Settings file not found. Using defaults and creating settings file\n");
return saveSettings();
}
while (fgets(buf, 200, m_fd)) {
if ((buf[0] == '#') || (buf[0] == ' ') || (buf[0] == '\n'))
// just a comment
continue;
if (sscanf(buf, "%[^=]=%s", key, val) != 2) {
HAL_ERROR1("Bad line in settings file: %s\n", buf);
fclose(m_fd);
return false;
}
// now decode keys
// general config
if (strcmp(key, RTIMULIB_IMU_TYPE) == 0) {
m_imuType = atoi(val);
} else if (strcmp(key, RTIMULIB_FUSION_TYPE) == 0) {
m_fusionType = atoi(val);
} else if (strcmp(key, RTIMULIB_I2C_BUS) == 0) {
m_I2CBus = atoi(val);
} else if (strcmp(key, RTIMULIB_I2C_SLAVEADDRESS) == 0) {
m_I2CSlaveAddress = atoi(val);
// compass calibration
} else if (strcmp(key, RTIMULIB_COMPASSCAL_VALID) == 0) {
m_compassCalValid = strcmp(val, "true") == 0;
} else if (strcmp(key, RTIMULIB_COMPASSCAL_MINX) == 0) {
sscanf(val, "%f", &ftemp);
m_compassCalMin.setX(ftemp);
} else if (strcmp(key, RTIMULIB_COMPASSCAL_MINY) == 0) {
sscanf(val, "%f", &ftemp);
m_compassCalMin.setY(ftemp);
} else if (strcmp(key, RTIMULIB_COMPASSCAL_MINZ) == 0) {
sscanf(val, "%f", &ftemp);
m_compassCalMin.setZ(ftemp);
} else if (strcmp(key, RTIMULIB_COMPASSCAL_MAXX) == 0) {
sscanf(val, "%f", &ftemp);
m_compassCalMax.setX(ftemp);
} else if (strcmp(key, RTIMULIB_COMPASSCAL_MAXY) == 0) {
sscanf(val, "%f", &ftemp);
m_compassCalMax.setY(ftemp);
} else if (strcmp(key, RTIMULIB_COMPASSCAL_MAXZ) == 0) {
sscanf(val, "%f", &ftemp);
m_compassCalMax.setZ(ftemp);
// MPU6050 settings
} else if (strcmp(key, RTIMULIB_MPU6050_GYROACCEL_SAMPLERATE) == 0) {
m_MPU6050GyroAccelSampleRate = atoi(val);
} else if (strcmp(key, RTIMULIB_MPU6050_COMPASS_SAMPLERATE) == 0) {
m_MPU6050CompassSampleRate = atoi(val);
} else if (strcmp(key, RTIMULIB_MPU6050_GYROACCEL_LPF) == 0) {
m_MPU6050GyroAccelLpf = atoi(val);
} else if (strcmp(key, RTIMULIB_MPU6050_GYRO_FSR) == 0) {
m_MPU6050GyroFsr = atoi(val);
} else if (strcmp(key, RTIMULIB_MPU6050_ACCEL_FSR) == 0) {
m_MPU6050AccelFsr = atoi(val);
// Handle unrecognized key
} else {
HAL_ERROR1("Unrecognized key in settings file: %s\n", buf);
}
}
HAL_INFO1("Settings file %s loaded\n", m_filename);
fclose(m_fd);
return saveSettings(); // make sure settings file is correct and complete
}
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 RTFusionAHRS::newIMUData(RTIMU_DATA& data, const RTIMUSettings *settings)
{
if (m_debug) {
HAL_INFO("\n------\n");
HAL_INFO1("IMU update delta time: %f\n", m_timeDelta);
}
m_gyro = data.gyro;
m_accel = data.accel;
m_compass = data.compass;
m_compassValid = data.compassValid;
if (m_firstTime) {
// adjust for compass declination, compute the correction data only at beginning
m_sin_theta_half = sin(-settings->m_compassAdjDeclination/2.0f);
m_cos_theta_half = cos(-settings->m_compassAdjDeclination/2.0f);
m_lastFusionTime = data.timestamp;
calculatePose(m_accel, m_compass, settings->m_compassAdjDeclination);
// initialize the poses
m_stateQ.fromEuler(m_measuredPose);
m_fusionQPose = m_stateQ;
m_fusionPose = m_measuredPose;
m_firstTime = false;
} else { // not first time
m_timeDelta = (RTFLOAT)(data.timestamp - m_lastFusionTime) / (RTFLOAT)1000000;
m_lastFusionTime = data.timestamp;
if (m_timeDelta <= 0)
return;
calculatePose(data.accel, data.compass, settings->m_compassAdjDeclination);
// =================================================
// AHRS
//
// Previous Q Pose; short name local variables for readability
float q1 = m_stateQ.scalar();
float q2 = m_stateQ.x();
float q3 = m_stateQ.y();
float q4 = m_stateQ.z();
float ax, ay, az, mx, my, mz, gx, gy, gz; // accelerometer, magnetometer, gyroscope
float gerrx, gerry, gerrz; // gyro bias error
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
float _2q1mx,_2q1my, _2q1mz,_2q2mx;
float _4bx, _4bz;
float _2q1, _2q2, _2q3, _2q4;
float q1q1, q1q2, q1q3, q1q4, q2q2, q2q3, q2q4, q3q4, q3q3, q4q4;
float _4q1, _4q2, _4q3, _8q2, _8q3;
float _2q3q4, _2q1q3;
if (m_enableCompass) {
mx = m_compass.x();
my = m_compass.y();
mz = m_compass.z();
} else {
mx = 0.0f;
my = 0.0f;
mz = 0.0f;
}
if (m_enableAccel) {
ax = m_accel.x();
ay = m_accel.y();
az = m_accel.z();
} else {
ax = 0.0f;
ay = 0.0f;
az = 0.0f; }
if (m_enableGyro) {
gx=m_gyro.x();
gy=m_gyro.y();
gz=m_gyro.z();
} else { return; } // We need to have valid gyroscope data
////////////////////////////////////////////////////////////////////////////
// Regular Algorithm
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz); // s
qDot2 = 0.5f * ( q1 * gx + q3 * gz - q4 * gy); // x
qDot3 = 0.5f * ( q1 * gy - q2 * gz + q4 * gx); // y
qDot4 = 0.5f * ( q1 * gz + q2 * gy - q3 * gx); // z
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Use this algorithm if accelerometer is valid
// If accelerometer is not valid, update pose based on previous qDot
// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0) return; // handle NaN
ax /= norm;
ay /= norm;
az /= norm;
// Auxiliary variables to avoid repeated arithmetic
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q4 = 2.0f * q4;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
q4q4 = q4 * q4;
if(((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
// If magnetometer is invalid
// Auxiliary variables to avoid repeated arithmetic
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_4q3 = 4.0f * q3;
_8q2 = 8.0f * q2;
_8q3 = 8.0f * q3;
// Gradient decent algorithm corrective step
s1 = _4q1 * q3q3 + _2q3 * ax + _4q1 * q2q2 - _2q2 * ay;
s2 = _4q2 * q4q4 - _2q4 * ax + 4.0f * q1q1 * q2 - _2q1 * ay - _4q2 + _8q2 * q2q2 + _8q2 * q3q3 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 + _2q1 * ax + _4q3 * q4q4 - _2q4 * ay - _4q3 + _8q3 * q2q2 + _8q3 * q3q3 + _4q3 * az;
s4 = 4.0f * q2q2 * q4 - _2q2 * ax + 4.0f * q3q3 * q4 - _2q3 * ay;
} else {
// Valid magnetometer available use this code
// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0) return; // handle NaN
mx /= norm;
my /= norm;
mz /= norm;
// Auxiliary variables to avoid repeated arithmetic
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q1q4 = q1 * q4;
q2q3 = q2 * q3;
q2q4 = q2 * q4;
q3q4 = q3 * q4;
_2q1q3 = 2.0f * q1q3;
_2q3q4 = 2.0f * q3q4;
_2q1mx = 2.0f * q1 * mx;
_2q1my = 2.0f * q1 * my;
_2q1mz = 2.0f * q1 * mz;
_2q2mx = 2.0f * q2 * mx;
// Reference direction of Earth's magnetic field
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
} // end valid magnetometer
norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
if (norm == 0.0) return; // handle NaN
s1 /= norm;
s2 /= norm;
s3 /= norm;
s4 /= norm;
// Compute estimated gyroscope biases
gerrx = _2q1 * s2 - _2q2 * s1 - _2q3 * s4 + _2q4 * s3;
gerry = _2q1 * s3 + _2q2 * s4 - _2q3 * s1 - _2q4 * s2;
gerrz = _2q1 * s4 - _2q2 * s3 + _2q3 * s2 - _2q4 * s1;
// Compute and remove gyroscope biases
m_gbiasx += gerrx * m_timeDelta * m_zeta;
m_gbiasy += gerry * m_timeDelta * m_zeta;
m_gbiasz += gerrz * m_timeDelta * m_zeta;
gx -= m_gbiasx;
gy -= m_gbiasy;
gz -= m_gbiasz;
// Apply feedback step
qDot1 -= m_beta * s1;
qDot2 -= m_beta * s2;
qDot3 -= m_beta * s3;
qDot4 -= m_beta * s4;
} // end if valid accelerometer
// Integrate to yield quaternion
q1 += qDot1 * m_timeDelta;
q2 += qDot2 * m_timeDelta;
q3 += qDot3 * m_timeDelta;
q4 += qDot4 * m_timeDelta;
// normalise quaternion
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
if (norm == 0.0) return; // handle NaN
q1 /= norm;
q2 /= norm;
q3 /= norm;
q4 /= norm;
m_stateQ.setScalar(q1);
m_stateQ.setX(q2);
m_stateQ.setY(q3);
m_stateQ.setZ(q4);
// Rotate Quaternion by Magnetic Declination
// m_stateQ = q_declination * m_statqQ;
/*
SAGE
N.<c,d,q1,q2,q3,q4,cos_theta_half, sin_theta_half> = QQ[]
H.<i,j,k> = QuaternionAlgebra(c,d)
q = q1 + q2 * i + q3 * j + q4 * k
// here rotation is around gravity vector by theta
mag_declination = cos_theta_half + sin_theta_half * (0 * i + 0 * j+ 1*k)
q * mag_declination
s : -q4*sin_theta_half + q1*cos_theta_half
x : q3*sin_theta_half + q2*cos_theta_half
y : -q2*sin_theta_half + q3*cos_theta_half
z : q4*cos_theta_half + q1*sin_theta_half
*/
m_stateQdec.setScalar(q1*m_cos_theta_half - q4*m_sin_theta_half);
m_stateQdec.setX(q3*m_sin_theta_half + q2*m_cos_theta_half);
m_stateQdec.setY(q3*m_cos_theta_half - q2*m_sin_theta_half);
m_stateQdec.setZ(q4*m_cos_theta_half + q1*m_sin_theta_half);
} // end not first time
// =================================================
if (m_enableCompass || m_enableAccel) {
m_stateQError = m_measuredQPose - m_stateQ;
} else {
m_stateQError = RTQuaternion();
}
m_stateQdec.toEuler(m_fusionPose);
m_fusionQPose = m_stateQdec;
if (m_debug | settings->m_fusionDebug) {
HAL_INFO(RTMath::displayRadians("Measured pose", m_measuredPose));
HAL_INFO(RTMath::displayRadians("AHRS pose", m_fusionPose));
HAL_INFO(RTMath::displayRadians("Measured quat", m_measuredPose));
HAL_INFO(RTMath::display("AHRS quat", m_stateQ));
HAL_INFO(RTMath::display("Error quat", m_stateQError));
HAL_INFO3("AHRS Gyro Bias: %+f, %+f, %+f\n", m_gbiasx, m_gbiasy, m_gbiasz);
}
data.fusionPoseValid = true;
data.fusionQPoseValid = true;
data.fusionPose = m_fusionPose;
data.fusionQPose = m_fusionQPose;
} //