mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF2: Fix bugs preventing use of optical flow navigation
This commit is contained in:
parent
24c0309e85
commit
d3a6690e4f
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue