From b1c85113862e97a3849399b2c63d192c09c0ee1c Mon Sep 17 00:00:00 2001 From: Tim Tuxworth Date: Thu, 14 Dec 2023 11:46:41 -0700 Subject: [PATCH] AP_TECS: use new pitch_trim in degrees --- libraries/AP_TECS/AP_TECS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index c5aaeabc0e..336e8b481e 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -859,7 +859,7 @@ void AP_TECS::_update_throttle_without_airspeed(int16_t throttle_nudge) _pitch_demand_lpf.apply(_pitch_dem, _DT); const float pitch_demand_hpf = _pitch_dem - _pitch_demand_lpf.get(); _pitch_measured_lpf.apply(_ahrs.get_pitch(), _DT); - const float pitch_corrected_lpf = _pitch_measured_lpf.get() - radians(0.01f * (float)aparm.pitch_trim_cd); + const float pitch_corrected_lpf = _pitch_measured_lpf.get(); const float pitch_blended = pitch_demand_hpf + pitch_corrected_lpf; if (pitch_blended > 0.0f && _PITCHmaxf > 0.0f)