Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

[IMU] Decouple Mag/CoG; Prefer MAG if available, if not - use CoG

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2019-02-18 20:57:05 +01:00
parent 7685d04ca2
commit a0e6c7e847
1 changed files with 8 additions and 15 deletions
Download patch file Download diff file Expand all files Collapse all files

23
src/main/flight/imu.c
View file

@ -493,13 +493,15 @@ static void imuCalculateEstimatedAttitude(float dT)
if (STATE(FIXED_WING)) { if (STATE(FIXED_WING)) {
bool canUseCOG = isGPSHeadingValid(); bool canUseCOG = isGPSHeadingValid();
if (canUseCOG) { // Prefer compass (if available)
if (canUseMAG) {
useMag = true;
gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if compass fails
}
else if (canUseCOG) {
if (gpsHeadingInitialized) { if (gpsHeadingInitialized) {
// Use GPS heading if error is acceptable or if it's the only source of heading courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
if (ABS(gpsSol.groundCourse - attitude.values.yaw) < DEGREES_TO_DECIDEGREES(MAX_GPS_HEADING_ERROR_DEG) || !canUseMAG) { useCOG = true;
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true;
}
} }
else { else {
// Re-initialize quaternion from known Roll, Pitch and GPS heading // Re-initialize quaternion from known Roll, Pitch and GPS heading
@ -509,15 +511,6 @@ static void imuCalculateEstimatedAttitude(float dT)
// Force reset of heading hold target // Force reset of heading hold target
resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
} }
// If we can't use COG and there's MAG available - fallback
if (!useCOG && canUseMAG) {
useMag = true;
}
}
else if (canUseMAG) {
useMag = true;
gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if possible
} }
} }
else { else {