Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PID: correct error caculation to use latest target

Browse Source
This commit is contained in:
Iampete1 2024-07-06 02:23:14 +01:00 committed by Randy Mackay
parent 7a51a18e15
commit e896a60b53
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; _pid_info.reset = _flags._reset_filter;
if (_flags._reset_filter) { if (_flags._reset_filter) {
_flags._reset_filter = false; _flags._reset_filter = false;
// Reset target filter
_target = target; _target = target;
_error = _target - measurement;
_derivative = 0.0f;
_target_derivative = 0.0f;
#if AP_FILTER_ENABLED #if AP_FILTER_ENABLED
if (_target_notch != nullptr) { if (_target_notch != nullptr) {
_target_notch->reset(); _target_notch->reset();
_target = _target_notch->apply(_target); _target = _target_notch->apply(_target);
} }
#endif
// Calculate error and reset error filter
_error = _target - measurement;
#if AP_FILTER_ENABLED
if (_error_notch != nullptr) { if (_error_notch != nullptr) {
_error_notch->reset(); _error_notch->reset();
_error = _error_notch->apply(_error); _error = _error_notch->apply(_error);
} }
#endif #endif
// Zero derivatives
_derivative = 0.0f;
_target_derivative = 0.0f;
} else { } else {
float error_last = _error;
float target_last = _target; // Apply target filters
float error = _target - measurement; const float target_last = _target;
#if AP_FILTER_ENABLED #if AP_FILTER_ENABLED
// apply notch filters before FTLD/FLTE to avoid shot noise // apply notch filters before FTLD/FLTE to avoid shot noise
if (_target_notch != nullptr) { if (_target_notch != nullptr) {
target = _target_notch->apply(target); 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) { if (_error_notch != nullptr) {
error = _error_notch->apply(error); error = _error_notch->apply(error);
} }
#endif #endif
_target += get_filt_T_alpha(dt) * (target - _target);
_error += get_filt_E_alpha(dt) * (error - _error); _error += get_filt_E_alpha(dt) * (error - _error);
// calculate and filter derivative // calculate and filter derivative