dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Fallback to optical flow for all in-flight loss of navigation scenarios

This commit is contained in:
Paul Riseborough 2018-05-28 18:20:15 +10:00 committed by Lorenz Meier
parent b4d2b8c57d
commit 6bdbe032f9
2 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
EKF/control.cpp
View File

@ -337,7 +337,7 @@ void Ekf::controlExternalVisionFusion()
void Ekf::controlOpticalFlowFusion()
{
// Check if motion is un-suitable for use of optical flow
// Check if on ground motion is un-suitable for use of optical flow
if (!_control_status.flags.in_air) {
// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation
float accel_norm = _accel_vec_filt.norm();
@ -365,19 +365,21 @@ void Ekf::controlOpticalFlowFusion()
} else {
gps_err_norm_lim = 1.0f;
}
bool flow_required = !_control_status.flags.gps || (_gps_error_norm > gps_err_norm_lim);
// If we are in-air and doing inertial dead reckoning or are using bad GPS, then optical flow use
// is necessary to control position drift
bool flow_required = _control_status.flags.in_air &&
(_is_dead_reckoning || (_control_status.flags.gps && (_gps_error_norm > gps_err_norm_lim)));
if (!_inhibit_flow_use) {
// check if we should inhibit use of optical flow
bool movement_not_ok = ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5)
&& !_range_aid_mode_enabled;
bool movement_not_ok = ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5) // on-ground check
&& !_range_aid_mode_enabled; // in-air check
if (movement_not_ok || !flow_required) {
_inhibit_flow_use = true;
}
} else {
bool movement_ok = ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6)
|| _range_aid_mode_enabled;
if (movement_ok && flow_required) {
bool movement_ok = ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6) || _range_aid_mode_enabled;
if (movement_ok || flow_required) {
_inhibit_flow_use = false;
}
}

2
EKF/ekf.h
View File

@ -436,7 +436,7 @@ private:
bool _bad_vert_accel_detected{false}; ///< true when bad vertical accelerometer data has been detected
// variables used to control range aid functionality
bool _range_aid_mode_enabled{false}; ///< true when range finder can be used as the height reference instead of the primary height sensor
bool _range_aid_mode_enabled{false}; ///< true when range finder can be used in flight as the height reference instead of the primary height sensor
bool _range_aid_mode_selected{false}; ///< true when range finder is being used as the height reference instead of the primary height sensor
// variables used to check for "stuck" rng data