Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 02:17:07 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

simple linear throttle PID attenuation test

Browse Source
This commit is contained in:
Mark Whitehorn
2016-08-14 21:43:10 -06:00
committed by Lorenz Meier
parent 31e634b082
commit 5c78af0f36
1 changed files with 4 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+4 -1
View File
@@ -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;