From 41474f862fa94bc203131234e8b1d8af1fc1087e Mon Sep 17 00:00:00 2001 From: pieniacy Date: Fri, 12 Apr 2024 21:03:16 +0200 Subject: [PATCH] Plane: Fix FBWB/CRUISE missing zero crossing of elevator input --- ArduPlane/navigation.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 762b190ffee72..3abf8a58becef 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -394,15 +394,16 @@ void Plane::update_fbwb_speed_height(void) elevator_input = -elevator_input; } - int32_t alt_change_cm = g.flybywire_climb_rate * elevator_input * dt * 100; - change_target_altitude(alt_change_cm); - - if (is_zero(elevator_input) && !is_zero(target_altitude.last_elevator_input)) { - // the user has just released the elevator, lock in - // the current altitude + bool input_stop_climb = !is_positive(elevator_input) && is_positive(target_altitude.last_elevator_input); + bool input_stop_descent = !is_negative(elevator_input) && is_negative(target_altitude.last_elevator_input); + if (input_stop_climb || input_stop_descent) { + // user elevator input reached or passed zero, lock in the current altitude set_target_altitude_current(); } + int32_t alt_change_cm = g.flybywire_climb_rate * elevator_input * dt * 100; + change_target_altitude(alt_change_cm); + #if HAL_SOARING_ENABLED if (g2.soaring_controller.is_active()) { if (g2.soaring_controller.get_throttle_suppressed()) {