From c7d4d1f45c67787b920b82d26a28a6629c022eaf Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 16 Mar 2023 11:57:17 +0100 Subject: [PATCH] FWPositionControl: HOTFIX: zero airspeed rate estimation input to TECS Signed-off-by: Silvan Fuhrer --- .../fw_path_navigation/FixedwingPositionControl.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index 10a8acd141..d517dc7fc1 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -2553,6 +2553,10 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min, throttle_max); + // HOTFIX: the airspeed rate estimate using acceleration in body-forward direction has shown to lead to high biases + // when flying tight turns. It's in this case much safer to just set the estimated airspeed rate to 0. + const float airspeed_rate_estimate = 0.f; + _tecs.update(_pitch - radians(_param_fw_psp_off.get()), _current_altitude, alt_sp, @@ -2566,11 +2570,11 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva pitch_max_rad - radians(_param_fw_psp_off.get()), desired_max_climbrate, desired_max_sinkrate, - _body_acceleration_x, + airspeed_rate_estimate, -_local_pos.vz, hgt_rate_sp); - tecs_status_publish(alt_sp, airspeed_sp, -_local_pos.vz, throttle_trim_comp); + tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_comp); } float