mirror of https://github.com/ArduPilot/ardupilot
AC_PID: upgrade to PID object
This commit is contained in:
Leonard Hall
Randy Mackay
parent
94ee2fb2fd
commit
ab0bdc9fe6
|
|
@ -8,45 +8,62 @@ const AP_Param::GroupInfo AC_PID::var_info[] = {
|
|||
// @Param: P
|
||||
// @DisplayName: PID Proportional Gain
|
||||
// @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
|
||||
// @DisplayName: PID Integral Gain
|
||||
// @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
|
||||
// @DisplayName: PID Derivative Gain
|
||||
// @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
|
||||
// 4 is used by TradHeli for FF
|
||||
|
||||
// @Param: IMAX
|
||||
// @DisplayName: PID Integral Maximum
|
||||
// @Description: The maximum/minimum value that the I term can output
|
||||
AP_GROUPINFO("IMAX", 5, AC_PID, _imax, 0),
|
||||
|
||||
// @Param: FILT
|
||||
// @DisplayName: PID Input filter frequency in Hz
|
||||
// @Description: Input filter frequency in Hz
|
||||
// @Units: Hz
|
||||
AP_GROUPINFO("FILT", 6, AC_PID, _filt_hz, AC_PID_FILT_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_GROUPINFO("FF", 4, AC_PID, _kff, 0),
|
||||
|
||||
// @Param: IMAX
|
||||
// @DisplayName: PID Integral Maximum
|
||||
// @Description: The maximum/minimum value that the I term can output
|
||||
AP_GROUPINFO("IMAX", 5, AC_PID, _kimax, 0),
|
||||
|
||||
// 6 was for float FILT
|
||||
|
||||
// 7 is for float ILMI and FF
|
||||
|
||||
// index 8 was for AFF
|
||||
|
||||
// @Param: FLTT
|
||||
// @DisplayName: PID Target filter frequency in Hz
|
||||
// @Description: Target filter frequency in Hz
|
||||
// @Units: Hz
|
||||
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),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
// 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),
|
||||
_integrator(0.0f),
|
||||
_input(0.0f),
|
||||
_error(0.0f),
|
||||
_derivative(0.0f)
|
||||
{
|
||||
// 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;
|
||||
_ki = initial_i;
|
||||
_kd = initial_d;
|
||||
_imax = fabsf(initial_imax);
|
||||
filt_hz(initial_filt_hz);
|
||||
_ff = initial_ff;
|
||||
_kff = initial_ff;
|
||||
_kimax = fabsf(initial_imax);
|
||||
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
|
||||
_flags._reset_filter = true;
|
||||
|
|
@ -72,109 +91,157 @@ void AC_PID::set_dt(float dt)
|
|||
_dt = dt;
|
||||
}
|
||||
|
||||
// filt_hz - set input filter hz
|
||||
void AC_PID::filt_hz(float hz)
|
||||
// filt_T_hz - set target filter hz
|
||||
void AC_PID::filt_T_hz(float hz)
|
||||
{
|
||||
_filt_hz.set(fabsf(hz));
|
||||
|
||||
// sanity check _filt_hz
|
||||
_filt_hz = MAX(_filt_hz, AC_PID_FILT_HZ_MIN);
|
||||
_filt_T_hz.set(fabsf(hz));
|
||||
}
|
||||
|
||||
// set_input_filter_all - set input to PID controller
|
||||
// input is filtered before the PID controllers are run
|
||||
// this should be called before any other calls to get_p, get_i or get_d
|
||||
void AC_PID::set_input_filter_all(float input)
|
||||
// filt_E_hz - set error filter hz
|
||||
void AC_PID::filt_E_hz(float hz)
|
||||
{
|
||||
_filt_E_hz.set(fabsf(hz));
|
||||
}
|
||||
|
||||
// filt_D_hz - set derivative filter hz
|
||||
void AC_PID::filt_D_hz(float hz)
|
||||
{
|
||||
_filt_D_hz.set(fabsf(hz));
|
||||
}
|
||||
|
||||
// update_all - set target and measured inputs to PID controller and calculate outputs
|
||||
// target and error are filtered
|
||||
// the derivative is then calculated and filtered
|
||||
// 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
|
||||
if (!isfinite(input)) {
|
||||
return;
|
||||
if (!isfinite(target) || !isfinite(measurement)) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
// reset input filter to value received
|
||||
if (_flags._reset_filter) {
|
||||
_flags._reset_filter = false;
|
||||
_input = input;
|
||||
_target = target;
|
||||
_error = _target - measurement;
|
||||
_derivative = 0.0f;
|
||||
}
|
||||
} else {
|
||||
float error_last = _error;
|
||||
_target += get_filt_T_alpha() * (target - _target);
|
||||
_error += get_filt_E_alpha() * ((_target - measurement) - _error);
|
||||
|
||||
// update filter and calculate derivative
|
||||
float input_filt_change = get_filt_alpha() * (input - _input);
|
||||
_input = _input + input_filt_change;
|
||||
if (_dt > 0.0f) {
|
||||
_derivative = input_filt_change / _dt;
|
||||
}
|
||||
}
|
||||
|
||||
// set_input_filter_d - set input to PID controller
|
||||
// only input to the D portion of the controller is filtered
|
||||
// this should be called before any other calls to get_p, get_i or get_d
|
||||
void AC_PID::set_input_filter_d(float input)
|
||||
{
|
||||
// don't process inf or NaN
|
||||
if (!isfinite(input)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// reset input filter to value received
|
||||
if (_flags._reset_filter) {
|
||||
_flags._reset_filter = false;
|
||||
_input = input;
|
||||
_derivative = 0.0f;
|
||||
}
|
||||
|
||||
// update filter and calculate derivative
|
||||
if (_dt > 0.0f) {
|
||||
float derivative = (input - _input) / _dt;
|
||||
_derivative = _derivative + get_filt_alpha() * (derivative-_derivative);
|
||||
}
|
||||
|
||||
_input = input;
|
||||
}
|
||||
|
||||
float AC_PID::get_p()
|
||||
{
|
||||
_pid_info.P = (_input * _kp);
|
||||
return _pid_info.P;
|
||||
}
|
||||
|
||||
float AC_PID::get_i()
|
||||
{
|
||||
if(!is_zero(_ki) && !is_zero(_dt)) {
|
||||
_integrator += ((float)_input * _ki) * _dt;
|
||||
if (_integrator < -_imax) {
|
||||
_integrator = -_imax;
|
||||
} else if (_integrator > _imax) {
|
||||
_integrator = _imax;
|
||||
// calculate and filter derivative
|
||||
if (_dt > 0.0f) {
|
||||
float derivative = (_error - error_last) / _dt;
|
||||
_derivative += get_filt_D_alpha() * (derivative - _derivative);
|
||||
}
|
||||
_pid_info.I = _integrator;
|
||||
return _integrator;
|
||||
}
|
||||
return 0;
|
||||
|
||||
// update I term
|
||||
update_i(limit);
|
||||
|
||||
float P_out = (_error * _kp);
|
||||
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;
|
||||
}
|
||||
|
||||
float AC_PID::get_d()
|
||||
// update_error - set error input to PID controller and calculate outputs
|
||||
// target is set to zero and error is set and filtered
|
||||
// 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 AC_PID::update_error(float error, bool limit)
|
||||
{
|
||||
// derivative component
|
||||
_pid_info.D = (_kd * _derivative);
|
||||
return _pid_info.D;
|
||||
// don't process inf or NaN
|
||||
if (!isfinite(error)) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
_target = 0.0f;
|
||||
|
||||
// reset input filter to value received
|
||||
if (_flags._reset_filter) {
|
||||
_flags._reset_filter = false;
|
||||
_error = error;
|
||||
_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 I term
|
||||
update_i(limit);
|
||||
|
||||
float P_out = (_error * _kp);
|
||||
float D_out = (_derivative * _kd);
|
||||
|
||||
_pid_info.target = 0.0f;
|
||||
_pid_info.actual = 0.0f;
|
||||
_pid_info.error = _error;
|
||||
_pid_info.P = P_out;
|
||||
_pid_info.D = D_out;
|
||||
|
||||
return P_out + _integrator + D_out;
|
||||
}
|
||||
|
||||
float AC_PID::get_ff(float requested_rate)
|
||||
// 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)
|
||||
{
|
||||
_pid_info.FF = (float)requested_rate * _ff;
|
||||
return _pid_info.FF;
|
||||
if (!is_zero(_ki) && is_positive(_dt)) {
|
||||
// Ensure that integrator can only be reduced if the output is saturated
|
||||
if (!limit || ((is_positive(_integrator) && is_negative(_error)) || (is_negative(_integrator) && is_positive(_error)))) {
|
||||
_integrator += ((float)_error * _ki) * _dt;
|
||||
_integrator = constrain_float(_integrator, -_kimax, _kimax);
|
||||
}
|
||||
} else {
|
||||
_integrator = 0.0f;
|
||||
}
|
||||
_pid_info.I = _integrator;
|
||||
}
|
||||
|
||||
|
||||
float AC_PID::get_pi()
|
||||
float AC_PID::get_p() const
|
||||
{
|
||||
return get_p() + get_i();
|
||||
return _error * _kp;
|
||||
}
|
||||
|
||||
float AC_PID::get_pid()
|
||||
float AC_PID::get_i() const
|
||||
{
|
||||
return get_p() + get_i() + get_d();
|
||||
return _integrator;
|
||||
}
|
||||
|
||||
float AC_PID::get_d() const
|
||||
{
|
||||
return _kd * _derivative;
|
||||
}
|
||||
|
||||
float AC_PID::get_ff()
|
||||
{
|
||||
_pid_info.FF = _target * _kff;
|
||||
return _target * _kff;
|
||||
}
|
||||
|
||||
// todo: remove function when it is no longer used.
|
||||
float AC_PID::get_ff(float target)
|
||||
{
|
||||
float FF_out = (target * _kff);
|
||||
_pid_info.FF = FF_out;
|
||||
return FF_out;
|
||||
}
|
||||
|
||||
void AC_PID::reset_I()
|
||||
|
|
@ -187,9 +254,12 @@ void AC_PID::load_gains()
|
|||
_kp.load();
|
||||
_ki.load();
|
||||
_kd.load();
|
||||
_imax.load();
|
||||
_imax = fabsf(_imax);
|
||||
_filt_hz.load();
|
||||
_kff.load();
|
||||
_kimax.load();
|
||||
_kimax = fabsf(_kimax);
|
||||
_filt_T_hz.load();
|
||||
_filt_E_hz.load();
|
||||
_filt_D_hz.load();
|
||||
}
|
||||
|
||||
// save_gains - save gains to eeprom
|
||||
|
|
@ -198,30 +268,63 @@ void AC_PID::save_gains()
|
|||
_kp.save();
|
||||
_ki.save();
|
||||
_kd.save();
|
||||
_imax.save();
|
||||
_filt_hz.save();
|
||||
_kff.save();
|
||||
_kimax.save();
|
||||
_filt_T_hz.save();
|
||||
_filt_E_hz.save();
|
||||
_filt_D_hz.save();
|
||||
}
|
||||
|
||||
/// 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;
|
||||
_ki = i;
|
||||
_kd = d;
|
||||
_imax = fabsf(imaxval);
|
||||
_filt_hz = input_filt_hz;
|
||||
_kp = p_val;
|
||||
_ki = i_val;
|
||||
_kd = d_val;
|
||||
_kff = ff_val;
|
||||
_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;
|
||||
_ff = ffval;
|
||||
}
|
||||
|
||||
// calc_filt_alpha - recalculate the input filter alpha
|
||||
float AC_PID::get_filt_alpha() const
|
||||
// get_filt_T_alpha - get the target filter alpha
|
||||
float AC_PID::get_filt_T_alpha() const
|
||||
{
|
||||
if (is_zero(_filt_hz)) {
|
||||
return get_filt_alpha(_filt_T_hz);
|
||||
}
|
||||
|
||||
// get_filt_E_alpha - get the error filter alpha
|
||||
float AC_PID::get_filt_E_alpha() const
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
// calculate alpha
|
||||
float rc = 1/(M_2PI*_filt_hz);
|
||||
float rc = 1 / (M_2PI * filt_hz);
|
||||
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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -9,8 +9,9 @@
|
|||
#include <cmath>
|
||||
#include <AP_Logger/AP_Logger.h>
|
||||
|
||||
#define AC_PID_FILT_HZ_DEFAULT 20.0f // default input filter frequency
|
||||
#define AC_PID_FILT_HZ_MIN 0.01f // minimum input filter frequency
|
||||
#define AC_PID_TFILT_HZ_DEFAULT 0.0f // default 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
|
||||
/// @brief Copter PID control class
|
||||
|
|
@ -18,93 +19,118 @@ class AC_PID {
|
|||
public:
|
||||
|
||||
// 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
|
||||
void set_dt(float dt);
|
||||
void set_dt(float dt);
|
||||
|
||||
// set_input_filter_all - set input to PID controller
|
||||
// input is filtered before the PID controllers are run
|
||||
// this should be called before any other calls to get_p, get_i or get_d
|
||||
void set_input_filter_all(float input);
|
||||
// update_all - set target and measured inputs to PID controller and calculate outputs
|
||||
// target and error are filtered
|
||||
// the derivative is then calculated and filtered
|
||||
// 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
|
||||
// this should be called before any other calls to get_p, get_i or get_d
|
||||
void set_input_filter_d(float input);
|
||||
// update_error - set error input to PID controller and calculate outputs
|
||||
// target is set to zero and error is set and filtered
|
||||
// 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
|
||||
float get_pid();
|
||||
float get_pi();
|
||||
float get_p();
|
||||
float get_i();
|
||||
float get_d();
|
||||
float get_ff(float requested_rate);
|
||||
float get_pid() const;
|
||||
float get_pi() const;
|
||||
float get_p() const;
|
||||
float get_i() const;
|
||||
float get_d() const;
|
||||
float get_ff();
|
||||
|
||||
// todo: remove function when it is no longer used.
|
||||
float get_ff(float target);
|
||||
|
||||
// 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()
|
||||
void reset_filter() { _flags._reset_filter = true; }
|
||||
void reset_filter() {
|
||||
_flags._reset_filter = true;
|
||||
}
|
||||
|
||||
// load gain from eeprom
|
||||
void load_gains();
|
||||
void load_gains();
|
||||
|
||||
// save gain to eeprom
|
||||
void save_gains();
|
||||
void save_gains();
|
||||
|
||||
/// 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
|
||||
AP_Float &kP() { return _kp; }
|
||||
AP_Float &kI() { return _ki; }
|
||||
AP_Float &kD() { return _kd; }
|
||||
AP_Float &filt_hz() { return _filt_hz; }
|
||||
float imax() const { return _imax.get(); }
|
||||
float get_filt_alpha() const;
|
||||
float ff() const { return _ff.get(); }
|
||||
AP_Float &kP() { return _kp; }
|
||||
AP_Float &kI() { return _ki; }
|
||||
AP_Float &kD() { return _kd; }
|
||||
AP_Float &ff() { return _kff;}
|
||||
AP_Float &filt_T_hz() { return _filt_T_hz; }
|
||||
AP_Float &filt_E_hz() { return _filt_E_hz; }
|
||||
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
|
||||
void kP(const float v) { _kp.set(v); }
|
||||
void kI(const float v) { _ki.set(v); }
|
||||
void kD(const float v) { _kd.set(v); }
|
||||
void imax(const float v) { _imax.set(fabsf(v)); }
|
||||
void filt_hz(const float v);
|
||||
void ff(const float v) { _ff.set(v); }
|
||||
|
||||
float get_integrator() const { return _integrator; }
|
||||
void set_integrator(float i) { _integrator = i; }
|
||||
void kP(const float v) { _kp.set(v); }
|
||||
void kI(const float v) { _ki.set(v); }
|
||||
void kD(const float v) { _kd.set(v); }
|
||||
void ff(const float v) { _kff.set(v); }
|
||||
void imax(const float v) { _kimax.set(fabsf(v)); }
|
||||
void filt_T_hz(const float v);
|
||||
void filt_E_hz(const float v);
|
||||
void filt_D_hz(const float v);
|
||||
|
||||
// set the desired and actual rates (for logging purposes)
|
||||
void set_desired_rate(float desired) { _pid_info.desired = desired; }
|
||||
void set_actual_rate(float actual) { _pid_info.actual = actual; }
|
||||
void set_target_rate(float target) { _pid_info.target = target; }
|
||||
void set_actual_rate(float actual) { _pid_info.actual = actual; }
|
||||
|
||||
const AP_Logger::PID_Info& get_pid_info(void) const { return _pid_info; }
|
||||
// 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; }
|
||||
|
||||
// parameter var table
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
protected:
|
||||
|
||||
// parameters
|
||||
AP_Float _kp;
|
||||
AP_Float _ki;
|
||||
AP_Float _kd;
|
||||
AP_Float _imax;
|
||||
AP_Float _filt_hz; // PID Input filter frequency in Hz
|
||||
AP_Float _ff;
|
||||
AP_Float _kp;
|
||||
AP_Float _ki;
|
||||
AP_Float _kd;
|
||||
AP_Float _kff;
|
||||
AP_Float _kimax;
|
||||
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
|
||||
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;
|
||||
|
||||
// internal variables
|
||||
float _dt; // timestep in seconds
|
||||
float _integrator; // integrator value
|
||||
float _input; // last input for derivative
|
||||
float _derivative; // last derivative for low-pass filter
|
||||
float _dt; // timestep in seconds
|
||||
float _integrator; // integrator value
|
||||
float _target; // target value to enable filtering
|
||||
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;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -70,8 +70,8 @@ void setup()
|
|||
void loop()
|
||||
{
|
||||
// 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_HELI_PID heli_pid(TEST_P, TEST_I, TEST_D, TEST_IMAX * 100, TEST_FILTER, TEST_DT, TEST_INITIAL_FF);
|
||||
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_INITIAL_FF, TEST_IMAX * 100, 0.0f, 0.0f, TEST_FILTER, TEST_DT);
|
||||
|
||||
// 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());
|
||||
|
|
@ -91,7 +91,7 @@ void loop()
|
|||
rc().read_input(); // poll the radio for new values
|
||||
const uint16_t radio_in = c->get_radio_in();
|
||||
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_I = pid.get_i();
|
||||
const float control_D = pid.get_d();
|
||||
|
|
|
|||
Loading…
Reference in New Issue