diff --git a/libraries/APM_Control/AP_PitchController.cpp b/libraries/APM_Control/AP_PitchController.cpp index ec129a2366..15f601a04d 100644 --- a/libraries/APM_Control/AP_PitchController.cpp +++ b/libraries/APM_Control/AP_PitchController.cpp @@ -177,7 +177,8 @@ float AP_PitchController::_get_rate_out(float desired_rate, float scaler, bool d // FF should be scaled by scaler/eas2tas, but since we have scaled // the AC_PID target above by scaler*scaler we need to instead // divide by scaler*eas2tas to get the right scaling - const float ff = degrees(rate_pid.get_ff() / (scaler * eas2tas)); + const float ff = degrees(ff_scale * rate_pid.get_ff() / (scaler * eas2tas)); + ff_scale = 1.0; if (disable_integrator) { rate_pid.reset_I(); diff --git a/libraries/APM_Control/AP_PitchController.h b/libraries/APM_Control/AP_PitchController.h index 279ad71bb5..1cf1d76a32 100644 --- a/libraries/APM_Control/AP_PitchController.h +++ b/libraries/APM_Control/AP_PitchController.h @@ -16,6 +16,10 @@ public: float get_rate_out(float desired_rate, float scaler); float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode); + // setup a one loop FF scale multiplier. This replaces any previous scale applied + // so should only be used when only one source of scaling is needed + void set_ff_scale(float _ff_scale) { ff_scale = _ff_scale; } + void reset_I(); /* @@ -56,6 +60,7 @@ private: float _last_out; AC_PID rate_pid{0.04, 0.15, 0, 0.345, 0.666, 3, 0, 12, 150, 1}; float angle_err_deg; + float ff_scale = 1.0; AP_PIDInfo _pid_info; diff --git a/libraries/APM_Control/AP_RollController.cpp b/libraries/APM_Control/AP_RollController.cpp index 3929aa46dd..2faff60caa 100644 --- a/libraries/APM_Control/AP_RollController.cpp +++ b/libraries/APM_Control/AP_RollController.cpp @@ -165,7 +165,8 @@ float AP_RollController::_get_rate_out(float desired_rate, float scaler, bool di // FF should be scaled by scaler/eas2tas, but since we have scaled // the AC_PID target above by scaler*scaler we need to instead // divide by scaler*eas2tas to get the right scaling - const float ff = degrees(rate_pid.get_ff() / (scaler * eas2tas)); + const float ff = degrees(ff_scale * rate_pid.get_ff() / (scaler * eas2tas)); + ff_scale = 1.0; if (disable_integrator) { rate_pid.reset_I(); diff --git a/libraries/APM_Control/AP_RollController.h b/libraries/APM_Control/AP_RollController.h index 376920f52e..85633f1d5f 100644 --- a/libraries/APM_Control/AP_RollController.h +++ b/libraries/APM_Control/AP_RollController.h @@ -16,6 +16,10 @@ public: float get_rate_out(float desired_rate, float scaler); float get_servo_out(int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode); + // setup a one loop FF scale multiplier. This replaces any previous scale applied + // so should only be used when only one source of scaling is needed + void set_ff_scale(float _ff_scale) { ff_scale = _ff_scale; } + void reset_I(); /* @@ -56,6 +60,7 @@ private: float _last_out; AC_PID rate_pid{0.08, 0.15, 0, 0.345, 0.666, 3, 0, 12, 150, 1}; float angle_err_deg; + float ff_scale = 1.0; AP_PIDInfo _pid_info;