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
This commit is contained in:
Thomas Stastny 2022-07-18 08:14:07 +02:00 committed by Daniel Agar
parent 4953fdd1ab
commit c60b215574
1 changed files with 7 additions and 1 deletions

View File

@ -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( const float along_track_dist_to_touchdown = -landing_approach_vector.unit_or_zero().dot(
local_position - local_land_point); local_position - local_land_point);
const float glide_slope = _landing_approach_entrance_rel_alt / _landing_approach_entrance_offset_vector.norm(); 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); _landing_approach_entrance_rel_alt);
const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt); const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt);