From 259e7d1d5373dbad1ee27b1d72d1ae66394c4ea3 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 11 Apr 2025 18:07:27 +0200 Subject: [PATCH] FWModeManger: remove throttle spike during flaring by waiting with height rate change Signed-off-by: Silvan Fuhrer --- .../fw_pos_control/FixedwingPositionControl.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index fc19682529..eb9472eae4 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -1356,8 +1356,14 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const float flare_ramp_interpolator_sqrt = sqrtf(flare_ramp_interpolator); - const float height_rate_setpoint = flare_ramp_interpolator_sqrt * (-_param_fw_lnd_fl_sink.get()) + - (1.0f - flare_ramp_interpolator_sqrt) * _flare_states.initial_height_rate_setpoint; + // Use separate ramp for the altitude setpoint that's starting only when the other is finished + // to allow motor to be ramped down before height rate setpoint is adapted for flaring. + const float flare_hieght_rate_interpolator = math::constrain((seconds_since_flare_start - + _param_fw_lnd_fl_time.get()) / (_param_fw_lnd_fl_time.get()), 0.f, 1.f); + const float flare_hieght_rate_interpolator_sqrt = sqrt(flare_hieght_rate_interpolator); + + const float height_rate_setpoint = flare_hieght_rate_interpolator_sqrt * (-_param_fw_lnd_fl_sink.get()) + + (1.0f - flare_hieght_rate_interpolator_sqrt) * _flare_states.initial_height_rate_setpoint; float pitch_min_rad = flare_ramp_interpolator_sqrt * radians(_param_fw_lnd_fl_pmin.get()) + (1.0f - flare_ramp_interpolator_sqrt) * radians(_param_fw_p_lim_min.get());