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)); }