mirror of https://github.com/ArduPilot/ardupilot
AC_PID: library update and additional functions
includes corrections from peer review
This commit is contained in:
parent
bc0da915c4
commit
1d720cc5e9
|
@ -1,7 +1,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
/// @file AC_PD.h
|
/// @file AC_PD.h
|
||||||
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
|
/// @brief Generic P controller with EEPROM-backed storage of constants.
|
||||||
|
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_Common/AP_Common.h>
|
||||||
#include <AP_Param/AP_Param.h>
|
#include <AP_Param/AP_Param.h>
|
||||||
|
|
|
@ -23,13 +23,13 @@ const AP_Param::GroupInfo AC_PID_2D::var_info[] = {
|
||||||
// @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", 2, AC_PID_2D, _imax, 0),
|
AP_GROUPINFO("IMAX", 2, AC_PID_2D, _kimax, 0),
|
||||||
|
|
||||||
// @Param: FILT
|
// @Param: FILT
|
||||||
// @DisplayName: PID Input filter frequency in Hz
|
// @DisplayName: PID Input filter frequency in Hz
|
||||||
// @Description: Input filter frequency in Hz
|
// @Description: Input filter frequency in Hz
|
||||||
// @Units: Hz
|
// @Units: Hz
|
||||||
AP_GROUPINFO("FILT", 3, AC_PID_2D, _filt_hz, AC_PID_2D_FILT_HZ_DEFAULT),
|
AP_GROUPINFO("FILT", 3, AC_PID_2D, _filt_E_hz, AC_PID_2D_FILT_HZ_DEFAULT),
|
||||||
|
|
||||||
// @Param: D
|
// @Param: D
|
||||||
// @DisplayName: PID Derivative Gain
|
// @DisplayName: PID Derivative Gain
|
||||||
|
@ -40,173 +40,105 @@ const AP_Param::GroupInfo AC_PID_2D::var_info[] = {
|
||||||
// @DisplayName: D term filter frequency in Hz
|
// @DisplayName: D term filter frequency in Hz
|
||||||
// @Description: D term filter frequency in Hz
|
// @Description: D term filter frequency in Hz
|
||||||
// @Units: Hz
|
// @Units: Hz
|
||||||
AP_GROUPINFO("D_FILT", 5, AC_PID_2D, _filt_d_hz, AC_PID_2D_FILT_D_HZ_DEFAULT),
|
AP_GROUPINFO("D_FILT", 5, AC_PID_2D, _filt_D_hz, AC_PID_2D_FILT_D_HZ_DEFAULT),
|
||||||
|
|
||||||
|
// @Param: FF
|
||||||
|
// @DisplayName: PID Feed Forward Gain
|
||||||
|
// @Description: FF Gain which produces an output that is proportional to the magnitude of the target
|
||||||
|
AP_GROUPINFO("FF", 6, AC_PID_2D, _kff, 0),
|
||||||
|
|
||||||
AP_GROUPEND
|
AP_GROUPEND
|
||||||
};
|
};
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
AC_PID_2D::AC_PID_2D(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float initial_filt_d_hz, float 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, float dt) :
|
||||||
_dt(dt)
|
_dt(dt)
|
||||||
{
|
{
|
||||||
// load parameter values from eeprom
|
// load parameter values from eeprom
|
||||||
AP_Param::setup_object_defaults(this, var_info);
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
|
|
||||||
_kp = initial_p;
|
_kp = initial_kP;
|
||||||
_ki = initial_i;
|
_ki = initial_kI;
|
||||||
_kd = initial_d;
|
_kd = initial_kD;
|
||||||
_imax = fabsf(initial_imax);
|
_kff = initial_kFF;
|
||||||
filt_hz(initial_filt_hz);
|
_kimax = fabsf(initial_imax);
|
||||||
filt_d_hz(initial_filt_d_hz);
|
filt_E_hz(initial_filt_E_hz);
|
||||||
|
filt_D_hz(initial_filt_D_hz);
|
||||||
|
|
||||||
// reset input filter to first value received and derivitive to zero
|
// reset input filter to first value received
|
||||||
reset_filter();
|
_reset_filter = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// set_dt - set time step in seconds
|
// update_all - set target and measured inputs to PID controller and calculate outputs
|
||||||
void AC_PID_2D::set_dt(float dt)
|
// target and error are filtered
|
||||||
{
|
// the derivative is then calculated and filtered
|
||||||
// set dt and calculate the input filter alpha
|
// the integral is then updated based on the setting of the limit flag
|
||||||
_dt = dt;
|
Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, bool limit)
|
||||||
calc_filt_alpha();
|
|
||||||
calc_filt_alpha_d();
|
|
||||||
}
|
|
||||||
|
|
||||||
// filt_hz - set input filter hz
|
|
||||||
void AC_PID_2D::filt_hz(float hz)
|
|
||||||
{
|
|
||||||
_filt_hz.set(fabsf(hz));
|
|
||||||
|
|
||||||
// sanity check _filt_hz
|
|
||||||
_filt_hz = MAX(_filt_hz, AC_PID_2D_FILT_HZ_MIN);
|
|
||||||
|
|
||||||
// calculate the input filter alpha
|
|
||||||
calc_filt_alpha();
|
|
||||||
}
|
|
||||||
|
|
||||||
// filt_d_hz - set input filter hz
|
|
||||||
void AC_PID_2D::filt_d_hz(float hz)
|
|
||||||
{
|
|
||||||
_filt_d_hz.set(fabsf(hz));
|
|
||||||
|
|
||||||
// sanity check _filt_hz
|
|
||||||
_filt_d_hz = MAX(_filt_d_hz, AC_PID_2D_FILT_D_HZ_MIN);
|
|
||||||
|
|
||||||
// calculate the input filter alpha
|
|
||||||
calc_filt_alpha_d();
|
|
||||||
}
|
|
||||||
|
|
||||||
// set_input - 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_2D::set_input(const Vector2f &input)
|
|
||||||
{
|
{
|
||||||
// don't process inf or NaN
|
// don't process inf or NaN
|
||||||
if (!isfinite(input.x) || !isfinite(input.y)) {
|
if (target.is_nan() || target.is_inf() ||
|
||||||
return;
|
measurement.is_nan() || measurement.is_inf()) {
|
||||||
|
return Vector2f{};
|
||||||
}
|
}
|
||||||
|
|
||||||
|
_target = target;
|
||||||
|
|
||||||
// reset input filter to value received
|
// reset input filter to value received
|
||||||
if (_flags._reset_filter) {
|
if (_reset_filter) {
|
||||||
_flags._reset_filter = false;
|
_reset_filter = false;
|
||||||
_input = input;
|
_error = _target - measurement;
|
||||||
|
_derivative.zero();
|
||||||
|
} else {
|
||||||
|
Vector2f error_last{_error};
|
||||||
|
_error += ((_target - measurement) - _error) * get_filt_E_alpha();
|
||||||
|
|
||||||
|
// calculate and filter derivative
|
||||||
|
if (_dt > 0.0f) {
|
||||||
|
const Vector2f derivative{(_error - error_last) / _dt};
|
||||||
|
_derivative += (derivative - _derivative) * get_filt_D_alpha();
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// update filter and calculate derivative
|
// update I term
|
||||||
const Vector2f input_delta = (input - _input) * _filt_alpha;
|
update_i(limit);
|
||||||
_input += input_delta;
|
|
||||||
|
|
||||||
set_input_filter_d(input_delta);
|
_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;
|
||||||
|
|
||||||
|
_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;
|
||||||
|
|
||||||
|
return _error * _kp + _integrator + _derivative * _kd + _target * _kff;
|
||||||
}
|
}
|
||||||
|
|
||||||
// set_input_filter_d - set input to PID controller
|
Vector2f AC_PID_2D::update_all(const Vector3f &target, const Vector3f &measurement, bool limit)
|
||||||
// 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_2D::set_input_filter_d(const Vector2f& input_delta)
|
|
||||||
{
|
{
|
||||||
// don't process inf or NaN
|
return update_all(Vector2f{target.x, target.y}, Vector2f{measurement.x, measurement.y}, limit);
|
||||||
if (!isfinite(input_delta.x) && !isfinite(input_delta.y)) {
|
|
||||||
return;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// update filter and calculate derivative
|
// update_i - update the integral
|
||||||
if (is_positive(_dt)) {
|
// If the limit flag is set the integral is only allowed to shrink
|
||||||
const Vector2f derivative = input_delta / _dt;
|
void AC_PID_2D::update_i(bool limit)
|
||||||
const Vector2f delta_derivative = (derivative - _derivative) * _filt_alpha_d;
|
|
||||||
_derivative += delta_derivative;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
Vector2f AC_PID_2D::get_p() const
|
|
||||||
{
|
{
|
||||||
return (_input * _kp);
|
float integrator_length_orig = _kimax;
|
||||||
|
if (limit) {
|
||||||
|
integrator_length_orig = MIN(integrator_length_orig, _integrator.length());
|
||||||
}
|
}
|
||||||
|
_integrator += (_error * _ki) * _dt;
|
||||||
Vector2f AC_PID_2D::get_i()
|
|
||||||
{
|
|
||||||
if (!is_zero(_ki) && !is_zero(_dt)) {
|
|
||||||
_integrator += (_input * _ki) * _dt;
|
|
||||||
const float integrator_length = _integrator.length();
|
|
||||||
if ((integrator_length > _imax) && is_positive(integrator_length)) {
|
|
||||||
_integrator *= (_imax / integrator_length);
|
|
||||||
}
|
|
||||||
return _integrator;
|
|
||||||
}
|
|
||||||
return Vector2f();
|
|
||||||
}
|
|
||||||
|
|
||||||
// get_i_shrink - get_i but do not allow integrator to grow in length (it may shrink)
|
|
||||||
Vector2f AC_PID_2D::get_i_shrink()
|
|
||||||
{
|
|
||||||
if (!is_zero(_ki) && !is_zero(_dt)) {
|
|
||||||
const float integrator_length_orig = MIN(_integrator.length(), _imax);
|
|
||||||
_integrator += (_input * _ki) * _dt;
|
|
||||||
const float integrator_length_new = _integrator.length();
|
const float integrator_length_new = _integrator.length();
|
||||||
if ((integrator_length_new > integrator_length_orig) && is_positive(integrator_length_new)) {
|
if (integrator_length_new > integrator_length_orig) {
|
||||||
_integrator *= (integrator_length_orig / integrator_length_new);
|
_integrator *= (integrator_length_orig / integrator_length_new);
|
||||||
}
|
}
|
||||||
return _integrator;
|
|
||||||
}
|
|
||||||
return Vector2f();
|
|
||||||
}
|
|
||||||
|
|
||||||
Vector2f AC_PID_2D::get_d()
|
|
||||||
{
|
|
||||||
// derivative component
|
|
||||||
return Vector2f(_kd * _derivative.x, _kd * _derivative.y);
|
|
||||||
}
|
|
||||||
|
|
||||||
Vector2f AC_PID_2D::get_pid()
|
|
||||||
{
|
|
||||||
return get_p() + get_i() + get_d();
|
|
||||||
}
|
|
||||||
|
|
||||||
void AC_PID_2D::reset_I()
|
|
||||||
{
|
|
||||||
_integrator.zero();
|
|
||||||
}
|
|
||||||
|
|
||||||
void AC_PID_2D::reset_filter()
|
|
||||||
{
|
|
||||||
_flags._reset_filter = true;
|
|
||||||
_derivative.x = 0.0f;
|
|
||||||
_derivative.y = 0.0f;
|
|
||||||
_integrator.zero();
|
|
||||||
}
|
|
||||||
|
|
||||||
void AC_PID_2D::load_gains()
|
|
||||||
{
|
|
||||||
_kp.load();
|
|
||||||
_ki.load();
|
|
||||||
_kd.load();
|
|
||||||
_imax.load();
|
|
||||||
_imax = fabsf(_imax);
|
|
||||||
_filt_hz.load();
|
|
||||||
_filt_d_hz.load();
|
|
||||||
|
|
||||||
// calculate the input filter alpha
|
|
||||||
calc_filt_alpha();
|
|
||||||
calc_filt_alpha_d();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// save_gains - save gains to eeprom
|
// save_gains - save gains to eeprom
|
||||||
|
@ -215,33 +147,40 @@ void AC_PID_2D::save_gains()
|
||||||
_kp.save();
|
_kp.save();
|
||||||
_ki.save();
|
_ki.save();
|
||||||
_kd.save();
|
_kd.save();
|
||||||
_imax.save();
|
_kff.save();
|
||||||
_filt_hz.save();
|
_kimax.save();
|
||||||
_filt_d_hz.save();
|
_filt_E_hz.save();
|
||||||
|
_filt_D_hz.save();
|
||||||
}
|
}
|
||||||
|
|
||||||
// calc_filt_alpha - recalculate the input filter alpha
|
// get the target filter alpha
|
||||||
void AC_PID_2D::calc_filt_alpha()
|
float AC_PID_2D::get_filt_E_alpha() const
|
||||||
{
|
{
|
||||||
if (is_zero(_filt_hz)) {
|
return calc_lowpass_alpha_dt(_dt, _filt_E_hz);
|
||||||
_filt_alpha = 1.0f;
|
|
||||||
return;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// calculate alpha
|
// get the derivative filter alpha
|
||||||
const float rc = 1/(M_2PI*_filt_hz);
|
float AC_PID_2D::get_filt_D_alpha() const
|
||||||
_filt_alpha = _dt / (_dt + rc);
|
|
||||||
}
|
|
||||||
|
|
||||||
// calc_filt_alpha - recalculate the input filter alpha
|
|
||||||
void AC_PID_2D::calc_filt_alpha_d()
|
|
||||||
{
|
{
|
||||||
if (is_zero(_filt_d_hz)) {
|
return calc_lowpass_alpha_dt(_dt, _filt_D_hz);
|
||||||
_filt_alpha_d = 1.0f;
|
|
||||||
return;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// calculate alpha
|
void AC_PID_2D::set_integrator(const Vector2f& target, const Vector2f& measurement, const Vector2f& i)
|
||||||
const float rc = 1/(M_2PI*_filt_d_hz);
|
{
|
||||||
_filt_alpha_d = _dt / (_dt + rc);
|
set_integrator(target - measurement, i);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void AC_PID_2D::set_integrator(const Vector2f& error, const Vector2f& i)
|
||||||
|
{
|
||||||
|
set_integrator(i - error * _kp);
|
||||||
|
}
|
||||||
|
|
||||||
|
void AC_PID_2D::set_integrator(const Vector2f& i)
|
||||||
|
{
|
||||||
|
_integrator = i;
|
||||||
|
const float integrator_length = _integrator.length();
|
||||||
|
if (integrator_length > _kimax) {
|
||||||
|
_integrator *= (_kimax / integrator_length);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
|
@ -7,6 +7,7 @@
|
||||||
#include <AP_Param/AP_Param.h>
|
#include <AP_Param/AP_Param.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
#include <cmath>
|
#include <cmath>
|
||||||
|
#include <AP_Logger/AP_Logger.h>
|
||||||
|
|
||||||
/// @class AC_PID_2D
|
/// @class AC_PID_2D
|
||||||
/// @brief Copter PID control class
|
/// @brief Copter PID control class
|
||||||
|
@ -14,32 +15,33 @@ class AC_PID_2D {
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Constructor for PID
|
// Constructor for PID
|
||||||
AC_PID_2D(float initial_p, float initial_i, float initial_d, 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, float dt);
|
||||||
|
|
||||||
// set_dt - set time step in seconds
|
// set time step in seconds
|
||||||
void set_dt(float dt);
|
void set_dt(float dt) { _dt = dt; }
|
||||||
|
|
||||||
// set_input - 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(const Vector2f &input);
|
// the integral is then updated based on the setting of the limit flag
|
||||||
void set_input(const Vector3f &input) { set_input(Vector2f(input.x, input.y)); }
|
Vector2f update_all(const Vector2f &target, const Vector2f &measurement, bool limit = false);
|
||||||
|
Vector2f update_all(const Vector3f &target, const Vector3f &measurement, bool limit = false);
|
||||||
|
|
||||||
// get_pi - get results from pid controller
|
// update the integral
|
||||||
Vector2f get_pid();
|
// if the limit flag is set the integral is only allowed to shrink
|
||||||
|
void update_i(bool limit);
|
||||||
|
|
||||||
|
// get results from pid controller
|
||||||
Vector2f get_p() const;
|
Vector2f get_p() const;
|
||||||
Vector2f get_i();
|
Vector2f get_i() const;
|
||||||
Vector2f get_i_shrink(); // get_i but do not allow integrator to grow (it may shrink)
|
Vector2f get_d() const;
|
||||||
Vector2f get_d();
|
Vector2f get_ff();
|
||||||
|
|
||||||
// reset_I - reset the integrator
|
// reset the integrator
|
||||||
void reset_I();
|
void reset_I() { _integrator.zero(); };
|
||||||
|
|
||||||
// reset_filter - input and D term filter will be reset to the next value provided to set_input()
|
// reset_filter - input and D term filter will be reset to the next value provided to set_input()
|
||||||
void reset_filter();
|
void reset_filter() { _reset_filter = true; }
|
||||||
|
|
||||||
// load gain from eeprom
|
|
||||||
void load_gains();
|
|
||||||
|
|
||||||
// save gain to eeprom
|
// save gain to eeprom
|
||||||
void save_gains();
|
void save_gains();
|
||||||
|
@ -47,58 +49,54 @@ public:
|
||||||
// get accessors
|
// get accessors
|
||||||
AP_Float &kP() { return _kp; }
|
AP_Float &kP() { return _kp; }
|
||||||
AP_Float &kI() { return _ki; }
|
AP_Float &kI() { return _ki; }
|
||||||
float imax() const { return _imax.get(); }
|
AP_Float &kD() { return _kd; }
|
||||||
float filt_hz() const { return _filt_hz.get(); }
|
AP_Float &ff() { return _kff;}
|
||||||
float get_filt_alpha() const { return _filt_alpha; }
|
AP_Float &filt_E_hz() { return _filt_E_hz; }
|
||||||
float filt_d_hz() const { return _filt_d_hz.get(); }
|
AP_Float &filt_D_hz() { return _filt_D_hz; }
|
||||||
float get_filt_alpha_D() const { return _filt_alpha_d; }
|
float imax() const { return _kimax.get(); }
|
||||||
|
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(float v) { _kp.set(v); }
|
||||||
void kI(const float v) { _ki.set(v); }
|
void kI(float v) { _ki.set(v); }
|
||||||
void kD(const float v) { _kd.set(v); }
|
void kD(float v) { _kd.set(v); }
|
||||||
void imax(const float v) { _imax.set(fabsf(v)); }
|
void ff(float v) { _kff.set(v); }
|
||||||
void filt_hz(const float v);
|
void imax(float v) { _kimax.set(fabsf(v)); }
|
||||||
void filt_d_hz(const float v);
|
void filt_E_hz(float hz) { _filt_E_hz.set(fabsf(hz)); }
|
||||||
|
void filt_D_hz(float hz) { _filt_D_hz.set(fabsf(hz)); }
|
||||||
|
|
||||||
Vector2f get_integrator() const { return _integrator; }
|
// integrator setting functions
|
||||||
void set_integrator(const Vector2f &i) { _integrator = i; }
|
void set_integrator(const Vector2f& target, const Vector2f& measurement, const Vector2f& i);
|
||||||
void set_integrator(const Vector3f &i) { _integrator.x = i.x; _integrator.y = i.y; }
|
void set_integrator(const Vector2f& error, const Vector2f& i);
|
||||||
|
void set_integrator(const Vector3f& i) { set_integrator(Vector2f(i.x, i.y)); }
|
||||||
|
void set_integrator(const Vector2f& i);
|
||||||
|
|
||||||
|
const AP_Logger::PID_Info& get_pid_info_x(void) const { return _pid_info_x; }
|
||||||
|
const AP_Logger::PID_Info& get_pid_info_y(void) const { return _pid_info_y; }
|
||||||
|
|
||||||
// parameter var table
|
// parameter var table
|
||||||
static const struct AP_Param::GroupInfo var_info[];
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
// 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(const Vector2f& input_delta);
|
|
||||||
|
|
||||||
// calc_filt_alpha - recalculate the input filter alpha
|
|
||||||
void calc_filt_alpha();
|
|
||||||
|
|
||||||
// calc_filt_alpha - recalculate the input filter alpha
|
|
||||||
void calc_filt_alpha_d();
|
|
||||||
|
|
||||||
// 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 _filt_d_hz; // D term 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
|
|
||||||
} _flags;
|
|
||||||
|
|
||||||
// internal variables
|
// internal variables
|
||||||
float _dt; // timestep in seconds
|
float _dt; // timestep in seconds
|
||||||
float _filt_alpha; // input filter alpha
|
Vector2f _target; // target value to enable filtering
|
||||||
float _filt_alpha_d; // input filter alpha
|
Vector2f _error; // error value to enable filtering
|
||||||
|
Vector2f _derivative; // last derivative from low-pass filter
|
||||||
Vector2f _integrator; // integrator value
|
Vector2f _integrator; // integrator value
|
||||||
Vector2f _input; // last input for derivative
|
bool _reset_filter; // true when input filter should be reset during next call to update_all
|
||||||
Vector2f _derivative; // last derivative for low-pass filter
|
|
||||||
|
AP_Logger::PID_Info _pid_info_x;
|
||||||
|
AP_Logger::PID_Info _pid_info_y;
|
||||||
};
|
};
|
||||||
|
|
|
@ -0,0 +1,179 @@
|
||||||
|
/// @file AC_PID_Basic.cpp
|
||||||
|
/// @brief Generic PID algorithm
|
||||||
|
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
|
#include <AP_InternalError/AP_InternalError.h>
|
||||||
|
#include "AC_PID_Basic.h"
|
||||||
|
|
||||||
|
#define AC_PID_Basic_FILT_E_HZ_DEFAULT 20.0f // default input filter frequency
|
||||||
|
#define AC_PID_Basic_FILT_E_HZ_MIN 0.01f // minimum input filter frequency
|
||||||
|
#define AC_PID_Basic_FILT_D_HZ_DEFAULT 10.0f // default input filter frequency
|
||||||
|
#define AC_PID_Basic_FILT_D_HZ_MIN 0.005f // minimum input filter frequency
|
||||||
|
|
||||||
|
const AP_Param::GroupInfo AC_PID_Basic::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_Basic, _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_Basic, _ki, 0),
|
||||||
|
|
||||||
|
// @Param: IMAX
|
||||||
|
// @DisplayName: PID Integral Maximum
|
||||||
|
// @Description: The maximum/minimum value that the I term can output
|
||||||
|
AP_GROUPINFO("IMAX", 2, AC_PID_Basic, _kimax, 0),
|
||||||
|
|
||||||
|
// @Param: FLTE
|
||||||
|
// @DisplayName: PID Error filter frequency in Hz
|
||||||
|
// @Description: Error filter frequency in Hz
|
||||||
|
// @Units: Hz
|
||||||
|
AP_GROUPINFO("FLTE", 3, AC_PID_Basic, _filt_E_hz, AC_PID_Basic_FILT_E_HZ_DEFAULT),
|
||||||
|
|
||||||
|
// @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", 4, AC_PID_Basic, _kd, 0),
|
||||||
|
|
||||||
|
// @Param: FLTD
|
||||||
|
// @DisplayName: D term filter frequency in Hz
|
||||||
|
// @Description: D term filter frequency in Hz
|
||||||
|
// @Units: Hz
|
||||||
|
AP_GROUPINFO("FLTD", 5, AC_PID_Basic, _filt_D_hz, AC_PID_Basic_FILT_D_HZ_DEFAULT),
|
||||||
|
|
||||||
|
// @Param: FF
|
||||||
|
// @DisplayName: PID Feed Forward Gain
|
||||||
|
// @Description: FF Gain which produces an output that is proportional to the magnitude of the target
|
||||||
|
AP_GROUPINFO("FF", 6, AC_PID_Basic, _kff, 0),
|
||||||
|
|
||||||
|
AP_GROUPEND
|
||||||
|
};
|
||||||
|
|
||||||
|
// 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)
|
||||||
|
{
|
||||||
|
// load parameter values from eeprom
|
||||||
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
|
|
||||||
|
_kp = initial_p;
|
||||||
|
_ki = initial_i;
|
||||||
|
_kd = initial_d;
|
||||||
|
_kff = initial_ff;
|
||||||
|
_kimax = fabsf(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;
|
||||||
|
}
|
||||||
|
|
||||||
|
float AC_PID_Basic::update_all(float target, float measurement, bool limit)
|
||||||
|
{
|
||||||
|
return update_all(target, measurement, (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)
|
||||||
|
{
|
||||||
|
// 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;
|
||||||
|
}
|
||||||
|
|
||||||
|
_target = target;
|
||||||
|
|
||||||
|
// reset input filter to value received
|
||||||
|
if (_reset_filter) {
|
||||||
|
_reset_filter = false;
|
||||||
|
_error = _target - measurement;
|
||||||
|
_derivative = 0.0f;
|
||||||
|
} else {
|
||||||
|
float error_last = _error;
|
||||||
|
_error += get_filt_E_alpha() * ((_target - measurement) - _error);
|
||||||
|
|
||||||
|
// calculate and filter derivative
|
||||||
|
if (is_positive(_dt)) {
|
||||||
|
float derivative = (_error - error_last) / _dt;
|
||||||
|
_derivative += get_filt_D_alpha() * (derivative - _derivative);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// update I term
|
||||||
|
update_i(limit_neg, limit_pos);
|
||||||
|
|
||||||
|
const float P_out = _error * _kp;
|
||||||
|
const float D_out = _derivative * _kd;
|
||||||
|
|
||||||
|
_pid_info.target = _target;
|
||||||
|
_pid_info.actual = measurement;
|
||||||
|
_pid_info.error = _error;
|
||||||
|
_pid_info.P = _error * _kp;
|
||||||
|
_pid_info.I = _integrator;
|
||||||
|
_pid_info.D = _derivative * _kd;
|
||||||
|
_pid_info.FF = _target * _kff;
|
||||||
|
|
||||||
|
return P_out + _integrator + D_out + _target * _kff;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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)
|
||||||
|
{
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// save_gains - save gains to eeprom
|
||||||
|
void AC_PID_Basic::save_gains()
|
||||||
|
{
|
||||||
|
_kp.save();
|
||||||
|
_ki.save();
|
||||||
|
_kd.save();
|
||||||
|
_kff.save();
|
||||||
|
_kimax.save();
|
||||||
|
_filt_E_hz.save();
|
||||||
|
_filt_D_hz.save();
|
||||||
|
}
|
||||||
|
|
||||||
|
// get_filt_T_alpha - get the target filter alpha
|
||||||
|
float AC_PID_Basic::get_filt_E_alpha() const
|
||||||
|
{
|
||||||
|
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
|
||||||
|
{
|
||||||
|
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);
|
||||||
|
}
|
|
@ -0,0 +1,97 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
/// @file AC_PID_Basic.h
|
||||||
|
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
|
||||||
|
|
||||||
|
#include <AP_Common/AP_Common.h>
|
||||||
|
#include <AP_Param/AP_Param.h>
|
||||||
|
#include <AP_Logger/AP_Logger.h>
|
||||||
|
|
||||||
|
/// @class AC_PID_Basic
|
||||||
|
/// @brief Copter PID control class
|
||||||
|
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; }
|
||||||
|
|
||||||
|
// 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;
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
|
// get results from pid controller
|
||||||
|
float get_p() const WARN_IF_UNUSED { return _error * _kp; }
|
||||||
|
float get_i() const WARN_IF_UNUSED { return _integrator; }
|
||||||
|
float get_d() const WARN_IF_UNUSED { return _derivative * _kd; }
|
||||||
|
float get_ff() const WARN_IF_UNUSED { return _target * _kff; }
|
||||||
|
|
||||||
|
// reset the integrator
|
||||||
|
void reset_I() { _integrator = 0.0f; }
|
||||||
|
|
||||||
|
// input and D term filter will be reset to the next value provided to set_input()
|
||||||
|
void reset_filter() { _reset_filter = true; }
|
||||||
|
|
||||||
|
// save gain to eeprom
|
||||||
|
void save_gains();
|
||||||
|
|
||||||
|
// get accessors
|
||||||
|
AP_Float &kP() WARN_IF_UNUSED { return _kp; }
|
||||||
|
AP_Float &kI() WARN_IF_UNUSED { return _ki; }
|
||||||
|
AP_Float &kD() WARN_IF_UNUSED { return _kd; }
|
||||||
|
AP_Float &ff() WARN_IF_UNUSED { return _kff;}
|
||||||
|
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;
|
||||||
|
|
||||||
|
// set accessors
|
||||||
|
void kP(float v) { _kp.set(v); }
|
||||||
|
void kI(float v) { _ki.set(v); }
|
||||||
|
void kD(float v) { _kd.set(v); }
|
||||||
|
void ff(float v) { _kff.set(v); }
|
||||||
|
void imax(float v) { _kimax.set(fabsf(v)); }
|
||||||
|
void filt_E_hz(float hz) { _filt_E_hz.set(fabsf(hz)); }
|
||||||
|
void filt_D_hz(float hz) { _filt_D_hz.set(fabsf(hz)); }
|
||||||
|
|
||||||
|
// integrator setting functions
|
||||||
|
void set_integrator(float target, float measurement, float i);
|
||||||
|
void set_integrator(float error, float i);
|
||||||
|
void set_integrator(float i);
|
||||||
|
|
||||||
|
const AP_Logger::PID_Info& get_pid_info(void) const WARN_IF_UNUSED { return _pid_info; }
|
||||||
|
|
||||||
|
// parameter var table
|
||||||
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
// parameters
|
||||||
|
AP_Float _kp;
|
||||||
|
AP_Float _ki;
|
||||||
|
AP_Float _kd;
|
||||||
|
AP_Float _kff;
|
||||||
|
AP_Float _kimax;
|
||||||
|
AP_Float _filt_E_hz; // PID error filter frequency in Hz
|
||||||
|
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
|
||||||
|
float _integrator; // integrator value
|
||||||
|
bool _reset_filter; // true when input filter should be reset during next call to set_input
|
||||||
|
|
||||||
|
AP_Logger::PID_Info _pid_info;
|
||||||
|
};
|
|
@ -1,27 +1,27 @@
|
||||||
/// @file AC_PI_2D.cpp
|
/// @file AC_PI_2D.cpp
|
||||||
/// @brief Generic PID algorithm
|
/// @brief 2-axis PI controller
|
||||||
|
|
||||||
#include <AP_Math/AP_Math.h>
|
#include <AP_Math/AP_Math.h>
|
||||||
#include "AC_PI_2D.h"
|
#include "AC_PI_2D.h"
|
||||||
|
|
||||||
const AP_Param::GroupInfo AC_PI_2D::var_info[] = {
|
const AP_Param::GroupInfo AC_PI_2D::var_info[] = {
|
||||||
// @Param: P
|
// @Param: P
|
||||||
// @DisplayName: PID Proportional Gain
|
// @DisplayName: PI 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_PI_2D, _kp, 0),
|
AP_GROUPINFO("P", 0, AC_PI_2D, _kp, 0),
|
||||||
|
|
||||||
// @Param: I
|
// @Param: I
|
||||||
// @DisplayName: PID Integral Gain
|
// @DisplayName: PI 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_PI_2D, _ki, 0),
|
AP_GROUPINFO("I", 1, AC_PI_2D, _ki, 0),
|
||||||
|
|
||||||
// @Param: IMAX
|
// @Param: IMAX
|
||||||
// @DisplayName: PID Integral Maximum
|
// @DisplayName: PI 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", 2, AC_PI_2D, _imax, 0),
|
AP_GROUPINFO("IMAX", 2, AC_PI_2D, _imax, 0),
|
||||||
|
|
||||||
// @Param: FILT_HZ
|
// @Param: FILT_HZ
|
||||||
// @DisplayName: PID Input filter frequency in Hz
|
// @DisplayName: PI Input filter frequency in Hz
|
||||||
// @Description: Input filter frequency in Hz
|
// @Description: Input filter frequency in Hz
|
||||||
// @Units: Hz
|
// @Units: Hz
|
||||||
AP_GROUPINFO("FILT_HZ", 3, AC_PI_2D, _filt_hz, AC_PI_2D_FILT_HZ_DEFAULT),
|
AP_GROUPINFO("FILT_HZ", 3, AC_PI_2D, _filt_hz, AC_PI_2D_FILT_HZ_DEFAULT),
|
||||||
|
@ -101,7 +101,7 @@ Vector2f AC_PI_2D::get_i()
|
||||||
}
|
}
|
||||||
return _integrator;
|
return _integrator;
|
||||||
}
|
}
|
||||||
return Vector2f();
|
return Vector2f{};
|
||||||
}
|
}
|
||||||
|
|
||||||
// get_i_shrink - get_i but do not allow integrator to grow in length (it may shrink)
|
// get_i_shrink - get_i but do not allow integrator to grow in length (it may shrink)
|
||||||
|
@ -116,7 +116,7 @@ Vector2f AC_PI_2D::get_i_shrink()
|
||||||
}
|
}
|
||||||
return _integrator;
|
return _integrator;
|
||||||
}
|
}
|
||||||
return Vector2f();
|
return Vector2f{};
|
||||||
}
|
}
|
||||||
|
|
||||||
Vector2f AC_PI_2D::get_pi()
|
Vector2f AC_PI_2D::get_pi()
|
||||||
|
|
|
@ -1,7 +1,7 @@
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
/// @file AC_PI_2D.h
|
/// @file AC_PI_2D.h
|
||||||
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
|
/// @brief 2-axis PI controller with EEPROM-backed storage of constants.
|
||||||
|
|
||||||
#include <AP_Common/AP_Common.h>
|
#include <AP_Common/AP_Common.h>
|
||||||
#include <AP_Param/AP_Param.h>
|
#include <AP_Param/AP_Param.h>
|
||||||
|
@ -12,18 +12,18 @@
|
||||||
#define AC_PI_2D_FILT_HZ_MIN 0.01f // minimum input filter frequency
|
#define AC_PI_2D_FILT_HZ_MIN 0.01f // minimum input filter frequency
|
||||||
|
|
||||||
/// @class AC_PI_2D
|
/// @class AC_PI_2D
|
||||||
/// @brief Copter PID control class
|
/// @brief 2-axis PI controller
|
||||||
class AC_PI_2D {
|
class AC_PI_2D {
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// Constructor for PID
|
// constructor
|
||||||
AC_PI_2D(float initial_p, float initial_i, float initial_imax, float initial_filt_hz, float dt);
|
AC_PI_2D(float initial_p, float initial_i, float initial_imax, float initial_filt_hz, float dt);
|
||||||
|
|
||||||
// set time step in seconds
|
// set time step in seconds
|
||||||
void set_dt(float dt);
|
void set_dt(float dt);
|
||||||
|
|
||||||
// set_input - set input to PID controller
|
// set_input - set input to PI controller
|
||||||
// input is filtered before the PID controllers are run
|
// input is filtered before the PI controllers are run
|
||||||
// this should be called before any other calls to get_p, get_i or get_d
|
// this should be called before any other calls to get_p, get_i or get_d
|
||||||
void set_input(const Vector2f &input);
|
void set_input(const Vector2f &input);
|
||||||
void set_input(const Vector3f &input) { set_input(Vector2f(input.x, input.y)); }
|
void set_input(const Vector3f &input) { set_input(Vector2f(input.x, input.y)); }
|
||||||
|
@ -78,7 +78,7 @@ private:
|
||||||
AP_Float _kp;
|
AP_Float _kp;
|
||||||
AP_Float _ki;
|
AP_Float _ki;
|
||||||
AP_Float _imax;
|
AP_Float _imax;
|
||||||
AP_Float _filt_hz; // PID Input filter frequency in Hz
|
AP_Float _filt_hz; // PI Input filter frequency in Hz
|
||||||
|
|
||||||
// flags
|
// flags
|
||||||
struct ac_pid_flags {
|
struct ac_pid_flags {
|
||||||
|
|
|
@ -0,0 +1,65 @@
|
||||||
|
/// @file AC_P_1D.cpp
|
||||||
|
/// @brief Generic P algorithm
|
||||||
|
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
|
#include "AC_P_1D.h"
|
||||||
|
|
||||||
|
const AP_Param::GroupInfo AC_P_1D::var_info[] = {
|
||||||
|
// @Param: P
|
||||||
|
// @DisplayName: P Proportional Gain
|
||||||
|
// @Description: P Gain which produces an output value that is proportional to the current error value
|
||||||
|
AP_GROUPINFO("P", 0, AC_P_1D, _kp, 0),
|
||||||
|
AP_GROUPEND
|
||||||
|
};
|
||||||
|
|
||||||
|
// Constructor
|
||||||
|
AC_P_1D::AC_P_1D(float initial_p, float dt) :
|
||||||
|
_dt(dt)
|
||||||
|
{
|
||||||
|
// load parameter values from eeprom
|
||||||
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
|
|
||||||
|
_kp = initial_p;
|
||||||
|
_lim_D_Out = 10.0f; // maximum first differential of output
|
||||||
|
}
|
||||||
|
|
||||||
|
// update_all - set target and measured inputs to P controller and calculate outputs
|
||||||
|
// target and measurement are filtered
|
||||||
|
// if measurement is further than error_min or error_max (see set_limits_error method)
|
||||||
|
// the target is moved closer to the measurement and limit_min or limit_max will be set true
|
||||||
|
float AC_P_1D::update_all(float &target, float measurement, bool &limit_min, bool &limit_max)
|
||||||
|
{
|
||||||
|
limit_min = false;
|
||||||
|
limit_max = false;
|
||||||
|
|
||||||
|
// calculate distance _error
|
||||||
|
float error = target - measurement;
|
||||||
|
|
||||||
|
if (error < _lim_err_min) {
|
||||||
|
error = _lim_err_min;
|
||||||
|
target = measurement + error;
|
||||||
|
limit_min = true;
|
||||||
|
} else if (error > _lim_err_max) {
|
||||||
|
error = _lim_err_max;
|
||||||
|
target = measurement + error;
|
||||||
|
limit_max = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// ToDo: Replace sqrt_controller with optimal acceleration and jerk limited curve
|
||||||
|
// MIN(_Dxy_max, _D2xy_max / _kxy_P) limits the max accel to the point where max jerk is exceeded
|
||||||
|
return sqrt_controller(error, _kp, _lim_D_Out, _dt);
|
||||||
|
}
|
||||||
|
|
||||||
|
// set limits on error, output and output from D term
|
||||||
|
// in normal use the lim_err_min and lim_out_min will be negative
|
||||||
|
// when using for a position controller, lim_err will be position error, lim_out will be correction velocity, lim_D will be acceleration, lim_D2 will be jerk
|
||||||
|
void AC_P_1D::set_limits_error(float lim_err_min, float lim_err_max, float lim_out_min, float lim_out_max, float lim_D_Out, float lim_D2_Out)
|
||||||
|
{
|
||||||
|
_lim_D_Out = lim_D_Out;
|
||||||
|
if (is_positive(lim_D2_Out)) {
|
||||||
|
// limit the first derivative so as not to exceed the second derivative
|
||||||
|
_lim_D_Out = MIN(_lim_D_Out, lim_D2_Out / _kp);
|
||||||
|
}
|
||||||
|
_lim_err_min = MAX(inv_sqrt_controller(lim_out_min, _kp, _lim_D_Out), lim_err_min);
|
||||||
|
_lim_err_max = MAX(inv_sqrt_controller(lim_out_max, _kp, _lim_D_Out), lim_err_max);
|
||||||
|
}
|
|
@ -0,0 +1,50 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
/// @file AC_P_1D.h
|
||||||
|
/// @brief Generic P controller, with EEPROM-backed storage of constants.
|
||||||
|
|
||||||
|
#include <AP_Common/AP_Common.h>
|
||||||
|
#include <AP_Param/AP_Param.h>
|
||||||
|
|
||||||
|
/// @class AC_P_1D
|
||||||
|
/// @brief Object managing one P controller
|
||||||
|
class AC_P_1D {
|
||||||
|
public:
|
||||||
|
|
||||||
|
// constructor
|
||||||
|
AC_P_1D(float initial_p, float dt);
|
||||||
|
|
||||||
|
// 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
|
||||||
|
// if measurement is further than error_min or error_max (see set_limits_error method)
|
||||||
|
// the target is moved closer to the measurement and limit_min or limit_max will be set true
|
||||||
|
float update_all(float &target, float measurement, bool &limit_min, bool &limit_max) WARN_IF_UNUSED;
|
||||||
|
|
||||||
|
// save gain to eeprom
|
||||||
|
void save_gains() { _kp.save(); }
|
||||||
|
|
||||||
|
// set limits on error, output and output from D term
|
||||||
|
void set_limits_error(float error_min, float error_max, float output_min, float output_max, float D_Out_max = 0.0f, float D2_Out_max = 0.0f);
|
||||||
|
|
||||||
|
// accessors
|
||||||
|
AP_Float &kP() WARN_IF_UNUSED { return _kp; }
|
||||||
|
const AP_Float &kP() const WARN_IF_UNUSED { return _kp; }
|
||||||
|
void kP(float v) { _kp.set(v); }
|
||||||
|
|
||||||
|
// parameter var table
|
||||||
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
// parameters
|
||||||
|
AP_Float _kp;
|
||||||
|
|
||||||
|
// internal variables
|
||||||
|
float _dt; // time step in seconds
|
||||||
|
float _lim_err_min; // error limit in negative direction
|
||||||
|
float _lim_err_max; // error limit in positive direction
|
||||||
|
float _lim_D_Out; // maximum first differential of output
|
||||||
|
};
|
|
@ -0,0 +1,41 @@
|
||||||
|
/// @file AC_P_2D.cpp
|
||||||
|
/// @brief 2-axis P controller
|
||||||
|
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
|
#include "AC_P_2D.h"
|
||||||
|
|
||||||
|
const AP_Param::GroupInfo AC_P_2D::var_info[] = {
|
||||||
|
// @Param: P
|
||||||
|
// @DisplayName: Proportional Gain
|
||||||
|
// @Description: P Gain which produces an output value that is proportional to the current error value
|
||||||
|
AP_GROUPINFO("P", 0, AC_P_2D, _kp, 0),
|
||||||
|
AP_GROUPEND
|
||||||
|
};
|
||||||
|
|
||||||
|
// Constructor
|
||||||
|
AC_P_2D::AC_P_2D(float initial_p, float dt) :
|
||||||
|
_dt(dt)
|
||||||
|
{
|
||||||
|
// load parameter values from eeprom
|
||||||
|
AP_Param::setup_object_defaults(this, var_info);
|
||||||
|
|
||||||
|
_kp = initial_p;
|
||||||
|
}
|
||||||
|
|
||||||
|
// set target and measured inputs to P controller and calculate outputs
|
||||||
|
Vector2f AC_P_2D::update_all(float &target_x, float &target_y, const Vector2f &measurement, float error_max, float D2_max)
|
||||||
|
{
|
||||||
|
// calculate distance _error
|
||||||
|
Vector2f error = Vector2f(target_x, target_y) - measurement;
|
||||||
|
|
||||||
|
// Constrain _error and target position
|
||||||
|
// Constrain the maximum length of _vel_target to the maximum position correction velocity
|
||||||
|
if (error.limit_length(error_max)) {
|
||||||
|
target_x = measurement.x + error.x;
|
||||||
|
target_y = measurement.y + error.y;
|
||||||
|
}
|
||||||
|
|
||||||
|
// MIN(_Dmax, _D2max / _kp) limits the max accel to the point where max jerk is exceeded
|
||||||
|
// return sqrt_controller(Vector2f(_error.x, _error.y), _kp, MIN(_D_max, _D2_max / _kp), _dt);
|
||||||
|
return sqrt_controller(error, _kp, D2_max, _dt);
|
||||||
|
}
|
|
@ -0,0 +1,45 @@
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
/// @file AC_P_2D.h
|
||||||
|
/// @brief 2-axis P controller with EEPROM-backed storage of constants.
|
||||||
|
|
||||||
|
#include <AP_Common/AP_Common.h>
|
||||||
|
#include <AP_Param/AP_Param.h>
|
||||||
|
|
||||||
|
/// @class AC_P_2D
|
||||||
|
/// @brief 2-axis P controller
|
||||||
|
class AC_P_2D {
|
||||||
|
public:
|
||||||
|
|
||||||
|
// constructor
|
||||||
|
AC_P_2D(float initial_p, float dt);
|
||||||
|
|
||||||
|
// 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(float &target_x, float &target_y, const Vector2f &measurement, float error_max, float D2_max) WARN_IF_UNUSED;
|
||||||
|
|
||||||
|
// set target and measured inputs to P controller and calculate outputs
|
||||||
|
// measurement is provided as 3-axis vector but only x and y are used
|
||||||
|
Vector2f update_all(float &target_x, float &target_y, const Vector3f &measurement, float error_max, float D2_max) WARN_IF_UNUSED {
|
||||||
|
return update_all(target_x, target_y, Vector2f(measurement.x, measurement.y), error_max, D2_max);
|
||||||
|
}
|
||||||
|
|
||||||
|
// get accessors
|
||||||
|
AP_Float &kP() WARN_IF_UNUSED { return _kp; }
|
||||||
|
const AP_Float &kP() const WARN_IF_UNUSED { return _kp; }
|
||||||
|
|
||||||
|
// set accessor
|
||||||
|
void kP(float v) { _kp.set(v); }
|
||||||
|
|
||||||
|
// parameter var table
|
||||||
|
static const struct AP_Param::GroupInfo var_info[];
|
||||||
|
|
||||||
|
private:
|
||||||
|
// parameters
|
||||||
|
AP_Float _kp;
|
||||||
|
|
||||||
|
// internal variables
|
||||||
|
float _dt; // time step in seconds
|
||||||
|
};
|
Loading…
Reference in New Issue