From 02aa6983a7929ca6d941138cfe5e6072973a2010 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 9 Jun 2015 09:50:08 +1000 Subject: [PATCH] APM_Control: fixed FF handling in steering controller --- libraries/APM_Control/AP_SteerController.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libraries/APM_Control/AP_SteerController.cpp b/libraries/APM_Control/AP_SteerController.cpp index b6eeb5bc7a..7c5f8dcc30 100644 --- a/libraries/APM_Control/AP_SteerController.cpp +++ b/libraries/APM_Control/AP_SteerController.cpp @@ -149,10 +149,11 @@ int32_t AP_SteerController::get_steering_out_rate(float desired_rate) _pid_info.I = constrain_float(_pid_info.I, -intLimScaled, intLimScaled); _pid_info.D = rate_error * _K_D * 4.0f; - _pid_info.FF = (ToRad(desired_rate) * kp_ff) * scaler; + _pid_info.P = (ToRad(desired_rate) * kp_ff) * scaler; + _pid_info.FF = (ToRad(desired_rate) * k_ff) * scaler; // Calculate the demanded control surface deflection - _last_out = _pid_info.D + _pid_info.FF + _pid_info.I; + _last_out = _pid_info.D + _pid_info.FF + _pid_info.P + _pid_info.I; // Convert to centi-degrees and constrain return constrain_float(_last_out * 100, -4500, 4500);