diff --git a/libraries/AC_PID/AC_PID_2D.cpp b/libraries/AC_PID/AC_PID_2D.cpp index 48f11f6096..a79c20678f 100644 --- a/libraries/AC_PID/AC_PID_2D.cpp +++ b/libraries/AC_PID/AC_PID_2D.cpp @@ -97,6 +97,10 @@ Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measureme // update I term update_i(dt, limit); + // calculate slew limit + _slew_calc.update(Vector2f{_pid_info_x.P + _pid_info_x.D, _pid_info_y.P + _pid_info_y.D}, dt); + _pid_info_x.slew_rate = _pid_info_y.slew_rate = _slew_calc.get_slew_rate(); + _pid_info_x.target = _target.x; _pid_info_x.actual = measurement.x; _pid_info_x.error = _error.x; diff --git a/libraries/AC_PID/AC_PID_2D.h b/libraries/AC_PID/AC_PID_2D.h index 0fd7e8a6ec..8213997df1 100644 --- a/libraries/AC_PID/AC_PID_2D.h +++ b/libraries/AC_PID/AC_PID_2D.h @@ -8,6 +8,7 @@ #include #include #include +#include /// @class AC_PID_2D /// @brief Copter PID control class @@ -72,6 +73,9 @@ public: void set_integrator(const Vector3f& i) { set_integrator(Vector2f{i.x, i.y}); } void set_integrator(const Vector2f& i); + // return current slew rate of slew limiter. Will return 0 if SMAX is zero + float get_slew_rate(void) const { return _slew_calc.get_slew_rate(); } + const AP_PIDInfo& get_pid_info_x(void) const { return _pid_info_x; } const AP_PIDInfo& get_pid_info_y(void) const { return _pid_info_y; } @@ -99,6 +103,8 @@ protected: AP_PIDInfo _pid_info_x; AP_PIDInfo _pid_info_y; + SlewCalculator2D _slew_calc; // 2D slew rate calculator + private: const float default_kp; const float default_ki;