From 0529ddcd67bee45c8afabc35fc86bad6cd58d23a Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 22 Nov 2020 15:05:42 +1100 Subject: [PATCH] AP_NavEKF3: move badMagYaw onto stack --- libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 1 - libraries/AP_NavEKF3/AP_NavEKF3_core.h | 1 - 3 files changed, 1 insertion(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp index f9009b7722..1a2e4ece61 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp @@ -145,7 +145,7 @@ void NavEKF3_core::realignYawGPS() float yawErr = MAX(fabsf(wrap_PI(gpsYaw - velYaw)),fabsf(wrap_PI(gpsYaw - eulerAngles.z))); // If the angles disagree by more than 45 degrees and GPS innovations are large or no previous yaw alignment, we declare the magnetic yaw as bad - badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete; + bool badMagYaw = ((yawErr > 0.7854f) && (velTestRatio > 1.0f) && (PV_AidingMode == AID_ABSOLUTE)) || !yawAlignComplete; // correct yaw angle using GPS ground course if compass yaw bad if (badMagYaw) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index a90b9d7fd0..52da27ab82 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -429,7 +429,6 @@ void NavEKF3_core::InitialiseVariablesMag() magYawResetTimer_ms = imuSampleTime_ms; magTimeout = false; allMagSensorsFailed = false; - badMagYaw = false; finalInflightMagInit = false; mag_state.q0 = 1; mag_state.DCM.identity(); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 33c531d325..f916adb741 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -943,7 +943,6 @@ private: bool hgtTimeout; // boolean true if height measurements have failed innovation consistency check and timed out bool magTimeout; // boolean true if magnetometer measurements have failed for too long and have timed out bool tasTimeout; // boolean true if true airspeed measurements have failed for too long and have timed out - bool badMagYaw; // boolean true if the magnetometer is declared to be producing bad data bool badIMUdata; // boolean true if the bad IMU data is detected float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts