From 2ebcc08b063f1d099bec87b43066dfb2c62b6e8f Mon Sep 17 00:00:00 2001 From: jasonshort Date: Mon, 5 Sep 2011 00:56:50 +0000 Subject: [PATCH] Adding ACM_PI git-svn-id: https://arducopter.googlecode.com/svn/trunk@3253 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/ACM_PI/APM_PI.cpp | 49 +++++++++++++++ libraries/ACM_PI/APM_PI.h | 122 ++++++++++++++++++++++++++++++++++++ 2 files changed, 171 insertions(+) create mode 100644 libraries/ACM_PI/APM_PI.cpp create mode 100644 libraries/ACM_PI/APM_PI.h diff --git a/libraries/ACM_PI/APM_PI.cpp b/libraries/ACM_PI/APM_PI.cpp new file mode 100644 index 0000000000..fbb8f01ed5 --- /dev/null +++ b/libraries/ACM_PI/APM_PI.cpp @@ -0,0 +1,49 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file ACM_PI.cpp +/// @brief Generic PI algorithm + +#include + +#include "APM_PI.h" + +long +APM_PI::get_pi(int32_t error, uint16_t dt) +{ + float output = 0; + float delta_time = (float)dt / 1000.0; + + // Compute proportional component + output += error * _kp; + + // Compute integral component if time has elapsed + _integrator += (error * _ki) * delta_time; + + if (_integrator < -_imax) { + _integrator = -_imax; + } else if (_integrator > _imax) { + _integrator = _imax; + } + + output += _integrator; + + return output; +} + +void +APM_PI::reset_I() +{ + _integrator = 0; +} + +void +APM_PI::load_gains() +{ + _group.load(); +} + +void +APM_PI::save_gains() +{ + _group.save(); +} diff --git a/libraries/ACM_PI/APM_PI.h b/libraries/ACM_PI/APM_PI.h new file mode 100644 index 0000000000..6701c4781d --- /dev/null +++ b/libraries/ACM_PI/APM_PI.h @@ -0,0 +1,122 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file PI.h +/// @brief Generic PI algorithm, with EEPROM-backed storage of constants. + +#ifndef APM_PI_h +#define APM_PI_h + +#include +//#include // for fabs() + +/// @class APM_PI +/// @brief Object managing one PI control +class APM_PI { +public: + + /// Constructor for PI that saves its settings to EEPROM + /// + /// @note PI must be named to avoid either multiple parameters with the + /// same name, or an overly complex constructor. + /// + /// @param key Storage key assigned to this PI. Should be unique. + /// @param name Name by which the PI is known, or NULL for an anonymous PI. + /// The name is prefixed to the P, I, 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_imax Initial value for the imax term.4 + /// + APM_PI(AP_Var::Key key, + const prog_char_t *name, + const float &initial_p = 0.0, + const float &initial_i = 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")), + _imax(&_group, 3, initial_imax, PSTR("IMAX")) + { + // no need for explicit load, assuming that the main code uses AP_Var::load_all. + } + + /// Constructor for PI that does not save its settings. + /// + /// @param name Name by which the PI is known, or NULL for an anonymous PI. + /// The name is prefixed to the P, I, 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_imax Initial value for the imax term.4 + /// + APM_PI(const prog_char_t *name, + const float &initial_p = 0.0, + const float &initial_i = 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")), + _imax(&_group, 3, initial_imax, PSTR("IMAX")) + { + } + + /// Iterate the PI, return the new control value + /// + /// Positive error produces positive output. + /// + /// @param error The measured error value + /// @param dt The time delta in milliseconds (note + /// that update interval cannot be more + /// than 65.535 seconds due to limited range + /// of the data type). + /// @param scaler An arbitrary scale factor + /// + /// @returns The updated control output. + /// + long get_pi(int32_t error, uint16_t dt); + + /// Reset the PI integrator + /// + void reset_I(); + + /// Load gain properties + /// + void load_gains(); + + /// Save gain properties + /// + void save_gains(); + + /// @name parameter accessors + //@{ + + /// Overload the function call operator to permit relatively easy initialisation + void operator() (const float p, + const float i, + const int16_t imaxval) { + _kp = p; _ki = i; _imax = imaxval; + } + + float kP() const { return _kp.get(); } + float kI() const { return _ki.get(); } + int16_t imax() const { return _imax.get(); } + + void kP(const float v) { _kp.set(v); } + void kI(const float v) { _ki.set(v); } + void imax(const int16_t v) { _imax.set(abs(v)); } + float get_integrator() const { return _integrator; } + +private: + AP_Var_group _group; + AP_Float16 _kp; + AP_Float16 _ki; + AP_Int16 _imax; + + float _integrator; ///< integrator value +}; + +#endif