diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 17f64659b4..69e5ebe484 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -771,9 +771,12 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) rates(1) = _ctrl_state.pitch_rate; rates(2) = _ctrl_state.yaw_rate; + /* throttle pid attenuation factor */ + float tpa = fminf(1.0f, 1.5f - fabsf(_v_rates_sp.thrust)); + /* angular rates error */ math::Vector<3> rates_err = _rates_sp - rates; - _att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + + _att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int + _params.rate_ff.emult(_rates_sp - _rates_sp_prev) / dt; _rates_sp_prev = _rates_sp; _rates_prev = rates;