ardupilot/libraries/AC_PID/AC_PID_2D.cpp

210 lines
6.4 KiB
C++
Raw Normal View History

2017-11-17 10:10:04 -04:00
/// @file AC_PID_2D.cpp
/// @brief Generic PID algorithm
#include <AP_Math/AP_Math.h>
#include "AC_PID_2D.h"
#define AC_PID_2D_FILT_D_HZ_MIN 0.005f // minimum input filter frequency
2017-11-17 10:10:04 -04:00
const AP_Param::GroupInfo AC_PID_2D::var_info[] = {
// @Param: P
// @DisplayName: PID Proportional Gain
// @Description: P Gain which produces an output value that is proportional to the current error value
2023-01-03 13:22:36 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("P", 0, AC_PID_2D, _kp, default_kp),
2017-11-17 10:10:04 -04:00
// @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
2023-01-03 13:22:36 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("I", 1, AC_PID_2D, _ki, default_ki),
2017-11-17 10:10:04 -04:00
// @Param: IMAX
// @DisplayName: PID Integral Maximum
// @Description: The maximum/minimum value that the I term can output
2023-01-03 13:22:36 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("IMAX", 2, AC_PID_2D, _kimax, default_kimax),
2017-11-17 10:10:04 -04:00
// @Param: FLTE
2017-11-17 10:10:04 -04:00
// @DisplayName: PID Input filter frequency in Hz
// @Description: Input filter frequency in Hz
// @Units: Hz
2023-01-03 13:22:36 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTE", 3, AC_PID_2D, _filt_E_hz, default_filt_E_hz),
2017-11-17 10:10:04 -04:00
// @Param: D
// @DisplayName: PID Derivative Gain
// @Description: D Gain which produces an output that is proportional to the rate of change of the error
2023-01-03 13:22:36 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("D", 4, AC_PID_2D, _kd, default_kd),
2017-11-17 10:10:04 -04:00
// @Param: FLTD
2017-11-17 10:10:04 -04:00
// @DisplayName: D term filter frequency in Hz
// @Description: D term filter frequency in Hz
// @Units: Hz
2023-01-03 13:22:36 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FLTD", 5, AC_PID_2D, _filt_D_hz, default_filt_D_hz),
// @Param: FF
// @DisplayName: PID Feed Forward Gain
// @Description: FF Gain which produces an output that is proportional to the magnitude of the target
2023-01-03 13:22:36 -04:00
AP_GROUPINFO_FLAGS_DEFAULT_POINTER("FF", 6, AC_PID_2D, _kff, default_kff),
2017-11-17 10:10:04 -04:00
AP_GROUPEND
};
// Constructor
2023-01-03 13:22:36 -04:00
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) :
default_kp(initial_kP),
default_ki(initial_kI),
default_kd(initial_kD),
default_kff(initial_kFF),
default_kimax(initial_imax),
default_filt_E_hz(initial_filt_E_hz),
default_filt_D_hz(initial_filt_D_hz)
2017-11-17 10:10:04 -04:00
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
// reset input filter to first value received
_reset_filter = true;
2017-11-17 10:10:04 -04:00
}
// 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
2021-05-19 11:10:14 -03:00
// the integral is then updated if it does not increase in the direction of the limit vector
2022-12-02 08:34:53 -04:00
Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, float dt, const Vector2f &limit)
2017-11-17 10:10:04 -04:00
{
// don't process inf or NaN
if (target.is_nan() || target.is_inf() ||
measurement.is_nan() || measurement.is_inf()) {
return Vector2f{};
2017-11-17 10:10:04 -04:00
}
_target = target;
2017-11-17 10:10:04 -04:00
// reset input filter to value received
if (_reset_filter) {
_reset_filter = false;
_error = _target - measurement;
_derivative.zero();
} else {
Vector2f error_last{_error};
2022-12-02 08:34:53 -04:00
_error += ((_target - measurement) - _error) * get_filt_E_alpha(dt);
// calculate and filter derivative
2022-12-02 08:34:53 -04:00
if (is_positive(dt)) {
const Vector2f derivative{(_error - error_last) / dt};
_derivative += (derivative - _derivative) * get_filt_D_alpha(dt);
}
2017-11-17 10:10:04 -04:00
}
// update I term
2022-12-02 08:34:53 -04:00
update_i(dt, limit);
2017-11-17 10:10:04 -04:00
_pid_info_x.target = _target.x;
_pid_info_x.actual = measurement.x;
_pid_info_x.error = _error.x;
_pid_info_x.P = _error.x * _kp;
_pid_info_x.I = _integrator.x;
_pid_info_x.D = _derivative.x * _kd;
_pid_info_x.FF = _target.x * _kff;
2017-11-17 10:10:04 -04:00
_pid_info_y.target = _target.y;
_pid_info_y.actual = measurement.y;
_pid_info_y.error = _error.y;
_pid_info_y.P = _error.y * _kp;
_pid_info_y.I = _integrator.y;
_pid_info_y.D = _derivative.y * _kd;
_pid_info_y.FF = _target.y * _kff;
2017-11-17 10:10:04 -04:00
return _error * _kp + _integrator + _derivative * _kd + _target * _kff;
2017-11-17 10:10:04 -04:00
}
2022-12-02 08:34:53 -04:00
Vector2f AC_PID_2D::update_all(const Vector3f &target, const Vector3f &measurement, float dt, const Vector3f &limit)
2017-11-17 10:10:04 -04:00
{
2022-12-02 08:34:53 -04:00
return update_all(Vector2f{target.x, target.y}, Vector2f{measurement.x, measurement.y}, dt, Vector2f{limit.x, limit.y});
2017-11-17 10:10:04 -04:00
}
// update_i - update the integral
// If the limit is set the integral is only allowed to reduce in the direction of the limit
2022-12-02 08:34:53 -04:00
void AC_PID_2D::update_i(float dt, const Vector2f &limit)
2017-11-17 10:10:04 -04:00
{
2021-11-19 17:14:01 -04:00
_pid_info_x.limit = false;
_pid_info_y.limit = false;
2022-12-02 08:34:53 -04:00
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
if (is_positive(delta_integrator * limit) && _integrator.limit_length(integrator_length)) {
_pid_info_x.limit = true;
_pid_info_y.limit = true;
2017-11-17 10:10:04 -04:00
}
_integrator.limit_length(_kimax);
}
Vector2f AC_PID_2D::get_p() const
{
return _error * _kp;
}
2021-05-20 07:42:20 -03:00
const Vector2f& AC_PID_2D::get_i() const
{
return _integrator;
}
Vector2f AC_PID_2D::get_d() const
{
return _derivative * _kd;
}
Vector2f AC_PID_2D::get_ff()
{
_pid_info_x.FF = _target.x * _kff;
_pid_info_y.FF = _target.y * _kff;
return _target * _kff;
2017-11-17 10:10:04 -04:00
}
void AC_PID_2D::reset_I()
{
_integrator.zero();
}
// save_gains - save gains to eeprom
void AC_PID_2D::save_gains()
2017-11-17 10:10:04 -04:00
{
_kp.save();
_ki.save();
_kd.save();
_kff.save();
_kimax.save();
_filt_E_hz.save();
_filt_D_hz.save();
2017-11-17 10:10:04 -04:00
}
// get the target filter alpha
2022-12-02 08:34:53 -04:00
float AC_PID_2D::get_filt_E_alpha(float dt) const
2017-11-17 10:10:04 -04:00
{
2022-12-02 08:34:53 -04:00
return calc_lowpass_alpha_dt(dt, _filt_E_hz);
}
// get the derivative filter alpha
2022-12-02 08:34:53 -04:00
float AC_PID_2D::get_filt_D_alpha(float dt) const
{
2022-12-02 08:34:53 -04:00
return calc_lowpass_alpha_dt(dt, _filt_D_hz);
2017-11-17 10:10:04 -04:00
}
void AC_PID_2D::set_integrator(const Vector2f& target, const Vector2f& measurement, const Vector2f& i)
2017-11-17 10:10:04 -04:00
{
set_integrator(target - measurement, i);
2017-11-17 10:10:04 -04:00
}
void AC_PID_2D::set_integrator(const Vector2f& error, const Vector2f& i)
2017-11-17 10:10:04 -04:00
{
set_integrator(i - error * _kp);
2017-11-17 10:10:04 -04:00
}
void AC_PID_2D::set_integrator(const Vector2f& i)
2017-11-17 10:10:04 -04:00
{
_integrator = i;
2021-11-19 17:14:01 -04:00
_integrator.limit_length(_kimax);
2017-11-17 10:10:04 -04:00
}