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: apply PD limit in such that limit flag is set and reported P and D terms are correct

Browse Source
This commit is contained in:
Iampete1 2023-09-12 20:51:54 +01:00 committed by Andrew Tridgell
parent 93ccec3203
commit 9599318fff
2 changed files with 25 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AC_PID/AC_PID.cpp
View File

@ -172,15 +172,23 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit,
P_out *= boost; P_out *= boost;
D_out *= boost; D_out *= boost;
// Apply PD sum limit if enabled
if (is_positive(_kpdmax)) {
const float PD_sum_abs = fabsf(P_out + D_out);
if (PD_sum_abs > _kpdmax) {
const float PD_scale = _kpdmax / PD_sum_abs;
P_out *= PD_scale;
D_out *= PD_scale;
_pid_info.PD_limit = true;
}
}
_pid_info.target = _target; _pid_info.target = _target;
_pid_info.actual = measurement; _pid_info.actual = measurement;
_pid_info.error = _error; _pid_info.error = _error;
_pid_info.P = P_out; _pid_info.P = P_out;
_pid_info.D = D_out; _pid_info.D = D_out;
if (is_positive(_kpdmax)) {
return constrain_float(P_out + D_out, -_kpdmax, _kpdmax) + _integrator;
}
return P_out + D_out + _integrator; return P_out + D_out + _integrator;
} }
@ -227,16 +235,24 @@ float AC_PID::update_error(float error, float dt, bool limit)
P_out *= _pid_info.Dmod; P_out *= _pid_info.Dmod;
D_out *= _pid_info.Dmod; D_out *= _pid_info.Dmod;
// Apply PD sum limit if enabled
if (is_positive(_kpdmax)) {
const float PD_sum_abs = fabsf(P_out + D_out);
if (PD_sum_abs > _kpdmax) {
const float PD_scale = _kpdmax / PD_sum_abs;
P_out *= PD_scale;
D_out *= PD_scale;
_pid_info.PD_limit = true;
}
}
_pid_info.target = 0.0f; _pid_info.target = 0.0f;
_pid_info.actual = 0.0f; _pid_info.actual = 0.0f;
_pid_info.error = _error; _pid_info.error = _error;
_pid_info.P = P_out; _pid_info.P = P_out;
_pid_info.D = D_out; _pid_info.D = D_out;
if (is_positive(_kpdmax)) {
return constrain_float(P_out + D_out, -_kpdmax, _kpdmax) + _integrator;
}
return P_out + D_out + _integrator; return P_out + D_out + _integrator;
} }

3
libraries/AC_PID/AP_PIDInfo.h
View File

@ -16,5 +16,6 @@ struct AP_PIDInfo {
float FF; float FF;
float Dmod; float Dmod;
float slew_rate; float slew_rate;
bool limit; bool limit;
bool PD_limit;
}; };