diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 3012192d74..4c23186b91 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -712,6 +712,12 @@ private: // lookahead value for height error reporting float lookahead; #endif + + // last input for FBWB/CRUISE height control + float last_elevator_input; + + // last time we checked for pilot control of height + uint32_t last_elev_check_us; } target_altitude {}; float relative_altitude = 0.0f; diff --git a/ArduPlane/navigation.cpp b/ArduPlane/navigation.cpp index 1831b9b9c7..cffad479b1 100644 --- a/ArduPlane/navigation.cpp +++ b/ArduPlane/navigation.cpp @@ -248,29 +248,38 @@ void Plane::update_cruise() */ void Plane::update_fbwb_speed_height(void) { - static float last_elevator_input; - float elevator_input; - elevator_input = channel_pitch->get_control_in() / 4500.0f; + uint32_t now = micros(); + if (now - target_altitude.last_elev_check_us >= 100000) { + // we don't run this on every loop as it would give too small granularity on quadplanes at 300Hz, and + // give below 1cm altitude change, which would result in no climb or descent + float dt = (now - target_altitude.last_elev_check_us) * 1.0e-6; + dt = constrain_float(dt, 0.1, 0.15); + + target_altitude.last_elev_check_us = now; + + float elevator_input = channel_pitch->get_control_in() / 4500.0f; - if (g.flybywire_elev_reverse) { - elevator_input = -elevator_input; + if (g.flybywire_elev_reverse) { + 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 + set_target_altitude_current(); + } + + target_altitude.last_elevator_input = elevator_input; } - change_target_altitude(g.flybywire_climb_rate * elevator_input * perf.delta_us_fast_loop * 0.0001f); - - if (is_zero(elevator_input) && !is_zero(last_elevator_input)) { - // the user has just released the elevator, lock in - // the current altitude - set_target_altitude_current(); - } - // check for FBWB altitude limit check_minimum_altitude(); altitude_error_cm = calc_altitude_error_cm(); - last_elevator_input = elevator_input; - calc_throttle(); calc_nav_pitch(); }