From 79a7ef2869d6fbac9af280b7b4fc129edceb5f19 Mon Sep 17 00:00:00 2001 From: Balduin Date: Fri, 13 Feb 2026 13:42:17 +0100 Subject: [PATCH] RtlTimeEstimator: Consider minimum ground speed (#26405) * RtlTimeEstimator: Consider minimum ground speed This fixes an issue seen when the wind is faster than the trim airspeed, but not faster than the max airspeed. In that case the RTL time estimator currently assumes that we always fly at trim airspeed and are thus unable to cover ground in the upwind direction. The result is a very large RTL time estimate, resulting in RTL if configured by COM_FLTT_LOW_ACT. By considering the FW_GND_SPD_MIN parameter, we correct this wrong assumption. The RTL remaining time estimate now becomes realistic in this situation. * RtlTimeEstimator: assume min ground speed of 5 if param unavailable --- src/lib/rtl/rtl_time_estimator.cpp | 14 +++++++++++++- src/lib/rtl/rtl_time_estimator.h | 1 + 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/lib/rtl/rtl_time_estimator.cpp b/src/lib/rtl/rtl_time_estimator.cpp index ddc851c5e31..c7cc22bfbbc 100644 --- a/src/lib/rtl/rtl_time_estimator.cpp +++ b/src/lib/rtl/rtl_time_estimator.cpp @@ -54,6 +54,7 @@ RtlTimeEstimator::RtlTimeEstimator() : ModuleParams(nullptr) _param_fw_climb_rate = param_find("FW_T_CLMB_R_SP"); _param_fw_sink_rate = param_find("FW_T_SINK_R_SP"); _param_fw_airspeed_trim = param_find("FW_AIRSPD_TRIM"); + _param_fw_gnd_spd_min = param_find("FW_GND_SPD_MIN"); _param_mpc_xy_cruise = param_find("MPC_XY_CRUISE"); _param_rover_cruise_speed = param_find("RO_SPEED_LIM"); }; @@ -142,7 +143,18 @@ float RtlTimeEstimator::getCruiseGroundSpeed(const matrix::Vector2f &direction_n const float ground_speed = sqrtf(cruise_speed * cruise_speed - wind_across_dir * wind_across_dir) + fminf( 0.f, wind_along_dir); - cruise_speed = ground_speed; + // Assume that minimum ground speed is always satisfied. If + // windspeed < cas2tas(FW_AIRSPD_MAX) - FW_GND_SPD_MIN + // the assumption always holds. Otherwise it breaks down when + // flying upwind, and the RTL time estimate is optimistic. + + float fw_gnd_spd_min = 5.0f; + + if (_param_fw_gnd_spd_min != PARAM_INVALID) { + param_get(_param_fw_gnd_spd_min, &fw_gnd_spd_min); + } + + cruise_speed = fmaxf(ground_speed, fw_gnd_spd_min); } return cruise_speed; diff --git a/src/lib/rtl/rtl_time_estimator.h b/src/lib/rtl/rtl_time_estimator.h index 7ca79fba1ff..c919f96e3c1 100644 --- a/src/lib/rtl/rtl_time_estimator.h +++ b/src/lib/rtl/rtl_time_estimator.h @@ -129,6 +129,7 @@ private: param_t _param_fw_sink_rate{PARAM_INVALID}; /**< FW descend speed parameter */ param_t _param_fw_airspeed_trim{PARAM_INVALID}; /**< FW cruise airspeed parameter */ + param_t _param_fw_gnd_spd_min{PARAM_INVALID}; /**< FW min groundspeed parameter */ param_t _param_mpc_xy_cruise{PARAM_INVALID}; /**< MC horizontal cruise speed parameter */ param_t _param_rover_cruise_speed{PARAM_INVALID}; /**< Rover cruise speed parameter */