Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: move badMagYaw onto stack

Browse Source
This commit is contained in:
Peter Barker 2020-11-22 15:05:41 +11:00 committed by Peter Barker
parent 2885026bba
commit addd57dcab
3 changed files with 1 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -194,7 +194,7 @@ void NavEKF2_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) {

1
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -348,7 +348,6 @@ void NavEKF2_core::InitialiseVariablesMag()
magYawResetTimer_ms = imuSampleTime_ms;
magTimeout = false;
allMagSensorsFailed = false;
badMagYaw = false;
finalInflightYawInit = false;
finalInflightMagInit = false;

1
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -815,7 +815,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