From 1a6b16c8ce74fad27a16bf7f0ced7704c61c7173 Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Wed, 16 Apr 2025 09:47:28 +0200 Subject: [PATCH] fw-lat-lon: set altitude setpoint to NAN if height rate setpoint is provided. When both are set, TECS only considers the height rate. --- .../FwLateralLongitudinalControl.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index dd3d7eb879..b0306b0655 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -206,7 +206,10 @@ void FwLateralLongitudinalControl::Run() const float airspeed_sp_eas = adapt_airspeed_setpoint(control_interval, _long_control_sp.equivalent_airspeed, _min_airspeed_from_guidance, _lateral_control_state.wind_speed.length()); - tecs_update_pitch_throttle(control_interval, _long_control_sp.altitude, + // If the both altitude and height rate are set, set altitude setpoint to NAN + const float altitude_sp = PX4_ISFINITE(_long_control_sp.height_rate) ? NAN : _long_control_sp.altitude; + + tecs_update_pitch_throttle(control_interval, altitude_sp, airspeed_sp_eas, _long_configuration.pitch_min, _long_configuration.pitch_max,