From d3a6690e4f52d97d70bc08b99c7e94e33f665392 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 14 Oct 2015 09:19:18 +1100 Subject: [PATCH] AP_NavEKF2: Fix bugs preventing use of optical flow navigation --- libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 993003a848..7e476d8b70 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -180,7 +180,7 @@ bool NavEKF2_core::getPosNED(Vector3f &pos) const // There are three modes of operation, absolute position (GPS fusion), relative position (optical flow fusion) and constant position (no position estimate available) nav_filter_status status; getFilterStatus(status); - if (status.flags.horiz_pos_abs || status.flags.horiz_pos_rel) { + if (PV_AidingMode != AID_NONE) { // This is the normal mode of operation where we can use the EKF position states pos.x = outputDataNew.position.x; pos.y = outputDataNew.position.y; @@ -398,7 +398,6 @@ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid; bool doingWindRelNav = !tasTimeout && assume_zero_sideslip(); bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE); - bool notDeadReckoning = (PV_AidingMode == AID_ABSOLUTE); bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout; bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav; bool optFlowNavPossible = flowDataValid && (frontend._fusionModeGPS == 3); @@ -407,10 +406,10 @@ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const // set individual flags status.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check) - status.flags.horiz_vel = someHorizRefData && notDeadReckoning && filterHealthy; // horizontal velocity estimate valid + status.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid status.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid - status.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && notDeadReckoning && filterHealthy; // relative horizontal position estimate valid - status.flags.horiz_pos_abs = doingNormalGpsNav && notDeadReckoning && filterHealthy; // absolute horizontal position estimate valid + status.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid + status.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid status.flags.vert_pos = !hgtTimeout && filterHealthy; // vertical position estimate valid status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid status.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode