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

AP_NavEKF2: Fix bugs preventing use of optical flow navigation

Browse Source
This commit is contained in:
Paul Riseborough 2015-10-14 09:19:18 +11:00 committed by Andrew Tridgell
parent 24c0309e85
commit d3a6690e4f
1 changed files with 4 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp
View File

@ -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) // 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; nav_filter_status status;
getFilterStatus(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 // This is the normal mode of operation where we can use the EKF position states
pos.x = outputDataNew.position.x; pos.x = outputDataNew.position.x;
pos.y = outputDataNew.position.y; 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 doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid;
bool doingWindRelNav = !tasTimeout && assume_zero_sideslip(); bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE); bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
bool notDeadReckoning = (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); bool optFlowNavPossible = flowDataValid && (frontend._fusionModeGPS == 3);
@ -407,10 +406,10 @@ void NavEKF2_core::getFilterStatus(nav_filter_status &status) const
// set individual flags // set individual flags
status.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check) 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.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_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid
status.flags.horiz_pos_abs = doingNormalGpsNav && notDeadReckoning && filterHealthy; // absolute 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.vert_pos = !hgtTimeout && filterHealthy; // vertical position estimate valid
status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height 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 status.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode