From e70206f74bc4f77a0e000da3c923f4459006d2b1 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 12 Dec 2017 08:39:26 +1100 Subject: [PATCH] EKF: Release flow speed limit with altitude gained When GPS use is gained whilst flying using optical flow data, the sudden release of the speed limit is unannounced to the operator and can cause unexpected acceleration. This patch releases the speed limit as height is gained, but does not reduce it when the vehicle descends, unless GPS use is lost. --- EKF/ekf.h | 1 + EKF/ekf_helper.cpp | 17 +++++++++++------ 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 1f2f5db835..f086e06f1e 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -348,6 +348,7 @@ private: Vector3f _flow_gyro_bias; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) Vector3f _imu_del_ang_of; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad) float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec) + float _flow_gnd_spd_max{0.0f}; ///< maximum ground speed that the flow sensor can reliably measure (m/s) float _mag_declination{0.0f}; ///< magnetic declination used by reset and fusion functions (rad) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 33f4c274a6..07c4d07f7c 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1037,25 +1037,30 @@ limit_hagl : Boolean true when height above ground needs to be controlled to rem */ void Ekf::get_ekf_ctrl_limits(float *vxy_max, bool *limit_hagl) { - float flow_gnd_spd_max; bool flow_limit_hagl; // If relying on optical flow for navigation we need to keep within flow and range sensor limits bool relying_on_optical_flow = _control_status.flags.opt_flow && !(_control_status.flags.gps || _control_status.flags.ev_pos); if (relying_on_optical_flow) { // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion - flow_gnd_spd_max = 0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2)); - flow_gnd_spd_max = fmaxf(flow_gnd_spd_max , 0.0f); - + _flow_gnd_spd_max = 0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2)); + _flow_gnd_spd_max = fmaxf(_flow_gnd_spd_max , 0.0f); flow_limit_hagl = true; + } else if (_control_status.flags.opt_flow) { + // We are using optical flow but not reliant on it + // Release the speed limit as the vehicle climbs, but do not reduce it when it descends + float lower_limit = fmaxf(0.5f * _params.flow_rate_max * (_terrain_vpos - _state.pos(2)) , 0.0f); + _flow_gnd_spd_max = fmaxf(_flow_gnd_spd_max , lower_limit); + flow_limit_hagl = false; + } else { - flow_gnd_spd_max = NAN; + _flow_gnd_spd_max = NAN; flow_limit_hagl = false; } - memcpy(vxy_max, &flow_gnd_spd_max, sizeof(float)); + memcpy(vxy_max, &_flow_gnd_spd_max, sizeof(float)); memcpy(limit_hagl, &flow_limit_hagl, sizeof(bool)); }