diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index 4f2c03f646..4fe529256f 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -1,45 +1,41 @@ +// -*- 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. + #include "PID.h" -PID::PID(float * _gain_array) : - gain_array(_gain_array) -{ -} - long -PID::get_pid(long err, long dt, float scaler) +PID::get_pid(long error, long dt, float scaler) { - // Positive error produces positive output - float output; - float error = (float)err; - float delta_time = (float)dt/1000.0; - - // Compute proportional component - if(_kp != 0){ - output += (_kp * error); - } + float output = 0; + float delta_time = (float)dt / 1000; - if(_kd != 0 && dt > 0){ - // Compute derivative component - //derivative = (error - previous_error)/delta_time - float derivative = (error - _last_error) / delta_time; + // Compute proportional component + output += error * _kp; + + // Compute derivative component if time has elapsed + if (_kd && (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 - derivative = _last_derivative + delta_time/ - (RC + delta_time)*(derivative - _last_derivative); - - //Serial.print("d: "); - //Serial.println(derivative,DEC); + derivative = _last_derivative + + ((delta_time / (RC + delta_time)) * (derivative - _last_derivative)); + // update state _last_error = error; _last_derivative = derivative; - output += _kd * derivative; // Sum the derivative component + + // add in derivative component + output += _kd * derivative; } - + + // scale the P and D components output *= scaler; - // Compute integral component - if(_ki != 0 && dt > 0){ + // Compute integral component if time has elapsed + if (_ki && (dt > 0)) { _integrator += (error * _ki) * scaler * delta_time; _integrator = constrain(_integrator, -_imax, _imax); output += _integrator; @@ -48,7 +44,6 @@ PID::get_pid(long err, long dt, float scaler) return output; } - void PID::reset_I(void) { @@ -57,155 +52,35 @@ PID::reset_I(void) _last_error_derivative = 0; } - - -/// setters -void -PID::set_P(float p) -{ - _kp = p; -} - -void -PID::set_I(float i) -{ - _ki = i; -} - -void -PID::set_D(float d) -{ - _kd = d; -} - -void -PID::set_imax(int imax) -{ - _imax = imax; - Serial.print("set imax "); - Serial.println(_imax, DEC); -} - -/// getters -float -PID::get_P(void) -{ - return _kp; -} - -float -PID::get_I(void) -{ - return _ki; -} - -float -PID::get_D(void) -{ - return _kd; -} - -int -PID::get_imax(void) -{ - return _imax; -} - - -void -PID::load_gains(int address) -{ - //Serial.println("load gains "); - //Serial.println(address, DEC); - _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; -} - -void -PID::save_gains(int 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); -} - void PID::load_gains() { - _kp = gain_array[0]/ 1000.0; - _ki = gain_array[1]/ 1000.0; - _kd = gain_array[2]/ 1000.0; - _imax = gain_array[3]/ 1000.0; + if (_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]/ 1000.0; + _ki = _gain_array[1]/ 1000.0; + _kd = _gain_array[2]/ 1000.0; + _imax = _gain_array[3]/ 1000.0; + } } void PID::save_gains() { - gain_array[0] = _kp * 1000; - gain_array[1] = _ki * 1000; - gain_array[2] = _kd * 1000; - gain_array[3] = _imax * 1000; + if (_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 * 1000; + _gain_array[1] = _ki * 1000; + _gain_array[2] = _kd * 1000; + _gain_array[3] = _imax * 1000; + } } -/* -float -read_EE_compressed_float(int address, byte places) -{ - float scale = places * 10; - int temp = eeprom_read_word((uint16_t *) address); - return ((float)temp / scale); -} - -void write_EE_compressed_float(float value, int address, byte places) -{ - float scale = places * 10; - int temp = value * scale; - eeprom_write_word((uint16_t *) address, temp); -} -*/ - -void -PID::test(void) -{ - /* - int address = 0x46; - Serial.print("imax= "); - Serial.print(eeprom_read_word((uint16_t *) address)); - Serial.print(" "); - address += 8; - Serial.print(eeprom_read_word((uint16_t *) address)); - Serial.print(" "); - address += 8; - Serial.print(eeprom_read_word((uint16_t *) address)); - Serial.print(" "); - address += 8; - Serial.print(eeprom_read_word((uint16_t *) address)); - Serial.print(" "); - address += 8; - Serial.print(eeprom_read_word((uint16_t *) address)); - Serial.print(" "); - address += 8; - Serial.print(eeprom_read_word((uint16_t *) address)); - Serial.print(" "); - address += 8; - Serial.print(eeprom_read_word((uint16_t *) address)); - Serial.print(" "); - address += 8; - Serial.print(eeprom_read_word((uint16_t *) address)); - Serial.print(" "); - address += 8; - Serial.print(eeprom_read_word((uint16_t *) address)); - Serial.print(" "); - address += 8; - Serial.print(eeprom_read_word((uint16_t *) address)); - Serial.print(" "); - address += 8; - Serial.print(eeprom_read_word((uint16_t *) address)); - Serial.print(" "); - address += 8; - Serial.println(eeprom_read_word((uint16_t *) address)); - */ -} diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h index 243d0f761f..8ccf949885 100644 --- a/libraries/PID/PID.h +++ b/libraries/PID/PID.h @@ -1,52 +1,100 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file PID.h +/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. + #ifndef PID_h #define PID_h +#include #include -#include "WProgram.h" - -//#define PID_SIZE 8 +/// @class PID +/// @brief Object managing one PID control class PID { public: - // note, if using the eeprom load_gains/save_gains functions - // it is not necessary to pass a gain_array pointer, instead - // you need to pass address to load_gains/ save_gains functions - PID(float * gain_array = 0); - long get_pid(long err, long dt, float scaler); - void reset_I(); - void load_gains(int address); - void save_gains(int address); - void load_gains(); - void save_gains(); - void set_P(float p); - void set_I(float i); - void set_D(float d); - void set_imax(int imax); - void test(void); + /// Constructor + /// + /// @param address EEPROM base address at which PID parameters + /// are stored. Zero if the PID does not support + /// save/restore. + /// + PID(uint16_t address = 0) : + _gain_array(0), + _address(address), + _integrator(0), + _last_error(0), + _last_error_derivative(0) + {} - float get_P(void); - float get_I(void); - float get_D(void); - int get_imax(void); - float _imax; // integrator max - float _integrator; // integrator value - float _last_error; // last error for derivative - float _kp; - float _ki; - float _kd; - + /// Constructor + /// + /// @param gain_array Address of an array of floats from which + /// gains will be loaded and to which they + /// are saved. + /// + PID(float *gain_array) : + _gain_array(gain_array), + _address(0), + _integrator(0), + _last_error(0), + _last_error_derivative(0) + {} + + /// Iterate the PID, return the new control value + /// + /// Positive error produces positive output. + /// + /// @param err The measured error value + /// @param dt The time delta in milliseconds + /// @param scaler An arbitrary scale factor + /// + /// @returns The updated control output. + /// + long get_pid(long err, long dt, float scaler); + + /// Reset the PID integrator + /// + void reset_I() { _integrator = 0; _last_error = 0; } + + /// Load gain properties + /// + void load_gains(); + + /// Save gain properties + /// + void save_gains(); + + /// @name parameter accessors + //@{ + float kP() { return _kp; } + float &kP() { return _kp; } + float kI() { return _ki; } + float &kI() { return _kd; } + float kD() { return _kd; } + float &kD() { return _kd; } + float imax() { return _imax; } + float &imax() { return _imax; } + //@} private: - // low pass filter cut frequency - // for derivative calculation, - // set to 20 Hz becasue anything over that - // is probably noise, see - // http://en.wikipedia.org/wiki/Low-pass_filter - static uint8_t RC = 20; - - // pointer to the gains for this pid, - // if not using eeprom save/load - float * gain_array; + 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 _integrator; ///< integrator value + int32_t _last_error; ///< last error for derivative + + /// low pass filter cut frequency + /// for derivative calculation, + /// set to 20 Hz becasue anything over that + /// is probably noise, see + /// http://en.wikipedia.org/wiki/Low-pass_filter + static uint8_t _RC = 20; }; #endif diff --git a/libraries/PID/examples/pid/pid.pde b/libraries/PID/examples/pid/pid.pde index ed1d1fe3a4..249c05d653 100644 --- a/libraries/PID/examples/pid/pid.pde +++ b/libraries/PID/examples/pid/pid.pde @@ -20,10 +20,10 @@ void setup() //rc.trim(); radio_trim = APM_RC.InputCh(0); - pid.set_P(1); - pid.set_I(0); - pid.set_D(.5); - pid.set_imax(50); + pid.kP() = 1; + pid.kI() = 0; + pid.kD() = 0.5; + pid.imax() = 50; } void loop() diff --git a/libraries/PID/keywords.txt b/libraries/PID/keywords.txt index 3efcc0fa52..68e861fd00 100644 --- a/libraries/PID/keywords.txt +++ b/libraries/PID/keywords.txt @@ -1,8 +1,9 @@ -APM_RC KEYWORD1 -begin KEYWORD2 -InputCh KEYWORD2 -OutputCh KEYWORD2 -GetState KEYWORD2 -Force_Out0_Out1 KEYWORD2 -Force_Out2_Out3 KEYWORD2 -Force_Out6_Out7 KEYWORD2 +PID KEYWORD1 +get_pid KEYWORD2 +reset_I KEYWORD2 +kP KEYWORD2 +kD KEYWORD2 +kI KEYWORD2 +imax KEYWORD2 +load_gains KEYWORD2 +save_gains KEYWORD2