fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 17:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

PID rate controller - Add controller gain to support Ideal PID form (ISA standard)

Browse Source
This commit is contained in:
bresch
2019-07-16 10:24:19 +02:00
committed by Mathieu Bresciani
parent 7c05073044
commit d24c415fd7
3 changed files with 71 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+7 -3
View File
@@ -106,6 +106,10 @@ MulticopterAttitudeControl::parameters_updated()
_rate_d = Vector3f(_param_mc_rollrate_d.get(), _param_mc_pitchrate_d.get(), _param_mc_yawrate_d.get());
_rate_ff = Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get());
// The controller gain K is used to convert the parallel (P + I/s + sD) form
// to the ideal (K * [1 + 1/sTi + sTd]) form
_rate_k = Vector3f(_param_mc_rollrate_k.get(), _param_mc_pitchrate_k.get(), _param_mc_yawrate_k.get());
if (fabsf(_lp_filters_d.get_cutoff_freq() - _param_mc_dterm_cutoff.get()) > 0.01f) {
_lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _param_mc_dterm_cutoff.get());
_lp_filters_d.reset(_rates_prev);
@@ -446,9 +450,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
/* apply low-pass filtering to the rates for D-term */
Vector3f rates_filtered(_lp_filters_d.apply(rates));
_att_control = rates_p_scaled.emult(rates_err) +
_rates_int -
rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt +
_att_control = _rate_k.emult(rates_p_scaled.emult(rates_err) +
_rates_int -
rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt) +
_rate_ff.emult(_rates_sp);
_rates_prev = rates;