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

EKF: rework optical flow switching

Browse Source
This commit is contained in:
Paul Riseborough
2018-05-18 15:03:43 +10:00
committed by Lorenz Meier
parent 1cfe845751
commit 39697f1196
1 changed files with 15 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+13 -10
View File
@@ -353,6 +353,7 @@ void Ekf::controlOpticalFlowFusion()
} }
} else { } else {
_time_bad_motion_us = 0;
_time_good_motion_us = _imu_sample_delayed.time_us; _time_good_motion_us = _imu_sample_delayed.time_us;
} }
@@ -364,17 +365,19 @@ void Ekf::controlOpticalFlowFusion()
} else { } else {
gps_err_norm_lim = 1.0f; gps_err_norm_lim = 1.0f;
} }
bool flow_required = !_control_status.flags.gps || (_gps_error_norm > gps_err_norm_lim);
if (!_inhibit_flow_use) { if (!_inhibit_flow_use) {
bool movement_not_ok = (_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5; // check if we should inhibit use of optical flow
bool good_gps_aiding = _control_status.flags.gps && _gps_error_norm < gps_err_norm_lim; bool movement_not_ok = ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5)
if (movement_not_ok || good_gps_aiding || !_range_aid_mode_enabled) { && !_range_aid_mode_enabled;
if (movement_not_ok || !flow_required) {
_inhibit_flow_use = true; _inhibit_flow_use = true;
} }
} else { } else {
bool movement_ok = (_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6; bool movement_ok = ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6)
bool bad_gps_aiding = !_control_status.flags.gps || _gps_error_norm > gps_err_norm_lim; || _range_aid_mode_enabled;
if (movement_ok || bad_gps_aiding) { if (movement_ok && flow_required) {
_inhibit_flow_use = false; _inhibit_flow_use = false;
} }
} }
@@ -403,19 +406,19 @@ void Ekf::controlOpticalFlowFusion()
if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user
&& !_control_status.flags.opt_flow // we are not yet using flow data && !_control_status.flags.opt_flow // we are not yet using flow data
&& _control_status.flags.tilt_align // we know our tilt attitude && _control_status.flags.tilt_align // we know our tilt attitude
&& get_terrain_valid()) { // we have a valid distance to ground estimate && !_inhibit_flow_use
&& get_terrain_valid()) // we have a valid distance to ground estimate
{
// If the heading is not aligned, reset the yaw and magnetic field states // If the heading is not aligned, reset the yaw and magnetic field states
if (!_control_status.flags.yaw_align) { if (!_control_status.flags.yaw_align) {
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
} }
// If the heading is valid and use is not inhibited , start using optical flow aiding // If the heading is valid and use is not inhibited , start using optical flow aiding
if (_control_status.flags.yaw_align && !_inhibit_flow_use) { if (_control_status.flags.yaw_align) {
// set the flag and reset the fusion timeout // set the flag and reset the fusion timeout
_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;
ECL_INFO("EKF Starting Optical Flow Use");
// if we are not using GPS then the velocity and position states and covariances need to be set // if we are not using GPS then the velocity and position states and covariances need to be set
if (!_control_status.flags.gps || !_control_status.flags.ev_pos) { if (!_control_status.flags.gps || !_control_status.flags.ev_pos) {