From 38d61f52815251af023d95fd4da3d7d07d730b8c Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 21 May 2016 11:54:26 +1000 Subject: [PATCH] AP_NavEKF2: Update status reporting to handle no-magnetometer startup --- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 67c3d9bfd9..4706784487 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -459,8 +459,8 @@ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout; bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav; bool optFlowNavPossible = flowDataValid && (frontend->_fusionModeGPS == 3); - bool gpsNavPossible = !gpsNotAvailable && (PV_AidingMode == AID_ABSOLUTE) && gpsGoodToAlign; - bool filterHealthy = healthy() && tiltAlignComplete && yawAlignComplete; + bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign; + bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE))); // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;