static void imuCalculateEstimatedAttitude(float dT)
{
float courseOverGround = 0;
int accWeight = 0;
bool useMag = false;
bool useCOG = false;
accWeight = imuCalculateAccelerometerConfidence();
if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) {
useMag = true;
}
#if defined(GPS)
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
// In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
if (gpsHeadingInitialized) {
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true;
}
else {
// Re-initialize quaternion from known Roll, Pitch and GPS heading
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
gpsHeadingInitialized = true;
}
}
#endif
imuMahonyAHRSupdate(dT, imuMeasuredRotationBF.A[X], imuMeasuredRotationBF.A[Y], imuMeasuredRotationBF.A[Z],
accWeight, imuMeasuredGravityBF.A[X], imuMeasuredGravityBF.A[Y], imuMeasuredGravityBF.A[Z],
useMag, magADC[X], magADC[Y], magADC[Z],
useCOG, courseOverGround);
imuUpdateEulerAngles();
}
static void imuCalculateEstimatedAttitude(void)
{
static biquad_t accLPFState[3];
static bool accStateIsSet;
static uint32_t previousIMUUpdateTime;
float rawYawError = 0;
int32_t axis;
bool useAcc = false;
bool useMag = false;
bool useYaw = false;
uint32_t currentTime = micros();
uint32_t deltaT = currentTime - previousIMUUpdateTime;
previousIMUUpdateTime = currentTime;
// Smooth and use only valid accelerometer readings
for (axis = 0; axis < 3; axis++) {
if (imuRuntimeConfig->acc_cut_hz) {
if (accStateIsSet) {
accSmooth[axis] = lrintf(applyBiQuadFilter((float) accADC[axis], &accLPFState[axis]));
} else {
for (axis = 0; axis < 3; axis++) BiQuadNewLpf(imuRuntimeConfig->acc_cut_hz, &accLPFState[axis], 1000);
accStateIsSet = true;
}
} else {
accSmooth[axis] = accADC[axis];
}
}
if (imuIsAccelerometerHealthy()) {
useAcc = true;
}
if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) {
useMag = true;
}
#if defined(GPS)
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5 && GPS_speed >= 300) {
// In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - GPS_ground_course);
useYaw = true;
}
#endif
imuMahonyAHRSupdate(deltaT * 1e-6f,
gyroADC[X] * gyroScale, gyroADC[Y] * gyroScale, gyroADC[Z] * gyroScale,
useAcc, accSmooth[X], accSmooth[Y], accSmooth[Z],
useMag, magADC[X], magADC[Y], magADC[Z],
useYaw, rawYawError);
imuUpdateEulerAngles();
imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
}
