diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index a44d5f3e13..67922cb75d 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -7,6 +7,14 @@ #include "PID.h" +const AP_Param::GroupInfo PID::var_info[] PROGMEM = { + { AP_PARAM_FLOAT, "P", VAROFFSET(PID, _kp) }, + { AP_PARAM_FLOAT, "I", VAROFFSET(PID, _ki) }, + { AP_PARAM_FLOAT, "D", VAROFFSET(PID, _kd) }, + { AP_PARAM_INT16, "IMAX", VAROFFSET(PID, _imax) }, + { AP_PARAM_NONE, "" } +}; + long PID::get_pid(int32_t error, uint16_t dt, float scaler) { @@ -22,7 +30,7 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler) // discrete low pass filter, cuts out the // high frequency noise that can drive the controller crazy - float RC = 1/(2*M_PI*_fCut); + float RC = 1/(2*M_PI*20); derivative = _last_derivative + (delta_time / (RC + delta_time)) * (derivative - _last_derivative); @@ -62,11 +70,15 @@ PID::reset_I() void PID::load_gains() { - _group.load(); + _kp.load(); + _ki.load(); + _kd.load(); } void PID::save_gains() { - _group.save(); + _kp.save(); + _ki.save(); + _kd.save(); } diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h index 6cf9919688..543fff9c4e 100644 --- a/libraries/PID/PID.h +++ b/libraries/PID/PID.h @@ -14,59 +14,10 @@ class PID { public: - /// Constructor for PID that saves its settings to EEPROM - /// - /// @note PIDs must be named to avoid either multiple parameters with the - /// same name, or an overly complex constructor. - /// - /// @param key Storage key assigned to this PID. Should be unique. - /// @param name Name by which the PID is known, or NULL for an anonymous PID. - /// The name is prefixed to the P, I, D, IMAX variable names when - /// they are reported. - /// @param initial_p Initial value for the P term. - /// @param initial_i Initial value for the I term. - /// @param initial_d Initial value for the D term. - /// @param initial_imax Initial value for the imax term.4 - /// - PID(AP_Var::Key key, - const prog_char_t *name, - const float &initial_p = 0.0, - const float &initial_i = 0.0, - const float &initial_d = 0.0, - const int16_t &initial_imax = 0.0) : - - _group(key, name), - // group, index, initial value, name - _kp (&_group, 0, initial_p, PSTR("P")), - _ki (&_group, 1, initial_i, PSTR("I")), - _kd (&_group, 2, initial_d, PSTR("D")), - _imax(&_group, 3, initial_imax, PSTR("IMAX")) - { - // no need for explicit load, assuming that the main code uses AP_Var::load_all. - } - - /// Constructor for PID that does not save its settings. - /// - /// @param name Name by which the PID is known, or NULL for an anonymous PID. - /// The name is prefixed to the P, I, D, IMAX variable names when - /// they are reported. - /// @param initial_p Initial value for the P term. - /// @param initial_i Initial value for the I term. - /// @param initial_d Initial value for the D term. - /// @param initial_imax Initial value for the imax term.4 - /// - PID(const prog_char_t *name, - const float &initial_p = 0.0, + PID(const float &initial_p = 0.0, const float &initial_i = 0.0, const float &initial_d = 0.0, - const int16_t &initial_imax = 0.0) : - - _group(AP_Var::k_key_none, name), - // group, index, initial value, name - _kp (&_group, 0, initial_p, PSTR("P")), - _ki (&_group, 1, initial_i, PSTR("I")), - _kd (&_group, 2, initial_d, PSTR("D")), - _imax(&_group, 3, initial_imax, PSTR("IMAX")) + const int16_t &initial_imax = 0.0) { } @@ -120,23 +71,17 @@ public: float get_integrator() const { return _integrator; } + static const struct AP_Param::GroupInfo var_info[]; + private: - AP_Var_group _group; - AP_Float16 _kp; - AP_Float16 _ki; - AP_Float16 _kd; + AP_Float _kp; + AP_Float _ki; + AP_Float _kd; AP_Int16 _imax; float _integrator; ///< integrator value int32_t _last_error; ///< last error for derivative float _last_derivative; ///< last derivative for low-pass filter - - /// Low pass filter cut frequency for derivative calculation. - /// - /// 20 Hz becasue anything over that is probably noise, see - /// http://en.wikipedia.org/wiki/Low-pass_filter. - /// - static const uint8_t _fCut = 20; }; #endif