From dc8ed978097b6efed46719cdc11bec0c6e914c4e Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 20 Jun 2022 18:10:02 -0400 Subject: [PATCH] ekf2: optical flow control limits constrain speed using HAGL max --- src/modules/ekf2/EKF/ekf_helper.cpp | 37 +++++++++++----------- src/modules/ekf2/EKF/estimator_interface.h | 4 +-- 2 files changed, 20 insertions(+), 21 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 5c54dc36c8..a8a7a08a2e 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -838,43 +838,42 @@ hagl_max : Maximum height above ground (meters). NaN when limiting is not needed */ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const { + // Do not require limiting by default + *vxy_max = NAN; + *vz_max = NAN; + *hagl_min = NAN; + *hagl_max = NAN; + // Calculate range finder limits const float rangefinder_hagl_min = _range_sensor.getValidMinVal(); + // Allow use of 75% of rangefinder maximum range to allow for angular motion const float rangefinder_hagl_max = 0.75f * _range_sensor.getValidMaxVal(); - // Calculate optical flow limits - // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion - const float flow_vxy_max = fmaxf(0.5f * _flow_max_rate * (_terrain_vpos - _state.pos(2)), 0.0f); - const float flow_hagl_min = _flow_min_distance; - const float flow_hagl_max = _flow_max_distance; - // TODO : calculate visual odometry limits - const bool relying_on_rangefinder = _control_status.flags.rng_hgt && !_params.range_aid; - const bool relying_on_optical_flow = isOnlyActiveSourceOfHorizontalAiding(_control_status.flags.opt_flow); - // Do not require limiting by default - *vxy_max = NAN; - *vz_max = NAN; - *hagl_min = NAN; - *hagl_max = NAN; - // Keep within range sensor limit when using rangefinder as primary height source if (relying_on_rangefinder) { - *vxy_max = NAN; - *vz_max = NAN; *hagl_min = rangefinder_hagl_min; *hagl_max = rangefinder_hagl_max; } // Keep within flow AND range sensor limits when exclusively using optical flow if (relying_on_optical_flow) { + // Calculate optical flow limits + const float flow_hagl_min = fmaxf(rangefinder_hagl_min, _flow_min_distance); + const float flow_hagl_max = fminf(rangefinder_hagl_max, _flow_max_distance); + + const float flow_constrained_height = math::constrain(_terrain_vpos - _state.pos(2), flow_hagl_min, flow_hagl_max); + + // Allow ground relative velocity to use 50% of available flow sensor range to allow for angular motion + const float flow_vxy_max = 0.5f * _flow_max_rate * flow_constrained_height; + *vxy_max = flow_vxy_max; - *vz_max = NAN; - *hagl_min = fmaxf(rangefinder_hagl_min, flow_hagl_min); - *hagl_max = fminf(rangefinder_hagl_max, flow_hagl_max); + *hagl_min = flow_hagl_min; + *hagl_max = flow_hagl_max; } } diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index 7144ee60fe..6693c2662e 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -303,9 +303,9 @@ protected: float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // air density (kg/m**3) // Sensor limitations - float _flow_max_rate{0.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s) + float _flow_max_rate{1.0f}; ///< maximum angular flow rate that the optical flow sensor can measure (rad/s) float _flow_min_distance{0.0f}; ///< minimum distance that the optical flow sensor can operate at (m) - float _flow_max_distance{0.0f}; ///< maximum distance that the optical flow sensor can operate at (m) + float _flow_max_distance{10.f}; ///< maximum distance that the optical flow sensor can operate at (m) // Output Predictor outputSample _output_new{}; // filter output on the non-delayed time horizon