From c60b215574f9efea2fee961e2da4fd3c54fb55a9 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Mon, 18 Jul 2022 08:14:07 +0200 Subject: [PATCH] fw pos ctrl: allow negative landing glide slope relative altitude when the vehicle did not track the slope well (e.g. at an offset above the track) and the altitude setpoint flattening on intersection with terrain, the throttle would spool up to smoothly intersect the newly flattened altitude setpoint, this could happen before the flare altitude was reached, which is bad. now the steady state glide behavior will be maintained, and flare can trigger at the appropriate time --- .../fw_pos_control_l1/FixedwingPositionControl.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index e14e24cd9b..3386cbc903 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1669,7 +1669,13 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo const float along_track_dist_to_touchdown = -landing_approach_vector.unit_or_zero().dot( local_position - local_land_point); const float glide_slope = _landing_approach_entrance_rel_alt / _landing_approach_entrance_offset_vector.norm(); - const float glide_slope_rel_alt = math::constrain(along_track_dist_to_touchdown * glide_slope, 0.0f, + + // NOTE: this relative altitude can go below zero, this is intentional. in the case the vehicle is tracking the glide + // slope at an offset above the track, making the altitude setpoint constant on intersection with terrain causes + // an increase in throttle (to slow down and smoothly intersect the flattened altitude setpoint), which is undesirable + // directly before the flare. instead, we keep the steady state behavior, and let the flare get triggered once at + // the desired altitude + const float glide_slope_rel_alt = math::min(along_track_dist_to_touchdown * glide_slope, _landing_approach_entrance_rel_alt); const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt);