Browse Source

AP_NavEKF2: fix reporting of optical flow use status

master
priseborough 8 years ago committed by Randy Mackay
parent
commit
b0072b587c
  1. 2
      libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

2
libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp

@ -407,7 +407,7 @@ void NavEKF2_core::updateFilterStatus(void)
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE); bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout; bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav; bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
bool optFlowNavPossible = flowDataValid && (frontend->_fusionModeGPS == 3) && delAngBiasLearned; bool optFlowNavPossible = flowDataValid && delAngBiasLearned;
bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned; bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE))); 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 // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks

Loading…
Cancel
Save