Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PID: correct error caculation to use latest target

This commit is contained in:
Iampete1 2024-07-06 02:23:14 +01:00 committed by Andrew Tridgell
parent 0e9c5a0440
commit 731405c465
1 changed files with 21 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AC_PID/AC_PID.cpp
View File

@ -201,34 +201,48 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit,
_pid_info.reset = _flags._reset_filter;
if (_flags._reset_filter) {
_flags._reset_filter = false;
// Reset target filter
_target = target;
_error = _target - measurement;
_derivative = 0.0f;
_target_derivative = 0.0f;
#if AP_FILTER_ENABLED
if (_target_notch != nullptr) {
_target_notch->reset();
_target = _target_notch->apply(_target);
}
#endif
// Calculate error and reset error filter
_error = _target - measurement;
#if AP_FILTER_ENABLED
if (_error_notch != nullptr) {
_error_notch->reset();
_error = _error_notch->apply(_error);
}
#endif
// Zero derivatives
_derivative = 0.0f;
_target_derivative = 0.0f;
} else {
float error_last = _error;
float target_last = _target;
float error = _target - measurement;
// Apply target filters
const float target_last = _target;
#if AP_FILTER_ENABLED
// apply notch filters before FTLD/FLTE to avoid shot noise
if (_target_notch != nullptr) {
target = _target_notch->apply(target);
}
#endif
_target += get_filt_T_alpha(dt) * (target - _target);
// Calculate error and apply error filter
const float error_last = _error;
float error = _target - measurement;
#if AP_FILTER_ENABLED
if (_error_notch != nullptr) {
error = _error_notch->apply(error);
}
#endif
_target += get_filt_T_alpha(dt) * (target - _target);
_error += get_filt_E_alpha(dt) * (error - _error);
// calculate and filter derivative