From 64a6971bdb8d92b702a56fe27c3acab8513f7c8f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 28 Jun 2024 01:37:24 -0400 Subject: [PATCH] ekf2: only limit opt flow HAGL if range only terrain - increase HALG limit from 75%->90% of sensor max --- src/modules/ekf2/EKF/ekf_helper.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 791ed89e26..5ac5f5467d 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -244,8 +244,8 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl // 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(); + // Allow use of 90% of rangefinder maximum range to allow for angular motion + const float rangefinder_hagl_max = 0.9f * _range_sensor.getValidMaxVal(); // TODO : calculate visual odometry limits const bool relying_on_rangefinder = isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.rng_hgt); @@ -262,8 +262,16 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl 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); + float flow_hagl_min = _flow_min_distance; + float flow_hagl_max = _flow_max_distance; + + // only limit optical flow height is dependent on range finder or terrain estimate invalid (precaution) + if ((!_hagl_sensor_status.flags.flow && _hagl_sensor_status.flags.range_finder) + || !isTerrainEstimateValid() + ) { + flow_hagl_min = math::max(flow_hagl_min, rangefinder_hagl_min); + flow_hagl_max = math::min(flow_hagl_max, rangefinder_hagl_max); + } const float flow_constrained_height = math::constrain(getHagl(), flow_hagl_min, flow_hagl_max);