UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-08-01 20:27:57 +08:00
Code Issues Packages Projects Releases Wiki Activity

RtlTimeEstimator: Consider minimum ground speed (#26405)

Browse Source 
* 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
This commit is contained in:
Balduin
2026-02-13 13:42:17 +01:00
committed by GitHub
parent b9f4de0b51
commit 79a7ef2869
2 changed files with 14 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/lib/rtl/rtl_time_estimator.cpp
View File

@@ -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;

1
src/lib/rtl/rtl_time_estimator.h
View File

@@ -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 */