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

AC_PID: library update and additional functions 

includes corrections from peer review
This commit is contained in:
Leonard Hall 2020-01-04 16:55:34 +10:30 committed by Randy Mackay
parent bc0da915c4
commit 1d720cc5e9
11 changed files with 660 additions and 246 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AC_PID/AC_P.h
View File

@ -1,7 +1,7 @@
#pragma once
/// @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_Param/AP_Param.h>

273
libraries/AC_PID/AC_PID_2D.cpp
View File

@ -23,13 +23,13 @@ const AP_Param::GroupInfo AC_PID_2D::var_info[] = {
// @Param: IMAX
// @DisplayName: PID Integral Maximum
// @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
// @DisplayName: PID Input filter frequency in Hz
// @Description: Input filter frequency in 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
// @DisplayName: PID Derivative Gain
@ -40,173 +40,105 @@ const AP_Param::GroupInfo AC_PID_2D::var_info[] = {
// @DisplayName: D term filter frequency in Hz
// @Description: D term filter frequency in 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
};
// 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)
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
_kp = initial_p;
_ki = initial_i;
_kd = initial_d;
_imax = fabsf(initial_imax);
filt_hz(initial_filt_hz);
filt_d_hz(initial_filt_d_hz);
_kp = initial_kP;
_ki = initial_kI;
_kd = initial_kD;
_kff = initial_kFF;
_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 and derivitive to zero
reset_filter();
// reset input filter to first value received
_reset_filter = true;
}
// set_dt - set time step in seconds
void AC_PID_2D::set_dt(float dt)
{
// set dt and calculate the input filter alpha
_dt = dt;
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)
// 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
Vector2f AC_PID_2D::update_all(const Vector2f &target, const Vector2f &measurement, bool limit)
{
// don't process inf or NaN
if (!isfinite(input.x) || !isfinite(input.y)) {
return;
if (target.is_nan() || target.is_inf() ||
measurement.is_nan() || measurement.is_inf()) {
return Vector2f{};
}
_target = target;
// reset input filter to value received
if (_flags._reset_filter) {
_flags._reset_filter = false;
_input = input;
}
if (_reset_filter) {
_reset_filter = false;
_error = _target - measurement;
_derivative.zero();
} else {
Vector2f error_last{_error};
_error += ((_target - measurement) - _error) * get_filt_E_alpha();
// update filter and calculate derivative
const Vector2f input_delta = (input - _input) * _filt_alpha;
_input += input_delta;
set_input_filter_d(input_delta);
}
// 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_2D::set_input_filter_d(const Vector2f& input_delta)
{
// don't process inf or NaN
if (!isfinite(input_delta.x) && !isfinite(input_delta.y)) {
return;
}
// update filter and calculate derivative
if (is_positive(_dt)) {
const Vector2f derivative = input_delta / _dt;
const Vector2f delta_derivative = (derivative - _derivative) * _filt_alpha_d;
_derivative += delta_derivative;
}
}
Vector2f AC_PID_2D::get_p() const
{
return (_input * _kp);
}
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);
// calculate and filter derivative
if (_dt > 0.0f) {
const Vector2f derivative{(_error - error_last) / _dt};
_derivative += (derivative - _derivative) * get_filt_D_alpha();
}
return _integrator;
}
return Vector2f();
// update I term
update_i(limit);
_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;
}
// get_i_shrink - get_i but do not allow integrator to grow in length (it may shrink)
Vector2f AC_PID_2D::get_i_shrink()
Vector2f AC_PID_2D::update_all(const Vector3f &target, const Vector3f &measurement, bool limit)
{
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();
if ((integrator_length_new > integrator_length_orig) && is_positive(integrator_length_new)) {
_integrator *= (integrator_length_orig / integrator_length_new);
}
return _integrator;
return update_all(Vector2f{target.x, target.y}, Vector2f{measurement.x, measurement.y}, limit);
}
// update_i - update the integral
// If the limit flag is set the integral is only allowed to shrink
void AC_PID_2D::update_i(bool limit)
{
float integrator_length_orig = _kimax;
if (limit) {
integrator_length_orig = MIN(integrator_length_orig, _integrator.length());
}
_integrator += (_error * _ki) * _dt;
const float integrator_length_new = _integrator.length();
if (integrator_length_new > integrator_length_orig) {
_integrator *= (integrator_length_orig / integrator_length_new);
}
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
@ -215,33 +147,40 @@ void AC_PID_2D::save_gains()
_kp.save();
_ki.save();
_kd.save();
_imax.save();
_filt_hz.save();
_filt_d_hz.save();
_kff.save();
_kimax.save();
_filt_E_hz.save();
_filt_D_hz.save();
}
// calc_filt_alpha - recalculate the input filter alpha
void AC_PID_2D::calc_filt_alpha()
// get the target filter alpha
float AC_PID_2D::get_filt_E_alpha() const
{
if (is_zero(_filt_hz)) {
_filt_alpha = 1.0f;
return;
}
// calculate alpha
const float rc = 1/(M_2PI*_filt_hz);
_filt_alpha = _dt / (_dt + rc);
return calc_lowpass_alpha_dt(_dt, _filt_E_hz);
}
// calc_filt_alpha - recalculate the input filter alpha
void AC_PID_2D::calc_filt_alpha_d()
// get the derivative filter alpha
float AC_PID_2D::get_filt_D_alpha() const
{
if (is_zero(_filt_d_hz)) {
_filt_alpha_d = 1.0f;
return;
}
// calculate alpha
const float rc = 1/(M_2PI*_filt_d_hz);
_filt_alpha_d = _dt / (_dt + rc);
return calc_lowpass_alpha_dt(_dt, _filt_D_hz);
}
void AC_PID_2D::set_integrator(const Vector2f& target, const Vector2f& measurement, const Vector2f& i)
{
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);
}
}

128
libraries/AC_PID/AC_PID_2D.h
View File

@ -7,6 +7,7 @@
#include <AP_Param/AP_Param.h>
#include <stdlib.h>
#include <cmath>
#include <AP_Logger/AP_Logger.h>
/// @class AC_PID_2D
/// @brief Copter PID control class
@ -14,91 +15,88 @@ class AC_PID_2D {
public:
// 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
void set_dt(float dt);
// set time step in seconds
void set_dt(float dt) { _dt = dt; }
// 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 set_input(const Vector2f &input);
void set_input(const Vector3f &input) { set_input(Vector2f(input.x, input.y)); }
// 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
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
Vector2f get_pid();
Vector2f get_p() const;
Vector2f get_i();
Vector2f get_i_shrink(); // get_i but do not allow integrator to grow (it may shrink)
Vector2f get_d();
// update the integral
// if the limit flag is set the integral is only allowed to shrink
void update_i(bool limit);
// reset_I - reset the integrator
void reset_I();
// get results from pid controller
Vector2f get_p() const;
Vector2f get_i() const;
Vector2f get_d() const;
Vector2f get_ff();
// reset the integrator
void reset_I() { _integrator.zero(); };
// reset_filter - input and D term filter will be reset to the next value provided to set_input()
void reset_filter();
// load gain from eeprom
void load_gains();
void reset_filter() { _reset_filter = true; }
// save gain to eeprom
void save_gains();
void save_gains();
// get accessors
AP_Float &kP() { return _kp; }
AP_Float &kI() { return _ki; }
float imax() const { return _imax.get(); }
float filt_hz() const { return _filt_hz.get(); }
float get_filt_alpha() const { return _filt_alpha; }
float filt_d_hz() const { return _filt_d_hz.get(); }
float get_filt_alpha_D() const { return _filt_alpha_d; }
AP_Float &kP() { return _kp; }
AP_Float &kI() { return _ki; }
AP_Float &kD() { return _kd; }
AP_Float &ff() { return _kff;}
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;
// 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 filt_d_hz(const float v);
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)); }
Vector2f get_integrator() const { return _integrator; }
void set_integrator(const Vector2f &i) { _integrator = i; }
void set_integrator(const Vector3f &i) { _integrator.x = i.x; _integrator.y = i.y; }
// integrator setting functions
void set_integrator(const Vector2f& target, const Vector2f& measurement, const Vector2f& i);
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
static const struct AP_Param::GroupInfo var_info[];
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
AP_Float _kp;
AP_Float _ki;
AP_Float _kd;
AP_Float _imax;
AP_Float _filt_hz; // PID Input filter frequency in Hz
AP_Float _filt_d_hz; // D term 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;
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 _filt_alpha; // input filter alpha
float _filt_alpha_d; // input filter alpha
Vector2f _integrator; // integrator value
Vector2f _input; // last input for derivative
Vector2f _derivative; // last derivative for low-pass filter
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
Vector2f _integrator; // integrator value
bool _reset_filter; // true when input filter should be reset during next call to update_all
AP_Logger::PID_Info _pid_info_x;
AP_Logger::PID_Info _pid_info_y;
};

179
libraries/AC_PID/AC_PID_Basic.cpp Normal file
View File

@ -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);
}

97
libraries/AC_PID/AC_PID_Basic.h Normal file
View File

@ -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;
};

14
libraries/AC_PID/AC_PI_2D.cpp
View File

@ -1,27 +1,27 @@
/// @file AC_PI_2D.cpp
/// @brief Generic PID algorithm
/// @brief 2-axis PI controller
#include <AP_Math/AP_Math.h>
#include "AC_PI_2D.h"
const AP_Param::GroupInfo AC_PI_2D::var_info[] = {
// @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
AP_GROUPINFO("P", 0, AC_PI_2D, _kp, 0),
// @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
AP_GROUPINFO("I", 1, AC_PI_2D, _ki, 0),
// @Param: IMAX
// @DisplayName: PID Integral Maximum
// @DisplayName: PI Integral Maximum
// @Description: The maximum/minimum value that the I term can output
AP_GROUPINFO("IMAX", 2, AC_PI_2D, _imax, 0),
// @Param: FILT_HZ
// @DisplayName: PID Input filter frequency in Hz
// @DisplayName: PI Input filter frequency in Hz
// @Description: Input filter frequency in Hz
// @Units: Hz
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 Vector2f();
return Vector2f{};
}
// 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 Vector2f();
return Vector2f{};
}
Vector2f AC_PI_2D::get_pi()

12
libraries/AC_PID/AC_PI_2D.h
View File

@ -1,7 +1,7 @@
#pragma once
/// @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_Param/AP_Param.h>
@ -12,18 +12,18 @@
#define AC_PI_2D_FILT_HZ_MIN 0.01f // minimum input filter frequency
/// @class AC_PI_2D
/// @brief Copter PID control class
/// @brief 2-axis PI controller
class AC_PI_2D {
public:
// Constructor for PID
// constructor
AC_PI_2D(float initial_p, float initial_i, float initial_imax, float initial_filt_hz, float dt);
// set time step in seconds
void set_dt(float dt);
// set_input - set input to PID controller
// input is filtered before the PID controllers are run
// set_input - set input to PI controller
// 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
void set_input(const Vector2f &input);
void set_input(const Vector3f &input) { set_input(Vector2f(input.x, input.y)); }
@ -78,7 +78,7 @@ private:
AP_Float _kp;
AP_Float _ki;
AP_Float _imax;
AP_Float _filt_hz; // PID Input filter frequency in Hz
AP_Float _filt_hz; // PI Input filter frequency in Hz
// flags
struct ac_pid_flags {

65
libraries/AC_PID/AC_P_1D.cpp Normal file
View File

@ -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);
}

50
libraries/AC_PID/AC_P_1D.h Normal file
View File

@ -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
};

41
libraries/AC_PID/AC_P_2D.cpp Normal file
View File

@ -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);
}

45
libraries/AC_PID/AC_P_2D.h Normal file
View File

@ -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
};