fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 07:07:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Flow aiding - Reset state when flow is enabled only if it is the only

Browse Source 
position/velocity aiding sensor.
Until now, it was alway resetting if the vehicle does not have gps or
external vision. This caused a reset/glitch at every stop (when range data gets
valid)
This commit is contained in:
bresch
2019-09-02 14:01:31 +02:00
committed by Roman Bapst
parent be368f3656
commit 3b32ee4166
1 changed files with 3 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+3 -4
View File
@@ -465,20 +465,19 @@ void Ekf::controlOpticalFlowFusion()
_control_status.flags.opt_flow = true; _control_status.flags.opt_flow = true;
_time_last_of_fuse = _time_last_imu; _time_last_of_fuse = _time_last_imu;
// if we are not using GPS then the velocity and position states and covariances need to be set // if we are not using GPS or external vision aiding, then the velocity and position states and covariances need to be set
if (!_control_status.flags.gps || !_control_status.flags.ev_pos) { const bool flow_aid_only = !(_control_status.flags.gps || _control_status.flags.ev_pos);
if (flow_aid_only) {
resetVelocity(); resetVelocity();
resetPosition(); resetPosition();
// align the output observer to the EKF states // align the output observer to the EKF states
alignOutputFilter(); alignOutputFilter();
} }
} }
} else if (!(_params.fusion_mode & MASK_USE_OF)) { } else if (!(_params.fusion_mode & MASK_USE_OF)) {
_control_status.flags.opt_flow = false; _control_status.flags.opt_flow = false;
} }
// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period