Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PID: Support changing update period

This commit is contained in:
lthall 2022-12-02 23:04:53 +10:30 committed by Randy Mackay
parent e1f15d53ad
commit faf9739ad8
13 changed files with 94 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AC_PID/AC_HELI_PID.cpp
View File

@ -73,8 +73,8 @@ const AP_Param::GroupInfo AC_HELI_PID::var_info[] = {
};
/// Constructor for PID
AC_HELI_PID::AC_HELI_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) :
AC_PID(initial_p, initial_i, initial_d, initial_ff, initial_imax, initial_filt_T_hz, initial_filt_E_hz, initial_filt_D_hz, dt)
AC_HELI_PID::AC_HELI_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) :
AC_PID(initial_p, initial_i, initial_d, initial_ff, initial_imax, initial_filt_T_hz, initial_filt_E_hz, initial_filt_D_hz)
{
_last_requested_rate = 0;
}
@ -84,7 +84,7 @@ AC_HELI_PID::AC_HELI_PID(float initial_p, float initial_i, float initial_d, floa
void AC_HELI_PID::update_leaky_i(float leak_rate)
{
if (!is_zero(_ki) && !is_zero(_dt)){
if (!is_zero(_ki)){
// integrator does not leak down below Leak Min
if (_integrator > _leak_min){

2
libraries/AC_PID/AC_HELI_PID.h
View File

@ -17,7 +17,7 @@ class AC_HELI_PID : public AC_PID {
public:
/// Constructor for PID
AC_HELI_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);
AC_HELI_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);
CLASS_NO_COPY(AC_HELI_PID);

73
libraries/AC_PID/AC_PID.cpp
View File

@ -69,8 +69,7 @@ const AP_Param::GroupInfo AC_PID::var_info[] = {
// Constructor
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, float initial_srmax, float initial_srtau):
_dt(dt)
float initial_srmax, float initial_srtau)
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
@ -96,13 +95,6 @@ AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_
_slew_limit_scale = 1;
}
// set_dt - set time step in seconds
void AC_PID::set_dt(float dt)
{
// set dt and calculate the input filter alpha
_dt = dt;
}
// filt_T_hz - set target filter hz
void AC_PID::filt_T_hz(float hz)
{
@ -131,7 +123,7 @@ void AC_PID::slew_limit(float smax)
// 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)
float AC_PID::update_all(float target, float measurement, float dt, bool limit)
{
// don't process inf or NaN
if (!isfinite(target) || !isfinite(measurement)) {
@ -146,24 +138,24 @@ float AC_PID::update_all(float target, float measurement, bool limit)
_derivative = 0.0f;
} else {
float error_last = _error;
_target += get_filt_T_alpha() * (target - _target);
_error += get_filt_E_alpha() * ((_target - measurement) - _error);
_target += get_filt_T_alpha(dt) * (target - _target);
_error += get_filt_E_alpha(dt) * ((_target - measurement) - _error);
// calculate and filter derivative
if (_dt > 0.0f) {
float derivative = (_error - error_last) / _dt;
_derivative += get_filt_D_alpha() * (derivative - _derivative);
if (is_positive(dt)) {
float derivative = (_error - error_last) / dt;
_derivative += get_filt_D_alpha(dt) * (derivative - _derivative);
}
}
// update I term
update_i(limit);
update_i(dt, limit);
float P_out = (_error * _kp);
float D_out = (_derivative * _kd);
// calculate slew limit modifier for P+D
_pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, _dt);
_pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, dt);
_pid_info.slew_rate = _slew_limiter.get_slew_rate();
P_out *= _pid_info.Dmod;
@ -184,7 +176,7 @@ float AC_PID::update_all(float target, float measurement, bool limit)
// 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)
float AC_PID::update_error(float error, float dt, bool limit)
{
// don't process inf or NaN
if (!isfinite(error)) {
@ -200,23 +192,23 @@ float AC_PID::update_error(float error, bool limit)
_derivative = 0.0f;
} else {
float error_last = _error;
_error += get_filt_E_alpha() * (error - _error);
_error += get_filt_E_alpha(dt) * (error - _error);
// calculate and filter derivative
if (_dt > 0.0f) {
float derivative = (_error - error_last) / _dt;
_derivative += get_filt_D_alpha() * (derivative - _derivative);
if (is_positive(dt)) {
float derivative = (_error - error_last) / dt;
_derivative += get_filt_D_alpha(dt) * (derivative - _derivative);
}
}
// update I term
update_i(limit);
update_i(dt, limit);
float P_out = (_error * _kp);
float D_out = (_derivative * _kd);
// calculate slew limit modifier for P+D
_pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, _dt);
_pid_info.Dmod = _slew_limiter.modifier((_pid_info.P + _pid_info.D) * _slew_limit_scale, dt);
_pid_info.slew_rate = _slew_limiter.get_slew_rate();
P_out *= _pid_info.Dmod;
@ -233,12 +225,12 @@ float AC_PID::update_error(float error, bool limit)
// 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)
void AC_PID::update_i(float dt, bool limit)
{
if (!is_zero(_ki) && is_positive(_dt)) {
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 += ((float)_error * _ki) * dt;
_integrator = constrain_float(_integrator, -_kimax, _kimax);
}
} else {
@ -302,7 +294,7 @@ void AC_PID::save_gains()
}
/// Overload the function call operator to permit easy initialisation
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)
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)
{
_kp.set(p_val);
_ki.set(i_val);
@ -312,31 +304,24 @@ void AC_PID::operator()(float p_val, float i_val, float d_val, float ff_val, flo
_filt_T_hz.set(input_filt_T_hz);
_filt_E_hz.set(input_filt_E_hz);
_filt_D_hz.set(input_filt_D_hz);
_dt = dt;
}
// get_filt_T_alpha - get the target filter alpha
float AC_PID::get_filt_T_alpha() const
float AC_PID::get_filt_T_alpha(float dt) const
{
return get_filt_alpha(_filt_T_hz);
return calc_lowpass_alpha_dt(dt, _filt_T_hz);
}
// get_filt_E_alpha - get the error filter alpha
float AC_PID::get_filt_E_alpha() const
float AC_PID::get_filt_E_alpha(float dt) const
{
return get_filt_alpha(_filt_E_hz);
return calc_lowpass_alpha_dt(dt, _filt_E_hz);
}
// get_filt_D_alpha - get the derivative filter alpha
float AC_PID::get_filt_D_alpha() const
float AC_PID::get_filt_D_alpha(float dt) 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
{
return calc_lowpass_alpha_dt(_dt, filt_hz);
return calc_lowpass_alpha_dt(dt, _filt_D_hz);
}
void AC_PID::set_integrator(float target, float measurement, float integrator)
@ -356,9 +341,11 @@ void AC_PID::set_integrator(float integrator)
_pid_info.I = _integrator;
}
void AC_PID::relax_integrator(float integrator, float time_constant)
void AC_PID::relax_integrator(float integrator, float dt, float time_constant)
{
integrator = constrain_float(integrator, -_kimax, _kimax);
_integrator = _integrator + (integrator - _integrator) * (_dt / (_dt + time_constant));
if (is_positive(dt)) {
_integrator = _integrator + (integrator - _integrator) * (dt / (dt + time_constant));
}
_pid_info.I = _integrator;
}

23
libraries/AC_PID/AC_PID.h
View File

@ -23,18 +23,15 @@ public:
// Constructor for 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, float initial_srmax=0, float initial_srtau=1.0);
float initial_srmax=0, float initial_srtau=1.0);
CLASS_NO_COPY(AC_PID);
// set_dt - set time step in seconds
void set_dt(float dt);
// 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);
float update_all(float target, float measurement, float dt, bool limit = false);
// update_error - set error input to PID controller and calculate outputs
// target is set to zero and error is set and filtered
@ -42,11 +39,11 @@ public:
// 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);
float update_error(float error, float dt, 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);
void update_i(float dt, bool limit);
// get_pid - get results from pid controller
float get_pid() const;
@ -71,7 +68,7 @@ public:
void save_gains();
/// operator function call for easy initialisation
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);
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);
// get accessors
AP_Float &kP() { return _kp; }
@ -85,10 +82,9 @@ public:
AP_Float &slew_limit() { return _slew_rate_max; }
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;
float get_filt_T_alpha(float dt) const;
float get_filt_E_alpha(float dt) const;
float get_filt_D_alpha(float dt) const;
// set accessors
void kP(const float v) { _kp.set(v); }
@ -109,7 +105,7 @@ public:
void set_integrator(float target, float measurement, float i);
void set_integrator(float error, float i);
void set_integrator(float i);
void relax_integrator(float integrator, float time_constant);
void relax_integrator(float integrator, float dt, float time_constant);
// set slew limiter scale factor
void set_slew_limit_scale(int8_t scale) { _slew_limit_scale = scale; }
@ -149,7 +145,6 @@ protected:
} _flags;
// internal variables
float _dt; // timestep in seconds
float _integrator; // integrator value
float _target; // target value to enable filtering
float _error; // error value to enable filtering

31
libraries/AC_PID/AC_PID_2D.cpp
View File

@ -50,8 +50,7 @@ const AP_Param::GroupInfo AC_PID_2D::var_info[] = {
};
// Constructor
AC_PID_2D::AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz, float dt) :
_dt(dt)
AC_PID_2D::AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz)
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
@ -72,7 +71,7 @@ AC_PID_2D::AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float
// target and error are filtered
// the derivative is then calculated and filtered
// the integral is then updated if it does not increase in the direction of the limit vector
Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, const Vector2f &limit)
Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, float dt, const Vector2f &limit)
{
// don't process inf or NaN
if (target.is_nan() || target.is_inf() ||
@ -89,17 +88,17 @@ Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measureme
_derivative.zero();
} else {
Vector2f error_last{_error};
_error += ((_target - measurement) - _error) * get_filt_E_alpha();
_error += ((_target - measurement) - _error) * get_filt_E_alpha(dt);
// calculate and filter derivative
if (_dt > 0.0f) {
const Vector2f derivative{(_error - error_last) / _dt};
_derivative += (derivative - _derivative) * get_filt_D_alpha();
if (is_positive(dt)) {
const Vector2f derivative{(_error - error_last) / dt};
_derivative += (derivative - _derivative) * get_filt_D_alpha(dt);
}
}
// update I term
update_i(limit);
update_i(dt, limit);
_pid_info_x.target = _target.x;
_pid_info_x.actual = measurement.x;
@ -120,19 +119,19 @@ Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measureme
return _error * _kp + _integrator + _derivative * _kd + _target * _kff;
}
Vector2f AC_PID_2D::update_all(const Vector3f &target, const Vector3f &measurement, const Vector3f &limit)
Vector2f AC_PID_2D::update_all(const Vector3f &target, const Vector3f &measurement, float dt, const Vector3f &limit)
{
return update_all(Vector2f{target.x, target.y}, Vector2f{measurement.x, measurement.y}, Vector2f{limit.x, limit.y});
return update_all(Vector2f{target.x, target.y}, Vector2f{measurement.x, measurement.y}, dt, Vector2f{limit.x, limit.y});
}
// update_i - update the integral
// If the limit is set the integral is only allowed to reduce in the direction of the limit
void AC_PID_2D::update_i(const Vector2f &limit)
void AC_PID_2D::update_i(float dt, const Vector2f &limit)
{
_pid_info_x.limit = false;
_pid_info_y.limit = false;
Vector2f delta_integrator = (_error * _ki) * _dt;
Vector2f delta_integrator = (_error * _ki) * dt;
float integrator_length = _integrator.length();
_integrator += delta_integrator;
// do not let integrator increase in length if delta_integrator is in the direction of limit
@ -186,15 +185,15 @@ void AC_PID_2D::save_gains()
}
// get the target filter alpha
float AC_PID_2D::get_filt_E_alpha() const
float AC_PID_2D::get_filt_E_alpha(float dt) const
{
return calc_lowpass_alpha_dt(_dt, _filt_E_hz);
return calc_lowpass_alpha_dt(dt, _filt_E_hz);
}
// get the derivative filter alpha
float AC_PID_2D::get_filt_D_alpha() const
float AC_PID_2D::get_filt_D_alpha(float dt) const
{
return calc_lowpass_alpha_dt(_dt, _filt_D_hz);
return calc_lowpass_alpha_dt(dt, _filt_D_hz);
}
void AC_PID_2D::set_integrator(const Vector2f& target, const Vector2f& measurement, const Vector2f& i)

16
libraries/AC_PID/AC_PID_2D.h
View File

@ -15,23 +15,20 @@ class AC_PID_2D {
public:
// Constructor for PID
AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_hz, float initial_filt_d_hz, float dt);
AC_PID_2D(float initial_kP, float initial_kI, float initial_kD, float initial_kFF, float initial_imax, float initial_filt_hz, float initial_filt_d_hz);
CLASS_NO_COPY(AC_PID_2D);
// set time step in seconds
void set_dt(float dt) { _dt = dt; }
// 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 if it does not increase in the direction of the limit vector
Vector2f update_all(const Vector2f &target, const Vector2f &measurement, const Vector2f &limit);
Vector2f update_all(const Vector3f &target, const Vector3f &measurement, const Vector3f &limit);
Vector2f update_all(const Vector2f &target, const Vector2f &measurement, float dt, const Vector2f &limit);
Vector2f update_all(const Vector3f &target, const Vector3f &measurement, float dt, const Vector3f &limit);
// update the integral
// if the limit flag is set the integral is only allowed to shrink
void update_i(const Vector2f &limit);
void update_i(float dt, const Vector2f &limit);
// get results from pid controller
Vector2f get_p() const;
@ -57,8 +54,8 @@ public:
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_E_alpha() const;
float get_filt_D_alpha() const;
float get_filt_E_alpha(float dt) const;
float get_filt_D_alpha(float dt) const;
// set accessors
void kP(float v) { _kp.set(v); }
@ -93,7 +90,6 @@ protected:
AP_Float _filt_D_hz; // PID derivative filter frequency in Hz
// internal variables
float _dt; // timestep in seconds
Vector2f _target; // target value to enable filtering
Vector2f _error; // error value to enable filtering
Vector2f _derivative; // last derivative from low-pass filter

31
libraries/AC_PID/AC_PID_Basic.cpp
View File

@ -52,8 +52,7 @@ const AP_Param::GroupInfo AC_PID_Basic::var_info[] = {
};
// Constructor
AC_PID_Basic::AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz, float dt) :
_dt(dt)
AC_PID_Basic::AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz)
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
@ -70,16 +69,16 @@ AC_PID_Basic::AC_PID_Basic(float initial_p, float initial_i, float initial_d, fl
_reset_filter = true;
}
float AC_PID_Basic::update_all(float target, float measurement, bool limit)
float AC_PID_Basic::update_all(float target, float measurement, float dt, bool limit)
{
return update_all(target, measurement, (limit && is_negative(_integrator)), (limit && is_positive(_integrator)));
return update_all(target, measurement, dt, (limit && is_negative(_integrator)), (limit && is_positive(_integrator)));
}
// 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_Basic::update_all(float target, float measurement, bool limit_neg, bool limit_pos)
float AC_PID_Basic::update_all(float target, float measurement, float dt, bool limit_neg, bool limit_pos)
{
// don't process inf or NaN
if (!isfinite(target) || isnan(target) ||
@ -97,17 +96,17 @@ float AC_PID_Basic::update_all(float target, float measurement, bool limit_neg,
_derivative = 0.0f;
} else {
float error_last = _error;
_error += get_filt_E_alpha() * ((_target - measurement) - _error);
_error += get_filt_E_alpha(dt) * ((_target - measurement) - _error);
// calculate and filter derivative
if (is_positive(_dt)) {
float derivative = (_error - error_last) / _dt;
_derivative += get_filt_D_alpha() * (derivative - _derivative);
if (is_positive(dt)) {
float derivative = (_error - error_last) / dt;
_derivative += get_filt_D_alpha(dt) * (derivative - _derivative);
}
}
// update I term
update_i(limit_neg, limit_pos);
update_i(dt, limit_neg, limit_pos);
const float P_out = _error * _kp;
const float D_out = _derivative * _kd;
@ -126,12 +125,12 @@ float AC_PID_Basic::update_all(float target, float measurement, bool limit_neg,
// update_i - update the integral
// if limit_neg is true, the integral can only increase
// if limit_pos is true, the integral can only decrease
void AC_PID_Basic::update_i(bool limit_neg, bool limit_pos)
void AC_PID_Basic::update_i(float dt, bool limit_neg, bool limit_pos)
{
if (!is_zero(_ki)) {
// Ensure that integrator can only be reduced if the output is saturated
if (!((limit_neg && is_negative(_error)) || (limit_pos && is_positive(_error)))) {
_integrator += ((float)_error * _ki) * _dt;
_integrator += ((float)_error * _ki) * dt;
_integrator = constrain_float(_integrator, -_kimax, _kimax);
}
} else {
@ -158,15 +157,15 @@ void AC_PID_Basic::save_gains()
}
// get_filt_T_alpha - get the target filter alpha
float AC_PID_Basic::get_filt_E_alpha() const
float AC_PID_Basic::get_filt_E_alpha(float dt) const
{
return calc_lowpass_alpha_dt(_dt, _filt_E_hz);
return calc_lowpass_alpha_dt(dt, _filt_E_hz);
}
// get_filt_D_alpha - get the derivative filter alpha
float AC_PID_Basic::get_filt_D_alpha() const
float AC_PID_Basic::get_filt_D_alpha(float dt) const
{
return calc_lowpass_alpha_dt(_dt, _filt_D_hz);
return calc_lowpass_alpha_dt(dt, _filt_D_hz);
}
void AC_PID_Basic::set_integrator(float target, float measurement, float i)

16
libraries/AC_PID/AC_PID_Basic.h
View File

@ -13,21 +13,18 @@ class AC_PID_Basic {
public:
// Constructor for PID
AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz, float dt);
// set time step in seconds
void set_dt(float dt) { _dt = dt; }
AC_PID_Basic(float initial_p, float initial_i, float initial_d, float initial_ff, float initial_imax, float initial_filt_E_hz, float initial_filt_D_hz);
// 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) WARN_IF_UNUSED;
float update_all(float target, float measurement, bool limit_neg, bool limit_pos) WARN_IF_UNUSED;
float update_all(float target, float measurement, float dt, bool limit = false) WARN_IF_UNUSED;
float update_all(float target, float measurement, float dt, bool limit_neg, bool limit_pos) WARN_IF_UNUSED;
// update the integral
// if the limit flags are set the integral is only allowed to shrink
void update_i(bool limit_neg, bool limit_pos);
void update_i(float dt, bool limit_neg, bool limit_pos);
// get results from pid controller
float get_p() const WARN_IF_UNUSED { return _error * _kp; }
@ -53,8 +50,8 @@ public:
AP_Float &filt_E_hz() WARN_IF_UNUSED { return _filt_E_hz; }
AP_Float &filt_D_hz() WARN_IF_UNUSED { return _filt_D_hz; }
float imax() const WARN_IF_UNUSED { return _kimax.get(); }
float get_filt_E_alpha() const WARN_IF_UNUSED;
float get_filt_D_alpha() const WARN_IF_UNUSED;
float get_filt_E_alpha(float dt) const WARN_IF_UNUSED;
float get_filt_D_alpha(float dt) const WARN_IF_UNUSED;
// set accessors
void kP(float v) { _kp.set(v); }
@ -87,7 +84,6 @@ protected:
AP_Float _filt_D_hz; // PID derivative filter frequency in Hz
// internal variables
float _dt; // timestep in seconds
float _target; // target value to enable filtering
float _error; // error value to enable filtering
float _derivative; // last derivative for low-pass filter

5
libraries/AC_PID/AC_P_1D.cpp
View File

@ -13,8 +13,7 @@ const AP_Param::GroupInfo AC_P_1D::var_info[] = {
};
// Constructor
AC_P_1D::AC_P_1D(float initial_p, float dt) :
_dt(dt)
AC_P_1D::AC_P_1D(float initial_p)
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
@ -38,7 +37,7 @@ float AC_P_1D::update_all(float &target, float measurement)
}
// MIN(_Dxy_max, _D2xy_max / _kxy_P) limits the max accel to the point where max jerk is exceeded
return sqrt_controller(_error, _kp, _D1_max, _dt);
return sqrt_controller(_error, _kp, _D1_max, 0.0);
}
// set_limits - sets the maximum error to limit output and first and second derivative of output

6
libraries/AC_PID/AC_P_1D.h
View File

@ -12,13 +12,10 @@ class AC_P_1D {
public:
// constructor
AC_P_1D(float initial_p, float dt);
AC_P_1D(float initial_p);
CLASS_NO_COPY(AC_P_1D);
// set time step in seconds
void set_dt(float dt) { _dt = dt; }
// update_all - set target and measured inputs to P controller and calculate outputs
// target and measurement are filtered
float update_all(float &target, float measurement) WARN_IF_UNUSED;
@ -56,7 +53,6 @@ private:
AP_Float _kp;
// internal variables
float _dt; // time step in seconds
float _error; // time step in seconds
float _error_min; // error limit in negative direction
float _error_max; // error limit in positive direction

5
libraries/AC_PID/AC_P_2D.cpp
View File

@ -13,8 +13,7 @@ const AP_Param::GroupInfo AC_P_2D::var_info[] = {
};
// Constructor
AC_P_2D::AC_P_2D(float initial_p, float dt) :
_dt(dt)
AC_P_2D::AC_P_2D(float initial_p)
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
@ -36,7 +35,7 @@ Vector2f AC_P_2D::update_all(postype_t &target_x, postype_t &target_y, const Vec
}
// MIN(_Dmax, _D2max / _kp) limits the max accel to the point where max jerk is exceeded
return sqrt_controller(_error, _kp, _D1_max, _dt);
return sqrt_controller(_error, _kp, _D1_max, 0.0);
}
// set_limits - sets the maximum error to limit output and first and second derivative of output

6
libraries/AC_PID/AC_P_2D.h
View File

@ -12,13 +12,10 @@ class AC_P_2D {
public:
// constructor
AC_P_2D(float initial_p, float dt);
AC_P_2D(float initial_p);
CLASS_NO_COPY(AC_P_2D);
// set time step in seconds
void set_dt(float dt) { _dt = dt; }
// set target and measured inputs to P controller and calculate outputs
Vector2f update_all(postype_t &target_x, postype_t &target_y, const Vector2f &measurement) WARN_IF_UNUSED;
@ -58,7 +55,6 @@ private:
AP_Float _kp;
// internal variables
float _dt; // time step in seconds
Vector2f _error; // time step in seconds
float _error_max; // error limit in positive direction
float _D1_max; // maximum first derivative of output

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

@ -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, 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);
AC_PID pid(TEST_P, TEST_I, TEST_D, 0.0f, TEST_IMAX * 100.0f, 0.0f, 0.0f, TEST_FILTER);
AC_HELI_PID heli_pid(TEST_P, TEST_I, TEST_D, TEST_INITIAL_FF, TEST_IMAX * 100, 0.0f, 0.0f, TEST_FILTER);
// 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.update_error(error);
pid.update_error(error, TEST_DT);
const float control_P = pid.get_p();
const float control_I = pid.get_i();
const float control_D = pid.get_d();