diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index 5e3f330949..a44d5f3e13 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -1,19 +1,12 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /// @file PID.cpp -/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. +/// @brief Generic PID algorithm #include -#include #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) { @@ -24,14 +17,14 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler) output += error * _kp; // Compute derivative component if time has elapsed - if (_kd && (dt > 0)) { + if ((fabs(_kd) > 0) && (dt > 0)) { float derivative = (error - _last_error) / delta_time; // discrete low pass filter, cuts out the // high frequency noise that can drive the controller crazy float RC = 1/(2*M_PI*_fCut); derivative = _last_derivative + - (delta_time / (RC + delta_time)) * (derivative - _last_derivative); + (delta_time / (RC + delta_time)) * (derivative - _last_derivative); // update state _last_error = error; @@ -45,7 +38,7 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler) output *= scaler; // Compute integral component if time has elapsed - if (_ki && (dt > 0)) { + if ((fabs(_ki) > 0) && (dt > 0)) { _integrator += (error * _ki) * scaler * delta_time; if (_integrator < -_imax) { _integrator = -_imax; @@ -58,63 +51,22 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler) return output; } +void +PID::reset_I() +{ + _integrator = 0; + _last_error = 0; + _last_derivative = 0; +} void PID::load_gains() { - switch(_storage) - { - case STORE_OFF: - // load is a NOP if the gain array is managed externally - break; - case STORE_EEPROM_UINT16: - _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 = (float)(eeprom_read_word((uint16_t *) (_address + 6))); - - //_kp = (float)_ee.read_int(_address) / 1000.0; - //_ki = (float)_ee.read_int(_address + 2) / 1000.0; - //_kd = (float)_ee.read_int(_address + 4) / 1000.0; - //_imax = (float)_ee.read_int(_address + 6) * 100; - - break; - - case STORE_EEPROM_FLOAT: - eeprom_read_block((void*)&_kp,(const void*)(_address),sizeof(_kp)); - eeprom_read_block((void*)&_ki,(const void*)(_address+4),sizeof(_ki)); - eeprom_read_block((void*)&_kd,(const void*)(_address+8),sizeof(_kd)); - eeprom_read_block((void*)&_imax,(const void*)(_address+12),sizeof(_imax)); - break; - } + _group.load(); } void PID::save_gains() { - switch(_storage) - { - case STORE_OFF: - // save is a NOP if the gain array is managed externally - break; - case STORE_EEPROM_UINT16: - eeprom_write_word((uint16_t *) _address, (int)(_kp * 1000.0)); - eeprom_write_word((uint16_t *) (_address + 2), (int)(_ki * 1000.0)); - eeprom_write_word((uint16_t *) (_address + 4), (int)(_kd * 1000.0)); - eeprom_write_word((uint16_t *) (_address + 6), (int)(_imax)); - - /*_ee.write_int(_address, (int)(_kp * 1000)); - _ee.write_int(_address + 2, (int)(_ki * 1000)); - _ee.write_int(_address + 4, (int)(_kd * 1000)); - _ee.write_int(_address + 6, (int)(_imax /100)); - */ - break; - case STORE_EEPROM_FLOAT: - eeprom_write_block((const void *)&_kp,(void *)(_address),sizeof(_kp)); - eeprom_write_block((const void *)&_ki,(void *)(_address+4),sizeof(_ki)); - eeprom_write_block((const void *)&_kd,(void *)(_address+8),sizeof(_kd)); - eeprom_write_block((const void *)&_imax,(void *)(_address+12),sizeof(_imax)); - break; - } + _group.save(); } - diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h index e160069a43..02aa9b688b 100644 --- a/libraries/PID/PID.h +++ b/libraries/PID/PID.h @@ -6,62 +6,68 @@ #ifndef PID_h #define PID_h -#include +#include +#include // for fabs() /// @class PID /// @brief Object managing one PID control class PID { public: - /// - // EEProm Storage Type - enum storage_t - { - STORE_OFF, - STORE_EEPROM_FLOAT, - STORE_EEPROM_UINT16 - } _storage; - /// Constructor + /// Constructor for PID that saves its settings to EEPROM /// - /// A PID constructed in this fashion does not support save/restore. - /// Gains are managed internally, and must be read/written using the - /// accessor functions. + /// @note PIDs must be named to avoid either multiple parameters with the + /// same name, or an overly complex constructor. /// - PID() : - _storage(STORE_OFF), - _address(0), - _gain_array(&_local_gains[0]) - {} + /// @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 *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) : - /// 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. - /// - PID(uint16_t address, storage_t storage = STORE_EEPROM_UINT16) : - _storage(storage), - _address(address), - _gain_array(&_local_gains[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")) { - load_gains(); + // no need for explicit load, assuming that the main code uses AP_Var::load_all. } - /// Constructor + /// Constructor for PID that does not save its settings. /// - /// Gain values for the PID are managed externally; load/save are a NOP. + /// @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 /// - /// @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) : - _storage(STORE_OFF), - _gain_array(gain_array) - { + PID(const prog_char *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(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")) + { } /// Iterate the PID, return the new control value @@ -76,16 +82,12 @@ public: /// @param scaler An arbitrary scale factor /// /// @returns The updated control output. - /// + /// long get_pid(int32_t error, uint16_t dt, float scaler = 1.0); /// Reset the PID integrator /// - void reset_I() { - _integrator = 0; - _last_error = 0; - _last_derivative = 0; - } + void reset_I(); /// Load gain properties /// @@ -97,41 +99,44 @@ public: /// @name parameter accessors //@{ - 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(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) { _gain_array[3] = fabs(v); } - - void address(const uint16_t v) { _address = 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); } - //@} - - float get_integrator() { return _integrator; } + /// Overload the function call operator to permit relatively easy initialisation + void operator() (const float p, + const float i, + const float d, + const int16_t imax) { + _kp = p; _ki = i; _kd = d; _imax = imax; + } + + float kP() const { return _kp.get(); } + float kI() const { return _ki.get(); } + float kD() const { return _kd.get(); } + int16_t imax() const { return _imax.get(); } + + void kP(const float v) { _kp = v; } + void kI(const float v) { _ki = v; } + void kD(const float v) { _kd = v; } + void imax(const int16_t v) { _imax = abs(v); } + + float get_integrator() const { return _integrator; } private: - uint16_t _address; ///< EEPROM address for save/restore of P/I/D - float *_gain_array; ///< pointer to the gains for this pid + AP_Var_group _group; + AP_Float16 _kp; + AP_Float16 _ki; + AP_Float16 _kd; + AP_Int16 _imax; - 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. /// /// 20 Hz becasue anything over that is probably noise, see /// http://en.wikipedia.org/wiki/Low-pass_filter. /// - static const uint8_t _fCut = 20; + static const uint8_t _fCut = 20; }; #endif diff --git a/libraries/PID/examples/pid/pid.pde b/libraries/PID/examples/pid/pid.pde index 1d041fe3e3..148f9644b8 100644 --- a/libraries/PID/examples/pid/pid.pde +++ b/libraries/PID/examples/pid/pid.pde @@ -3,13 +3,15 @@ 2010 Code by Jason Short. DIYDrones.com */ -#include // ArduPilot Mega RC Library +#include +#include #include // ArduPilot RC Library +#include // ArduPilot Mega RC Library long radio_in; long radio_trim; -PID pid(0x16); +PID pid(10, "TEST1_"); void setup() { @@ -25,6 +27,13 @@ void setup() pid.kI(0); pid.kD(0.5); pid.imax(50); + pid.save_gains(); + pid.kP(0); + pid.kI(0); + pid.kD(0); + pid.imax(0); + pid.load_gains(); + Serial.printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax()); } void loop() @@ -36,4 +45,4 @@ void loop() Serial.print("control: "); Serial.println(control,DEC); -} \ No newline at end of file +}