yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

MC rate control - Scale the integrator with K during the integration

Browse Source 
part to avoid having to scale its saturation separately. This is
required to avoid premature saturation of the integrator when using
the K term.
Also remove double saturation of the integrator
This commit is contained in:
bresch
2019-09-19 09:17:15 +02:00
committed by Matthias Grob
parent 67e528ec0c
commit e9ab6a75ba
1 changed files with 5 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@@ -415,9 +415,9 @@ MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rat
/* apply low-pass filtering to the rates for D-term */ /* apply low-pass filtering to the rates for D-term */
Vector3f rates_filtered(_lp_filters_d.apply(rates)); Vector3f rates_filtered(_lp_filters_d.apply(rates));
_att_control = _rate_k.emult(rates_p_scaled.emult(rates_err) + _att_control = _rate_k.emult(rates_p_scaled.emult(rates_err) -
_rates_int -
rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt) + rates_d_scaled.emult(rates_filtered - _rates_prev_filtered) / dt) +
_rates_int +
_rate_ff.emult(_rates_sp); _rate_ff.emult(_rates_sp);
_rates_prev = rates; _rates_prev = rates;
@@ -441,13 +441,11 @@ MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rat
// prevent further positive control saturation // prevent further positive control saturation
if (positive_saturation) { if (positive_saturation) {
rates_err(i) = math::min(rates_err(i), 0.0f); rates_err(i) = math::min(rates_err(i), 0.0f);
} }
// prevent further negative control saturation // prevent further negative control saturation
if (negative_saturation) { if (negative_saturation) {
rates_err(i) = math::max(rates_err(i), 0.0f); rates_err(i) = math::max(rates_err(i), 0.0f);
} }
// I term factor: reduce the I gain with increasing rate error. // I term factor: reduce the I gain with increasing rate error.
@@ -460,19 +458,13 @@ MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rat
i_factor = math::max(0.0f, 1.f - i_factor * i_factor); i_factor = math::max(0.0f, 1.f - i_factor * i_factor);
// Perform the integration using a first order method and do not propagate the result if out of range or invalid // 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) + i_factor * rates_i_scaled(i) * rates_err(i) * dt; float rate_i = _rates_int(i) + i_factor * _rate_k(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;
if (PX4_ISFINITE(rate_i)) {
_rates_int(i) = math::constrain(rate_i, -_rate_int_lim(i), _rate_int_lim(i));
} }
} }
} }
/* explicitly limit the integrator state */
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
_rates_int(i) = math::constrain(_rates_int(i), -_rate_int_lim(i), _rate_int_lim(i));
}
} }
void void