From 4e29e1e6b0401dc430c30ddf77b436f743ec41f4 Mon Sep 17 00:00:00 2001 From: pieniacy Date: Sat, 20 Apr 2024 14:08:45 +0200 Subject: [PATCH] Plane: Limit possible climb/sink rate in FBWB/CRUISE to TECS limits --- ArduPlane/navigation.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 3abf8a58be..4dc71bbee3 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -401,7 +401,10 @@ void Plane::update_fbwb_speed_height(void) set_target_altitude_current(); } - int32_t alt_change_cm = g.flybywire_climb_rate * elevator_input * dt * 100; + float climb_rate = g.flybywire_climb_rate * elevator_input; + climb_rate = constrain_float(climb_rate, -TECS_controller.get_max_sinkrate(), TECS_controller.get_max_climbrate()); + + int32_t alt_change_cm = climb_rate * dt * 100; change_target_altitude(alt_change_cm); #if HAL_SOARING_ENABLED