#include "AC_PID_Basic.h"
|
|
|
|
// 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)
|
{
|
// load parameter values from eeprom
|
|
|
_kp=(initial_p);
|
_ki=(initial_i);
|
_kd=(initial_d);
|
_kff=(initial_ff);
|
_kimax=(initial_imax);
|
_filt_E_hz=(initial_filt_E_hz);
|
_filt_D_hz=(initial_filt_D_hz);
|
|
// reset input filter to first value received
|
_reset_filter = true;
|
_error=0;
|
}
|
|
float AC_PID_Basic::update_all(float target, float measurement, float dt, bool limit)
|
{
|
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, float dt, bool limit_neg, bool limit_pos)
|
{
|
// don't process inf or NaN
|
/*if (!isfinite(target) || isnan(target) ||
|
!isfinite(measurement) || isnan(measurement)) {
|
INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
|
return 0.0f;
|
}*/
|
|
|
if(target<0)
|
target+=2*M_PI;
|
if(measurement<0)
|
measurement+=2*M_PI;
|
_error=target-measurement;
|
_target = target;
|
float error_last = _error;
|
if(_error>(M_PI+2.0*M_PI/180.0))
|
_error=-2*M_PI+_error;
|
if(_error<(-M_PI-2.0*M_PI/180.0))
|
_error=2*M_PI+_error;
|
_error*=180.0/M_PI;
|
// reset input filter to value received
|
if (_reset_filter) {
|
_reset_filter = false;
|
error_last=_error;
|
//_error = _target - measurement;
|
_derivative = 0.0f;
|
} else {
|
|
_error += get_filt_E_alpha(dt) * (_error - error_last);
|
|
// calculate and filter derivative
|
if (is_positive(dt)) {
|
float derivative = (_error - error_last) / dt;
|
_derivative += get_filt_D_alpha(dt) * (derivative - _derivative);
|
}
|
}
|
|
// update I term
|
update_i(dt, limit_neg, limit_pos);
|
|
const float P_out = _error * _kp;
|
const float D_out = _derivative * _kd;
|
|
float out=P_out + _integrator + D_out + _target * _kff;
|
if(out>45.0f)
|
out=45.0f;
|
if(out<-45.0f)
|
out=-45.0f;
|
return out;
|
}
|
|
// 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(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 = constrain_float(_integrator, -_kimax, _kimax);
|
}
|
} else {
|
_integrator = 0.0f;
|
}
|
}
|
|
void AC_PID_Basic::reset_I()
|
{
|
_integrator = 0.0;
|
|
}
|
|
|
// get_filt_T_alpha - get the target filter alpha
|
float AC_PID_Basic::get_filt_E_alpha(float dt)
|
{
|
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(float dt)
|
{
|
return calc_lowpass_alpha_dt(dt, _filt_D_hz);
|
}
|
|
void AC_PID_Basic::set_integrator(float target, float measurement, float i)
|
{
|
set_integrator(target - measurement, i);
|
}
|
|
void AC_PID_Basic::set_integrator(float error, float i)
|
{
|
set_integrator(i - error * _kp);
|
}
|
|
void AC_PID_Basic::set_integrator(float i)
|
{
|
_integrator = constrain_float(i, -_kimax, _kimax);
|
|
}
|
|
float AC_PID_Basic::calc_lowpass_alpha_dt(float dt, float cutoff_freq)
|
{
|
if (is_negative(dt) || is_negative(cutoff_freq)) {
|
|
return 1.0;
|
}
|
if (is_zero(cutoff_freq)) {
|
return 0.0;
|
}
|
if (is_zero(dt)) {
|
return 0.0;
|
}
|
float rc = 1.0f / (M_2PI * cutoff_freq);
|
return dt / (dt + rc);
|
}
|
