diff --git a/libraries/AC_PID/AC_P.cpp b/libraries/AC_PID/AC_P.cpp index 10e53178a0..5d62752af7 100644 --- a/libraries/AC_PID/AC_P.cpp +++ b/libraries/AC_PID/AC_P.cpp @@ -8,7 +8,7 @@ const AP_Param::GroupInfo AC_P::var_info[] = { // @Param: P // @DisplayName: PI Proportional Gain // @Description: P Gain which produces an output value that is proportional to the current error value - AP_GROUPINFO("P", 0, AC_P, _kp, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_P, _kp, default_kp), AP_GROUPEND }; diff --git a/libraries/AC_PID/AC_P.h b/libraries/AC_PID/AC_P.h index 706cf55e9a..e067e445de 100644 --- a/libraries/AC_PID/AC_P.h +++ b/libraries/AC_PID/AC_P.h @@ -20,10 +20,10 @@ public: /// /// @param initial_p Initial value for the P term. /// - AC_P(const float &initial_p = 0.0f) + AC_P(const float &initial_p = 0.0f) : + default_kp(initial_p) { - AP_Param::setup_object_defaults(this, var_info); - _kp.set_and_default(initial_p); + AP_Param::setup_object_defaults(this, var_info); } CLASS_NO_COPY(AC_P); @@ -65,4 +65,6 @@ public: private: AP_Float _kp; + + const float default_kp; }; diff --git a/libraries/AC_PID/AC_PI.cpp b/libraries/AC_PID/AC_PI.cpp index c28a998780..3819a797b9 100644 --- a/libraries/AC_PID/AC_PI.cpp +++ b/libraries/AC_PID/AC_PI.cpp @@ -25,14 +25,13 @@ const AP_Param::GroupInfo AC_PI::var_info[] = { }; // Constructor -AC_PI::AC_PI(float initial_p, float initial_i, float initial_imax) +AC_PI::AC_PI(float initial_p, float initial_i, float initial_imax) : + default_kp(initial_p), + default_ki(initial_i), + default_imax(initial_imax) { // load parameter values from eeprom AP_Param::setup_object_defaults(this, var_info); - - kP.set_and_default(initial_p); - kI.set_and_default(initial_i); - imax.set_and_default(initial_imax); } float AC_PI::update(float measurement, float target, float dt) diff --git a/libraries/AC_PID/AC_PI.h b/libraries/AC_PID/AC_PI.h index cdd894af98..bfae7c59fe 100644 --- a/libraries/AC_PID/AC_PI.h +++ b/libraries/AC_PID/AC_PI.h @@ -33,4 +33,10 @@ protected: AP_Float imax; float integrator; float output_P; + +private: + const float default_kp; + const float default_ki; + const float default_imax; + }; diff --git a/libraries/AC_PID/AC_PID_2D.cpp b/libraries/AC_PID/AC_PID_2D.cpp index 2792e24928..48f11f6096 100644 --- a/libraries/AC_PID/AC_PID_2D.cpp +++ b/libraries/AC_PID/AC_PID_2D.cpp @@ -4,65 +4,62 @@ #include #include "AC_PID_2D.h" -#define AC_PID_2D_FILT_E_HZ_DEFAULT 20.0f // default input filter frequency -#define AC_PID_2D_FILT_D_HZ_DEFAULT 10.0f // default input filter frequency #define AC_PID_2D_FILT_D_HZ_MIN 0.005f // minimum input filter frequency const AP_Param::GroupInfo AC_PID_2D::var_info[] = { // @Param: P // @DisplayName: PID Proportional Gain // @Description: P Gain which produces an output value that is proportional to the current error value - AP_GROUPINFO("P", 0, AC_PID_2D, _kp, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_PID_2D, _kp, default_kp), // @Param: I // @DisplayName: PID Integral Gain // @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error - AP_GROUPINFO("I", 1, AC_PID_2D, _ki, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("I", 1, AC_PID_2D, _ki, default_ki), // @Param: IMAX // @DisplayName: PID Integral Maximum // @Description: The maximum/minimum value that the I term can output - AP_GROUPINFO("IMAX", 2, AC_PID_2D, _kimax, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("IMAX", 2, AC_PID_2D, _kimax, default_kimax), // @Param: FLTE // @DisplayName: PID Input filter frequency in Hz // @Description: Input filter frequency in Hz // @Units: Hz - AP_GROUPINFO("FLTE", 3, AC_PID_2D, _filt_E_hz, AC_PID_2D_FILT_E_HZ_DEFAULT), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTE", 3, AC_PID_2D, _filt_E_hz, default_filt_E_hz), // @Param: D // @DisplayName: PID Derivative Gain // @Description: D Gain which produces an output that is proportional to the rate of change of the error - AP_GROUPINFO("D", 4, AC_PID_2D, _kd, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("D", 4, AC_PID_2D, _kd, default_kd), // @Param: FLTD // @DisplayName: D term filter frequency in Hz // @Description: D term filter frequency in Hz // @Units: Hz - AP_GROUPINFO("FLTD", 5, AC_PID_2D, _filt_D_hz, AC_PID_2D_FILT_D_HZ_DEFAULT), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTD", 5, AC_PID_2D, _filt_D_hz, default_filt_D_hz), // @Param: FF // @DisplayName: PID Feed Forward Gain // @Description: FF Gain which produces an output that is proportional to the magnitude of the target - AP_GROUPINFO("FF", 6, AC_PID_2D, _kff, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FF", 6, AC_PID_2D, _kff, default_kff), AP_GROUPEND }; // Constructor -AC_PID_2D::AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz) +AC_PID_2D::AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz) : + default_kp(initial_kP), + default_ki(initial_kI), + default_kd(initial_kD), + default_kff(initial_kFF), + default_kimax(initial_imax), + default_filt_E_hz(initial_filt_E_hz), + default_filt_D_hz(initial_filt_D_hz) { // load parameter values from eeprom AP_Param::setup_object_defaults(this, var_info); - _kp.set_and_default(initial_kP); - _ki.set_and_default(initial_kI); - _kd.set_and_default(initial_kD); - _kff.set_and_default(initial_kFF); - _kimax.set_and_default(initial_imax); - _filt_E_hz.set_and_default(initial_filt_E_hz); - _filt_D_hz.set_and_default(initial_filt_D_hz); - // reset input filter to first value received _reset_filter = true; } diff --git a/libraries/AC_PID/AC_PID_2D.h b/libraries/AC_PID/AC_PID_2D.h index 549152a990..0fd7e8a6ec 100644 --- a/libraries/AC_PID/AC_PID_2D.h +++ b/libraries/AC_PID/AC_PID_2D.h @@ -98,4 +98,13 @@ protected: AP_PIDInfo _pid_info_x; AP_PIDInfo _pid_info_y; + +private: + const float default_kp; + const float default_ki; + const float default_kd; + const float default_kff; + const float default_kimax; + const float default_filt_E_hz; + const float default_filt_D_hz; }; diff --git a/libraries/AC_PID/AC_PID_Basic.cpp b/libraries/AC_PID/AC_PID_Basic.cpp index 5fbdfa1a04..d3f98a946b 100644 --- a/libraries/AC_PID/AC_PID_Basic.cpp +++ b/libraries/AC_PID/AC_PID_Basic.cpp @@ -5,66 +5,63 @@ #include #include "AC_PID_Basic.h" -#define AC_PID_Basic_FILT_E_HZ_DEFAULT 20.0f // default input filter frequency #define AC_PID_Basic_FILT_E_HZ_MIN 0.01f // minimum input filter frequency -#define AC_PID_Basic_FILT_D_HZ_DEFAULT 10.0f // default input filter frequency #define AC_PID_Basic_FILT_D_HZ_MIN 0.005f // minimum input filter frequency const AP_Param::GroupInfo AC_PID_Basic::var_info[] = { // @Param: P // @DisplayName: PID Proportional Gain // @Description: P Gain which produces an output value that is proportional to the current error value - AP_GROUPINFO("P", 0, AC_PID_Basic, _kp, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_PID_Basic, _kp, default_kp), // @Param: I // @DisplayName: PID Integral Gain // @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error - AP_GROUPINFO("I", 1, AC_PID_Basic, _ki, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("I", 1, AC_PID_Basic, _ki, default_ki), // @Param: IMAX // @DisplayName: PID Integral Maximum // @Description: The maximum/minimum value that the I term can output - AP_GROUPINFO("IMAX", 2, AC_PID_Basic, _kimax, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("IMAX", 2, AC_PID_Basic, _kimax, default_kimax), // @Param: FLTE // @DisplayName: PID Error filter frequency in Hz // @Description: Error filter frequency in Hz // @Units: Hz - AP_GROUPINFO("FLTE", 3, AC_PID_Basic, _filt_E_hz, AC_PID_Basic_FILT_E_HZ_DEFAULT), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTE", 3, AC_PID_Basic, _filt_E_hz, default_filt_E_hz), // @Param: D // @DisplayName: PID Derivative Gain // @Description: D Gain which produces an output that is proportional to the rate of change of the error - AP_GROUPINFO("D", 4, AC_PID_Basic, _kd, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("D", 4, AC_PID_Basic, _kd, default_kd), // @Param: FLTD // @DisplayName: D term filter frequency in Hz // @Description: D term filter frequency in Hz // @Units: Hz - AP_GROUPINFO("FLTD", 5, AC_PID_Basic, _filt_D_hz, AC_PID_Basic_FILT_D_HZ_DEFAULT), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTD", 5, AC_PID_Basic, _filt_D_hz, default_filt_D_hz), // @Param: FF // @DisplayName: PID Feed Forward Gain // @Description: FF Gain which produces an output that is proportional to the magnitude of the target - AP_GROUPINFO("FF", 6, AC_PID_Basic, _kff, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FF", 6, AC_PID_Basic, _kff, default_kff), AP_GROUPEND }; // Constructor -AC_PID_Basic::AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz) +AC_PID_Basic::AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz) : + default_kp(initial_p), + default_ki(initial_i), + default_kd(initial_d), + default_kff(initial_ff), + default_kimax(initial_imax), + default_filt_E_hz(initial_filt_E_hz), + default_filt_D_hz(initial_filt_D_hz) { // load parameter values from eeprom AP_Param::setup_object_defaults(this, var_info); - _kp.set_and_default(initial_p); - _ki.set_and_default(initial_i); - _kd.set_and_default(initial_d); - _kff.set_and_default(initial_ff); - _kimax.set_and_default(initial_imax); - _filt_E_hz.set_and_default(initial_filt_E_hz); - _filt_D_hz.set_and_default(initial_filt_D_hz); - // reset input filter to first value received _reset_filter = true; } diff --git a/libraries/AC_PID/AC_PID_Basic.h b/libraries/AC_PID/AC_PID_Basic.h index 25d24ba485..c9342593fb 100644 --- a/libraries/AC_PID/AC_PID_Basic.h +++ b/libraries/AC_PID/AC_PID_Basic.h @@ -91,4 +91,13 @@ protected: bool _reset_filter; // true when input filter should be reset during next call to set_input AP_PIDInfo _pid_info; + +private: + const float default_kp; + const float default_ki; + const float default_kd; + const float default_kff; + const float default_kimax; + const float default_filt_E_hz; + const float default_filt_D_hz; }; diff --git a/libraries/AC_PID/AC_PI_2D.cpp b/libraries/AC_PID/AC_PI_2D.cpp index c4f9865723..e6c6362464 100644 --- a/libraries/AC_PID/AC_PI_2D.cpp +++ b/libraries/AC_PID/AC_PI_2D.cpp @@ -8,38 +8,38 @@ const AP_Param::GroupInfo AC_PI_2D::var_info[] = { // @Param: P // @DisplayName: PI Proportional Gain // @Description: P Gain which produces an output value that is proportional to the current error value - AP_GROUPINFO("P", 0, AC_PI_2D, _kp, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_PI_2D, _kp, default_kp), // @Param: I // @DisplayName: PI Integral Gain // @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error - AP_GROUPINFO("I", 1, AC_PI_2D, _ki, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("I", 1, AC_PI_2D, _ki, default_ki), // @Param: IMAX // @DisplayName: PI Integral Maximum // @Description: The maximum/minimum value that the I term can output - AP_GROUPINFO("IMAX", 2, AC_PI_2D, _imax, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("IMAX", 2, AC_PI_2D, _imax, default_imax), // @Param: FILT_HZ // @DisplayName: PI Input filter frequency in Hz // @Description: Input filter frequency in Hz // @Units: Hz - AP_GROUPINFO("FILT_HZ", 3, AC_PI_2D, _filt_hz, AC_PI_2D_FILT_HZ_DEFAULT), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FILT_HZ", 3, AC_PI_2D, _filt_hz, default_filt_hz), AP_GROUPEND }; // Constructor AC_PI_2D::AC_PI_2D(float initial_p, float initial_i, float initial_imax, float initial_filt_hz, float dt) : - _dt(dt) + _dt(dt), + default_kp(initial_p), + default_ki(initial_i), + default_imax(initial_imax), + default_filt_hz(initial_filt_hz) { // load parameter values from eeprom AP_Param::setup_object_defaults(this, var_info); - _kp.set_and_default(initial_p); - _ki.set_and_default(initial_i); - _imax.set_and_default(initial_imax); - _filt_hz.set_and_default(initial_filt_hz); filt_hz(initial_filt_hz); // reset input filter to first value received diff --git a/libraries/AC_PID/AC_PI_2D.h b/libraries/AC_PID/AC_PI_2D.h index 9e4ded2059..2020134bab 100644 --- a/libraries/AC_PID/AC_PI_2D.h +++ b/libraries/AC_PID/AC_PI_2D.h @@ -8,7 +8,6 @@ #include #include -#define AC_PI_2D_FILT_HZ_DEFAULT 20.0f // default input filter frequency #define AC_PI_2D_FILT_HZ_MIN 0.01f // minimum input filter frequency /// @class AC_PI_2D @@ -92,4 +91,10 @@ private: Vector2f _integrator; // integrator value Vector2f _input; // last input for derivative float _filt_alpha; // input filter alpha + + const float default_kp; + const float default_ki; + const float default_imax; + const float default_filt_hz; + }; diff --git a/libraries/AC_PID/AC_P_1D.cpp b/libraries/AC_PID/AC_P_1D.cpp index 69670839d5..dac669caec 100644 --- a/libraries/AC_PID/AC_P_1D.cpp +++ b/libraries/AC_PID/AC_P_1D.cpp @@ -8,17 +8,16 @@ const AP_Param::GroupInfo AC_P_1D::var_info[] = { // @Param: P // @DisplayName: P Proportional Gain // @Description: P Gain which produces an output value that is proportional to the current error value - AP_GROUPINFO("P", 0, AC_P_1D, _kp, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_P_1D, _kp, default_kp), AP_GROUPEND }; // Constructor -AC_P_1D::AC_P_1D(float initial_p) +AC_P_1D::AC_P_1D(float initial_p) : + default_kp(initial_p) { // load parameter values from eeprom AP_Param::setup_object_defaults(this, var_info); - - _kp.set_and_default(initial_p); } // update_all - set target and measured inputs to P controller and calculate outputs diff --git a/libraries/AC_PID/AC_P_1D.h b/libraries/AC_PID/AC_P_1D.h index 1cdb56e799..482e5ca27a 100644 --- a/libraries/AC_PID/AC_P_1D.h +++ b/libraries/AC_PID/AC_P_1D.h @@ -57,4 +57,6 @@ private: float _error_min; // error limit in negative direction float _error_max; // error limit in positive direction float _D1_max; // maximum first derivative of output + + const float default_kp; }; diff --git a/libraries/AC_PID/AC_P_2D.cpp b/libraries/AC_PID/AC_P_2D.cpp index 4cc2df4f63..faac93b7d0 100644 --- a/libraries/AC_PID/AC_P_2D.cpp +++ b/libraries/AC_PID/AC_P_2D.cpp @@ -8,17 +8,16 @@ const AP_Param::GroupInfo AC_P_2D::var_info[] = { // @Param: P // @DisplayName: Proportional Gain // @Description: P Gain which produces an output value that is proportional to the current error value - AP_GROUPINFO("P", 0, AC_P_2D, _kp, 0), + AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_P_2D, _kp, default_kp), AP_GROUPEND }; // Constructor -AC_P_2D::AC_P_2D(float initial_p) +AC_P_2D::AC_P_2D(float initial_p) : + default_kp(initial_p) { // load parameter values from eeprom AP_Param::setup_object_defaults(this, var_info); - - _kp.set_and_default(initial_p); } // update_all - set target and measured inputs to P controller and calculate outputs diff --git a/libraries/AC_PID/AC_P_2D.h b/libraries/AC_PID/AC_P_2D.h index d0dd792172..8d1afc6798 100644 --- a/libraries/AC_PID/AC_P_2D.h +++ b/libraries/AC_PID/AC_P_2D.h @@ -58,4 +58,6 @@ private: Vector2f _error; // time step in seconds float _error_max; // error limit in positive direction float _D1_max; // maximum first derivative of output + + const float default_kp; };