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

AP_NavEKF3: move badMagYaw onto stack

Browse Source
This commit is contained in:
Peter Barker 2020-11-22 15:05:42 +11:00 committed by Peter Barker
parent addd57dcab
commit 0529ddcd67
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_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -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) {

1
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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();

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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