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

AC_PID: Support PD Max

This commit is contained in:
Leonard Hall 2023-09-19 11:16:00 +09:30 committed by Andrew Tridgell
parent 2030e6c9e2
commit 3de0bcefdb
2 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AC_PID/AC_PID.cpp
View File

@ -64,6 +64,12 @@ const AP_Param::GroupInfo AC_PID::var_info[] = {
// @User: Advanced
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("SMAX", 12, AC_PID, _slew_rate_max, default_slew_rate_max),
// @Param: PDMX
// @DisplayName: PD sum maximum
// @Description: The maximum/minimum value that the sum of the P and D term can output
// @User: Advanced
AP_GROUPINFO("PDMX", 13, AC_PID, _kpdmax, 0),
AP_GROUPEND
};
@ -172,7 +178,10 @@ float AC_PID::update_all(float target, float measurement, float dt, bool limit,
_pid_info.P = P_out;
_pid_info.D = D_out;
return P_out + _integrator + D_out;
if (is_positive(_kpdmax)) {
return constrain_float(P_out + D_out, -_kpdmax, _kpdmax) + _integrator;
}
return P_out + D_out + _integrator;
}
// update_error - set error input to PID controller and calculate outputs
@ -225,7 +234,10 @@ float AC_PID::update_error(float error, float dt, bool limit)
_pid_info.P = P_out;
_pid_info.D = D_out;
return P_out + _integrator + D_out;
if (is_positive(_kpdmax)) {
return constrain_float(P_out + D_out, -_kpdmax, _kpdmax) + _integrator;
}
return P_out + D_out + _integrator;
}
// update_i - update the integral
@ -279,6 +291,8 @@ void AC_PID::load_gains()
_kff.load();
_kimax.load();
_kimax.set(fabsf(_kimax));
_kpdmax.load();
_kpdmax.set(fabsf(_kpdmax));
_filt_T_hz.load();
_filt_E_hz.load();
_filt_D_hz.load();

4
libraries/AC_PID/AC_PID.h
View File

@ -103,6 +103,7 @@ public:
AP_Float &kI() { return _ki; }
AP_Float &kD() { return _kd; }
AP_Float &kIMAX() { return _kimax; }
AP_Float &kPDMAX() { return _kpdmax; }
AP_Float &ff() { return _kff;}
AP_Float &filt_T_hz() { return _filt_T_hz; }
AP_Float &filt_E_hz() { return _filt_E_hz; }
@ -110,6 +111,7 @@ public:
AP_Float &slew_limit() { return _slew_rate_max; }
float imax() const { return _kimax.get(); }
float pdmax() const { return _kpdmax.get(); }
float get_filt_T_alpha(float dt) const;
float get_filt_E_alpha(float dt) const;
float get_filt_D_alpha(float dt) const;
@ -120,6 +122,7 @@ public:
void kD(const float v) { _kd.set(v); }
void ff(const float v) { _kff.set(v); }
void imax(const float v) { _kimax.set(fabsf(v)); }
void pdmax(const float v) { _kpdmax.set(fabsf(v)); }
void filt_T_hz(const float v);
void filt_E_hz(const float v);
void filt_D_hz(const float v);
@ -160,6 +163,7 @@ protected:
AP_Float _kd;
AP_Float _kff;
AP_Float _kimax;
AP_Float _kpdmax;
AP_Float _filt_T_hz; // PID target filter frequency in Hz
AP_Float _filt_E_hz; // PID error filter frequency in Hz
AP_Float _filt_D_hz; // PID derivative filter frequency in Hz