zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AC_PID: upgrade to PID object

master
Leonard Hall 6 years ago committed by Randy Mackay
parent
94ee2fb2fd
commit
ab0bdc9fe6
3 changed files with 281 additions and 152 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 287
      libraries/AC_PID/AC_PID.cpp
  2. 140
      libraries/AC_PID/AC_PID.h
  3. 6
      libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp

287
libraries/AC_PID/AC_PID.cpp
Unescape View File

@ -8,45 +8,62 @@ const AP_Param::GroupInfo AC_PID::var_info[] = {
// @Param: P // @Param: P
// @DisplayName: PID Proportional Gain // @DisplayName: PID Proportional Gain
// @Description: P Gain which produces an output value that is proportional to the current error value // @Description: P Gain which produces an output value that is proportional to the current error value
AP_GROUPINFO("P", 0, AC_PID, _kp, 0), AP_GROUPINFO("P", 0, AC_PID, _kp, 0),
// @Param: I // @Param: I
// @DisplayName: PID Integral Gain // @DisplayName: PID Integral Gain
// @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error // @Description: I Gain which produces an output that is proportional to both the magnitude and the duration of the error
AP_GROUPINFO("I", 1, AC_PID, _ki, 0), AP_GROUPINFO("I", 1, AC_PID, _ki, 0),
// @Param: D // @Param: D
// @DisplayName: PID Derivative Gain // @DisplayName: PID Derivative Gain
// @Description: D Gain which produces an output that is proportional to the rate of change of the error // @Description: D Gain which produces an output that is proportional to the rate of change of the error
AP_GROUPINFO("D", 2, AC_PID, _kd, 0), AP_GROUPINFO("D", 2, AC_PID, _kd, 0),
// 3 was for uint16 IMAX // 3 was for uint16 IMAX
// 4 is used by TradHeli for FF
// @Param: FF
// @DisplayName: FF FeedForward Gain
// @Description: FF Gain which produces an output value that is proportional to the demanded input
AP_GROUPINFO("FF", 4, AC_PID, _kff, 0),
// @Param: IMAX // @Param: IMAX
// @DisplayName: PID Integral Maximum // @DisplayName: PID Integral Maximum
// @Description: The maximum/minimum value that the I term can output // @Description: The maximum/minimum value that the I term can output
AP_GROUPINFO("IMAX", 5, AC_PID, _imax, 0), AP_GROUPINFO("IMAX", 5, AC_PID, _kimax, 0),
// 6 was for float FILT
// 7 is for float ILMI and FF
// @Param: FILT // index 8 was for AFF
// @DisplayName: PID Input filter frequency in Hz
// @Description: Input filter frequency in Hz // @Param: FLTT
// @DisplayName: PID Target filter frequency in Hz
// @Description: Target filter frequency in Hz
// @Units: Hz // @Units: Hz
AP_GROUPINFO("FILT", 6, AC_PID, _filt_hz, AC_PID_FILT_HZ_DEFAULT), AP_GROUPINFO("FLTT", 9, AC_PID, _filt_T_hz, AC_PID_TFILT_HZ_DEFAULT),
// @Param: FLTE
// @DisplayName: PID Error filter frequency in Hz
// @Description: Error filter frequency in Hz
// @Units: Hz
AP_GROUPINFO("FLTE", 10, AC_PID, _filt_E_hz, AC_PID_EFILT_HZ_DEFAULT),
// @Param: FLTD
// @DisplayName: PID Derivative term filter frequency in Hz
// @Description: Derivative filter frequency in Hz
// @Units: Hz
AP_GROUPINFO("FLTD", 11, AC_PID, _filt_D_hz, AC_PID_DFILT_HZ_DEFAULT),
// @Param: FF
// @DisplayName: FF FeedForward Gain
// @Description: FF Gain which produces an output value that is proportional to the demanded input
AP_GROUPINFO("FF", 7, AC_PID, _ff, 0),
AP_GROUPEND AP_GROUPEND
}; };
// Constructor // Constructor
AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff) : AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float dt) :
_dt(dt), _dt(dt),
_integrator(0.0f), _integrator(0.0f),
_input(0.0f), _error(0.0f),
_derivative(0.0f) _derivative(0.0f)
{ {
// load parameter values from eeprom // load parameter values from eeprom
@ -55,9 +72,11 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_
_kp = initial_p; _kp = initial_p;
_ki = initial_i; _ki = initial_i;
_kd = initial_d; _kd = initial_d;
_imax = fabsf(initial_imax); _kff = initial_ff;
filt_hz(initial_filt_hz); _kimax = fabsf(initial_imax);
_ff = initial_ff; filt_T_hz(initial_filt_T_hz);
filt_E_hz(initial_filt_E_hz);
filt_D_hz(initial_filt_D_hz);
// reset input filter to first value received // reset input filter to first value received
_flags._reset_filter = true; _flags._reset_filter = true;
@ -72,109 +91,157 @@ void AC_PID::set_dt(float dt)
_dt = dt; _dt = dt;
} }
// filt_hz - set input filter hz // filt_T_hz - set target filter hz
void AC_PID::filt_hz(float hz) void AC_PID::filt_T_hz(float hz)
{
_filt_T_hz.set(fabsf(hz));
}
// filt_E_hz - set error filter hz
void AC_PID::filt_E_hz(float hz)
{ {
_filt_hz.set(fabsf(hz)); _filt_E_hz.set(fabsf(hz));
}
// sanity check _filt_hz // filt_D_hz - set derivative filter hz
_filt_hz = MAX(_filt_hz, AC_PID_FILT_HZ_MIN); void AC_PID::filt_D_hz(float hz)
{
_filt_D_hz.set(fabsf(hz));
} }
// set_input_filter_all - set input to PID controller // update_all - set target and measured inputs to PID controller and calculate outputs
// input is filtered before the PID controllers are run // target and error are filtered
// this should be called before any other calls to get_p, get_i or get_d // the derivative is then calculated and filtered
void AC_PID::set_input_filter_all(float input) // the integral is then updated based on the setting of the limit flag
float AC_PID::update_all(float target, float measurement, bool limit)
{ {
// don't process inf or NaN // don't process inf or NaN
if (!isfinite(input)) { if (!isfinite(target) || !isfinite(measurement)) {
return; return 0.0f;
} }
// reset input filter to value received // reset input filter to value received
if (_flags._reset_filter) { if (_flags._reset_filter) {
_flags._reset_filter = false; _flags._reset_filter = false;
_input = input; _target = target;
_error = _target - measurement;
_derivative = 0.0f; _derivative = 0.0f;
} else {
float error_last = _error;
_target += get_filt_T_alpha() * (target - _target);
_error += get_filt_E_alpha() * ((_target - measurement) - _error);
// calculate and filter derivative
if (_dt > 0.0f) {
float derivative = (_error - error_last) / _dt;
_derivative += get_filt_D_alpha() * (derivative - _derivative);
}
} }
// update filter and calculate derivative // update I term
float input_filt_change = get_filt_alpha() * (input - _input); update_i(limit);
_input = _input + input_filt_change;
if (_dt > 0.0f) { float P_out = (_error * _kp);
_derivative = input_filt_change / _dt; float D_out = (_derivative * _kd);
}
_pid_info.target = _target;
_pid_info.actual = measurement;
_pid_info.error = _error;
_pid_info.P = P_out;
_pid_info.D = D_out;
return P_out + _integrator + D_out;
} }
// set_input_filter_d - set input to PID controller // update_error - set error input to PID controller and calculate outputs
// only input to the D portion of the controller is filtered // target is set to zero and error is set and filtered
// this should be called before any other calls to get_p, get_i or get_d // the derivative then is calculated and filtered
void AC_PID::set_input_filter_d(float input) // the integral is then updated based on the setting of the limit flag
// Target and Measured must be set manually for logging purposes.
// todo: remove function when it is no longer used.
float AC_PID::update_error(float error, bool limit)
{ {
// don't process inf or NaN // don't process inf or NaN
if (!isfinite(input)) { if (!isfinite(error)) {
return; return 0.0f;
} }
_target = 0.0f;
// reset input filter to value received // reset input filter to value received
if (_flags._reset_filter) { if (_flags._reset_filter) {
_flags._reset_filter = false; _flags._reset_filter = false;
_input = input; _error = error;
_derivative = 0.0f; _derivative = 0.0f;
} else {
float error_last = _error;
_error += get_filt_E_alpha() * (error - _error);
// calculate and filter derivative
if (_dt > 0.0f) {
float derivative = (_error - error_last) / _dt;
_derivative += get_filt_D_alpha() * (derivative - _derivative);
}
} }
// update filter and calculate derivative // update I term
if (_dt > 0.0f) { update_i(limit);
float derivative = (input - _input) / _dt;
_derivative = _derivative + get_filt_alpha() * (derivative-_derivative);
}
_input = input; float P_out = (_error * _kp);
} float D_out = (_derivative * _kd);
float AC_PID::get_p() _pid_info.target = 0.0f;
{ _pid_info.actual = 0.0f;
_pid_info.P = (_input * _kp); _pid_info.error = _error;
return _pid_info.P; _pid_info.P = P_out;
_pid_info.D = D_out;
return P_out + _integrator + D_out;
} }
float AC_PID::get_i() // update_i - update the integral
// If the limit flag is set the integral is only allowed to shrink
void AC_PID::update_i(bool limit)
{ {
if(!is_zero(_ki) && !is_zero(_dt)) { if (!is_zero(_ki) && is_positive(_dt)) {
_integrator += ((float)_input * _ki) * _dt; // Ensure that integrator can only be reduced if the output is saturated
if (_integrator < -_imax) { if (!limit || ((is_positive(_integrator) && is_negative(_error)) || (is_negative(_integrator) && is_positive(_error)))) {
_integrator = -_imax; _integrator += ((float)_error * _ki) * _dt;
} else if (_integrator > _imax) { _integrator = constrain_float(_integrator, -_kimax, _kimax);
_integrator = _imax;
} }
_pid_info.I = _integrator; } else {
return _integrator; _integrator = 0.0f;
} }
return 0; _pid_info.I = _integrator;
} }
float AC_PID::get_d() float AC_PID::get_p() const
{ {
// derivative component return _error * _kp;
_pid_info.D = (_kd * _derivative);
return _pid_info.D;
} }
float AC_PID::get_ff(float requested_rate) float AC_PID::get_i() const
{ {
_pid_info.FF = (float)requested_rate * _ff; return _integrator;
return _pid_info.FF;
} }
float AC_PID::get_d() const
{
return _kd * _derivative;
}
float AC_PID::get_pi() float AC_PID::get_ff()
{ {
return get_p() + get_i(); _pid_info.FF = _target * _kff;
return _target * _kff;
} }
float AC_PID::get_pid() // todo: remove function when it is no longer used.
float AC_PID::get_ff(float target)
{ {
return get_p() + get_i() + get_d(); float FF_out = (target * _kff);
_pid_info.FF = FF_out;
return FF_out;
} }
void AC_PID::reset_I() void AC_PID::reset_I()
@ -187,9 +254,12 @@ void AC_PID::load_gains()
_kp.load(); _kp.load();
_ki.load(); _ki.load();
_kd.load(); _kd.load();
_imax.load(); _kff.load();
_imax = fabsf(_imax); _kimax.load();
_filt_hz.load(); _kimax = fabsf(_kimax);
_filt_T_hz.load();
_filt_E_hz.load();
_filt_D_hz.load();
} }
// save_gains - save gains to eeprom // save_gains - save gains to eeprom
@ -198,30 +268,63 @@ void AC_PID::save_gains()
_kp.save(); _kp.save();
_ki.save(); _ki.save();
_kd.save(); _kd.save();
_imax.save(); _kff.save();
_filt_hz.save(); _kimax.save();
_filt_T_hz.save();
_filt_E_hz.save();
_filt_D_hz.save();
} }
/// Overload the function call operator to permit easy initialisation /// Overload the function call operator to permit easy initialisation
void AC_PID::operator() (float p, float i, float d, float imaxval, float input_filt_hz, float dt, float ffval) void AC_PID::operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz, float dt)
{ {
_kp = p; _kp = p_val;
_ki = i; _ki = i_val;
_kd = d; _kd = d_val;
_imax = fabsf(imaxval); _kff = ff_val;
_filt_hz = input_filt_hz; _kimax = fabsf(imax_val);
_filt_T_hz = input_filt_T_hz;
_filt_E_hz = input_filt_E_hz;
_filt_D_hz = input_filt_D_hz;
_dt = dt; _dt = dt;
_ff = ffval;
} }
// calc_filt_alpha - recalculate the input filter alpha // get_filt_T_alpha - get the target filter alpha
float AC_PID::get_filt_alpha() const float AC_PID::get_filt_T_alpha() const
{
return get_filt_alpha(_filt_T_hz);
}
// get_filt_E_alpha - get the error filter alpha
float AC_PID::get_filt_E_alpha() const
{ {
if (is_zero(_filt_hz)) { return get_filt_alpha(_filt_E_hz);
}
// get_filt_D_alpha - get the derivative filter alpha
float AC_PID::get_filt_D_alpha() const
{
return get_filt_alpha(_filt_D_hz);
}
// get_filt_alpha - calculate a filter alpha
float AC_PID::get_filt_alpha(float filt_hz) const
{
if (is_zero(filt_hz)) {
return 1.0f; return 1.0f;
} }
// calculate alpha // calculate alpha
float rc = 1/(M_2PI*_filt_hz); float rc = 1 / (M_2PI * filt_hz);
return _dt / (_dt + rc); return _dt / (_dt + rc);
} }
void AC_PID::set_integrator(float target, float measurement, float i)
{
set_integrator(target - measurement, i);
}
void AC_PID::set_integrator(float error, float i)
{
_integrator = constrain_float(i - error * _kp, -_kimax, _kimax);
}

140
libraries/AC_PID/AC_PID.h
Unescape View File

@ -9,8 +9,9 @@
#include <cmath> #include <cmath>
#include <AP_Logger/AP_Logger.h> #include <AP_Logger/AP_Logger.h>
#define AC_PID_FILT_HZ_DEFAULT 20.0f // default input filter frequency #define AC_PID_TFILT_HZ_DEFAULT 0.0f // default input filter frequency
#define AC_PID_FILT_HZ_MIN 0.01f // minimum input filter frequency #define AC_PID_EFILT_HZ_DEFAULT 0.0f // default input filter frequency
#define AC_PID_DFILT_HZ_DEFAULT 20.0f // default input filter frequency
/// @class AC_PID /// @class AC_PID
/// @brief Copter PID control class /// @brief Copter PID control class
@ -18,93 +19,118 @@ class AC_PID {
public: public:
// Constructor for PID // Constructor for PID
AC_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff = 0); AC_PID(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_T_hz, float initial_filt_E_hz, float initial_filt_D_hz, float dt);
// set_dt - set time step in seconds // set_dt - set time step in seconds
void set_dt(float dt); void set_dt(float dt);
// set_input_filter_all - set input to PID controller // update_all - set target and measured inputs to PID controller and calculate outputs
// input is filtered before the PID controllers are run // target and error are filtered
// this should be called before any other calls to get_p, get_i or get_d // the derivative is then calculated and filtered
void set_input_filter_all(float input); // the integral is then updated based on the setting of the limit flag
float update_all(float target, float measurement, bool limit = false);
// set_input_filter_d - set input to PID controller
// only input to the D portion of the controller is filtered // update_error - set error input to PID controller and calculate outputs
// this should be called before any other calls to get_p, get_i or get_d // target is set to zero and error is set and filtered
void set_input_filter_d(float input); // the derivative then is calculated and filtered
// the integral is then updated based on the setting of the limit flag
// Target and Measured must be set manually for logging purposes.
// todo: remove function when it is no longer used.
float update_error(float error, bool limit = false);
// update_i - update the integral
// if the limit flag is set the integral is only allowed to shrink
void update_i(bool limit);
// get_pid - get results from pid controller // get_pid - get results from pid controller
float get_pid(); float get_pid() const;
float get_pi(); float get_pi() const;
float get_p(); float get_p() const;
float get_i(); float get_i() const;
float get_d(); float get_d() const;
float get_ff(float requested_rate); float get_ff();
// todo: remove function when it is no longer used.
float get_ff(float target);
// reset_I - reset the integrator // reset_I - reset the integrator
void reset_I(); void reset_I();
// reset_filter - input filter will be reset to the next value provided to set_input() // reset_filter - input filter will be reset to the next value provided to set_input()
void reset_filter() { _flags._reset_filter = true; } void reset_filter() {
_flags._reset_filter = true;
}
// load gain from eeprom // load gain from eeprom
void load_gains(); void load_gains();
// save gain to eeprom // save gain to eeprom
void save_gains(); void save_gains();
/// operator function call for easy initialisation /// operator function call for easy initialisation
void operator() (float p, float i, float d, float imaxval, float input_filt_hz, float dt, float ffval = 0); void operator()(float p_val, float i_val, float d_val, float ff_val, float imax_val, float input_filt_T_hz, float input_filt_E_hz, float input_filt_D_hz, float dt);
// get accessors // get accessors
AP_Float &kP() { return _kp; } AP_Float &kP() { return _kp; }
AP_Float &kI() { return _ki; } AP_Float &kI() { return _ki; }
AP_Float &kD() { return _kd; } AP_Float &kD() { return _kd; }
AP_Float &filt_hz() { return _filt_hz; } AP_Float &ff() { return _kff;}
float imax() const { return _imax.get(); } AP_Float &filt_T_hz() { return _filt_T_hz; }
float get_filt_alpha() const; AP_Float &filt_E_hz() { return _filt_E_hz; }
float ff() const { return _ff.get(); } AP_Float &filt_D_hz() { return _filt_D_hz; }
float imax() const { return _kimax.get(); }
float get_filt_alpha(float filt_hz) const;
float get_filt_T_alpha() const;
float get_filt_E_alpha() const;
float get_filt_D_alpha() const;
// set accessors // set accessors
void kP(const float v) { _kp.set(v); } void kP(const float v) { _kp.set(v); }
void kI(const float v) { _ki.set(v); } void kI(const float v) { _ki.set(v); }
void kD(const float v) { _kd.set(v); } void kD(const float v) { _kd.set(v); }
void imax(const float v) { _imax.set(fabsf(v)); } void ff(const float v) { _kff.set(v); }
void filt_hz(const float v); void imax(const float v) { _kimax.set(fabsf(v)); }
void ff(const float v) { _ff.set(v); } void filt_T_hz(const float v);
void filt_E_hz(const float v);
float get_integrator() const { return _integrator; } void filt_D_hz(const float v);
void set_integrator(float i) { _integrator = i; }
// set the desired and actual rates (for logging purposes) // set the desired and actual rates (for logging purposes)
void set_desired_rate(float desired) { _pid_info.desired = desired; } void set_target_rate(float target) { _pid_info.target = target; }
void set_actual_rate(float actual) { _pid_info.actual = actual; } void set_actual_rate(float actual) { _pid_info.actual = actual; }
// integrator setting functions
void set_integrator(float target, float measurement, float i);
void set_integrator(float error, float i);
void set_integrator(float i) { _integrator = constrain_float(i, -_kimax, _kimax); }
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; } const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
// parameter var table // parameter var table
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
protected: protected:
// parameters // parameters
AP_Float _kp; AP_Float _kp;
AP_Float _ki; AP_Float _ki;
AP_Float _kd; AP_Float _kd;
AP_Float _imax; AP_Float _kff;
AP_Float _filt_hz; // PID Input filter frequency in Hz AP_Float _kimax;
AP_Float _ff; AP_Float _filt_T_hz; // PID target filter frequency in Hz
AP_Float _filt_E_hz; // PID error filter frequency in Hz
AP_Float _filt_D_hz; // PID derivative filter frequency in Hz
// flags // flags
struct ac_pid_flags { struct ac_pid_flags {
bool _reset_filter : 1; // true when input filter should be reset during next call to set_input bool _reset_filter :1; // true when input filter should be reset during next call to set_input
} _flags; } _flags;
// internal variables // internal variables
float _dt; // timestep in seconds float _dt; // timestep in seconds
float _integrator; // integrator value float _integrator; // integrator value
float _input; // last input for derivative float _target; // target value to enable filtering
float _derivative; // last derivative for low-pass filter float _error; // error value to enable filtering
float _derivative; // derivative value to enable filtering
AP_Logger::PID_Info _pid_info; AP_Logger::PID_Info _pid_info;
}; };

6
libraries/AC_PID/examples/AC_PID_test/AC_PID_test.cpp
Unescape View File

@ -70,8 +70,8 @@ void setup()
void loop() void loop()
{ {
// setup (unfortunately must be done here as we cannot create a global AC_PID object) // setup (unfortunately must be done here as we cannot create a global AC_PID object)
AC_PID pid(TEST_P, TEST_I, TEST_D, TEST_IMAX * 100, TEST_FILTER, TEST_DT); AC_PID pid(TEST_P, TEST_I, TEST_D, 0.0f, TEST_IMAX * 100.0f, 0.0f, 0.0f, TEST_FILTER, TEST_DT);
AC_HELI_PID heli_pid(TEST_P, TEST_I, TEST_D, TEST_IMAX * 100, TEST_FILTER, TEST_DT, TEST_INITIAL_FF); AC_HELI_PID heli_pid(TEST_P, TEST_I, TEST_D, TEST_INITIAL_FF, TEST_IMAX * 100, 0.0f, 0.0f, TEST_FILTER, TEST_DT);
// display PID gains // display PID gains
hal.console->printf("P %f I %f D %f imax %f\n", (double)pid.kP(), (double)pid.kI(), (double)pid.kD(), (double)pid.imax()); hal.console->printf("P %f I %f D %f imax %f\n", (double)pid.kP(), (double)pid.kI(), (double)pid.kD(), (double)pid.imax());
@ -91,7 +91,7 @@ void loop()
rc().read_input(); // poll the radio for new values rc().read_input(); // poll the radio for new values
const uint16_t radio_in = c->get_radio_in(); const uint16_t radio_in = c->get_radio_in();
const int16_t error = radio_in - radio_trim; const int16_t error = radio_in - radio_trim;
pid.set_input_filter_all(error); pid.update_error(error);
const float control_P = pid.get_p(); const float control_P = pid.get_p();
const float control_I = pid.get_i(); const float control_I = pid.get_i();
const float control_D = pid.get_d(); const float control_D = pid.get_d();

Write Preview
Loading…
Cancel
Save