diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp index 51b216c42d..4e8c1b5812 100644 --- a/libraries/AC_PID/AC_PID.cpp +++ b/libraries/AC_PID/AC_PID.cpp @@ -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(); diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h index 5f3befd096..7ae4592473 100644 --- a/libraries/AC_PID/AC_PID.h +++ b/libraries/AC_PID/AC_PID.h @@ -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