diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index 8eb65da747..7b50998d9f 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -8,6 +8,12 @@ #include "PID.h" +// make the gain array members a little easier to identify +#define _kp _gain_array[0] +#define _ki _gain_array[1] +#define _kd _gain_array[2] +#define _imax _gain_array[3] + long PID::get_pid(int32_t error, uint16_t dt, float scaler) { @@ -60,32 +66,24 @@ PID::imax(float v) void PID::load_gains() { - if (_address) { + if ((_gain_array == &_local_gains[0]) && (_address)) { _kp = (float)(eeprom_read_word((uint16_t *) _address)) / 1000.0; _ki = (float)(eeprom_read_word((uint16_t *) (_address + 2))) / 1000.0; _kd = (float)(eeprom_read_word((uint16_t *) (_address + 4))) / 1000.0; _imax = eeprom_read_word((uint16_t *) (_address + 6)) * 100; - } else { - _kp = _gain_array[0]; - _ki = _gain_array[1]; - _kd = _gain_array[2]; - _imax = _gain_array[3]; } + // load is a NOP if the gain array is managed externally } void PID::save_gains() { - if (_address) { + if ((_gain_array == &_local_gains[0]) && (_address)) { eeprom_write_word((uint16_t *) _address, (int)(_kp * 1000)); eeprom_write_word((uint16_t *) (_address + 2), (int)(_ki * 1000)); eeprom_write_word((uint16_t *) (_address + 4), (int)(_kd * 1000)); eeprom_write_word((uint16_t *) (_address + 6), (int)_imax/100); - } else { - _gain_array[0] = _kp; - _gain_array[1] = _ki; - _gain_array[2] = _kd; - _gain_array[3] = _imax; } + // save is a NOP if the gain array is managed externally } diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h index f2d4ac8acc..264ec88e8f 100644 --- a/libraries/PID/PID.h +++ b/libraries/PID/PID.h @@ -15,30 +15,37 @@ public: /// Constructor /// /// A PID constructed in this fashion does not support save/restore. + /// Gains are managed internally, and must be read/written using the + /// accessor functions. /// PID() : - _address(0) + _address(0), + _gain_array(&_local_gains[0]) {} /// Constructor /// + /// The PID will manage gains internally, and the load/save functions + /// will use 16 bytes of EEPROM storage to store gain values. + /// /// @param address EEPROM base address at which PID parameters - /// are stored. Zero if the PID does not support - /// save/restore. + /// are stored. /// PID(uint16_t address) : - _address(address) + _address(address), + _gain_array(&_local_gains[0]) {} /// Constructor /// - /// @param gain_array Address of an array of floats from which - /// gains will be loaded and to which they - /// are saved. + /// Gain values for the PID are managed externally; load/save are a NOP. + /// + /// @param gain_array Address of an array of float values. The + /// array is used as kP, kI, kD and imax + /// respectively. /// PID(float *gain_array) : - _gain_array(gain_array), - _address(0) // must init address to zero to have gain_array respected + _gain_array(gain_array) {} /// Iterate the PID, return the new control value @@ -74,29 +81,30 @@ public: /// @name parameter accessors //@{ - float kP() { return _kp; } - float kI() { return _ki; } - float kD() { return _kd; } - float imax() { return _imax; } + float kP() { return _gain_array[0]; } + float kI() { return _gain_array[1]; } + float kD() { return _gain_array[2]; } + float imax() { return _gain_array[3]; } - void kP(float v) { _kp = v; } - void kI(float v) { _ki = v; } - void kD(float v) { _kd = v; } - void imax(float v); + void kP(const float v) { _gain_array[0] = v; } + void kI(const float v) { _gain_array[1] = v; } + void kD(const float v) { _gain_array[2] = v; } + void imax(const float v); + + // one-shot operator for setting all of the gain properties at once + void operator ()(const float p, const float i, const float d, const float max) + { kP(p); kI(i); kD(d); imax(max); } //@} private: - uint16_t _address; ///< EEPROM address for save/restore of P/I/D - float *_gain_array; ///< pointer to the gains for this pid + uint16_t _address; ///< EEPROM address for save/restore of P/I/D + float *_gain_array; ///< pointer to the gains for this pid - float _kp; ///< proportional gain - float _ki; ///< integral gain - float _kd; ///< derivative gain - float _imax; ///< integrator magnitude clamp + float _local_gains[4]; ///< local storage for gains when not globally managed - float _integrator; ///< integrator value - int32_t _last_error; ///< last error for derivative - float _last_derivative; ///< last derivative for low-pass filter + 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. ///