From 5c78af0f36bf7613c86bfcea6d50359125d240af Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 14 Aug 2016 21:43:10 -0600 Subject: [PATCH] simple linear throttle PID attenuation test --- src/modules/mc_att_control/mc_att_control_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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;