Plane: Limit possible climb/sink rate in FBWB/CRUISE to TECS limits

ArduPilot · Apr 22, 2024 · 4e29e1e · 4e29e1e
1 parent d1d71db
commit 4e29e1e
Showing 1 changed file with 4 additions and 1 deletion.
diff --git 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