From 76bf9c6465a7bdae7c2037cf0cb17c451fcd26b2 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 26 Mar 2018 16:59:08 +0100 Subject: [PATCH] mc_att_control: reenable TPA for I term In commit eb67686b110a995a258e80298fe180149c41c2a5 the TPA scaled integral gain was reverted back to the normal unscaled gain which rendered the I part of TPA useless. --- src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 b23ea997fd7c..96362c76a9f8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -519,7 +519,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) rates(2) -= _sensor_bias.gyro_z_bias; Vector3f rates_p_scaled = _rate_p.emult(pid_attenuations(_tpa_breakpoint_p.get(), _tpa_rate_p.get())); - //Vector3f rates_i_scaled = _rate_i.emult(pid_attenuations(_tpa_breakpoint_i.get(), _tpa_rate_i.get())); + Vector3f rates_i_scaled = _rate_i.emult(pid_attenuations(_tpa_breakpoint_i.get(), _tpa_rate_i.get())); Vector3f rates_d_scaled = _rate_d.emult(pid_attenuations(_tpa_breakpoint_d.get(), _tpa_rate_d.get())); /* angular rates error */ @@ -567,7 +567,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) } // Perform the integration using a first order method and do not propagate the result if out of range or invalid - float rate_i = _rates_int(i) + _rate_i(i) * rates_err(i) * dt; + float rate_i = _rates_int(i) + rates_i_scaled(i) * rates_err(i) * dt; if (PX4_ISFINITE(rate_i) && rate_i > -_rate_int_lim(i) && rate_i < _rate_int_lim(i)) { _rates_int(i) = rate_i;