Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PID: add input filtering and restructure

Browse Source
This commit is contained in:
Leonard Hall 2015-02-11 21:06:04 +09:00 committed by Randy Mackay
parent 644d0c223a
commit 517448e536
2 changed files with 191 additions and 124 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

172
libraries/AC_PID/AC_PID.cpp
View File

@ -11,30 +11,122 @@ const AP_Param::GroupInfo AC_PID::var_info[] PROGMEM = {
// @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, _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, _ki, 0),
// @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", 2, AC_PID, _kd, 0),
// 3 was for uint16 IMAX
// 4 is used by TradHeli for FF
// @Param: IMAX
// @DisplayName: PID Integral Maximum
// @Description: The maximum/minimum value that the I term can output
AP_GROUPINFO("IMAX", 3, AC_PID, _imax, 0),
AP_GROUPINFO("IMAX", 5, AC_PID, _imax, 0),
// @Param: FILT_HZ
// @DisplayName: PID Input filter frequency in Hz
// @Description: Input filter frequency in Hz
// @Unit: Hz
AP_GROUPINFO("FILT_HZ", 6, AC_PID, _filt_hz, AC_PID_FILT_HZ_DEFAULT),
AP_GROUPEND
};
float AC_PID::get_p(float error) const
// Constructor
AC_PID::AC_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt) :
_dt(dt),
_integrator(0.0f),
_input(0.0f),
_derivative(0.0f)
{
return (float)error * _kp;
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
_kp = initial_p;
_ki = initial_i;
_kd = initial_d;
_imax = fabs(initial_imax);
_filt_hz = initial_filt_hz;
// reset input filter to first value received
_flags._reset_filter = true;
// calculate the input filter alpha
calc_filt_alpha();
}
float AC_PID::get_i(float error, float dt)
// set_dt - set time step in seconds
void AC_PID::set_dt(float dt)
{
if((_ki != 0) && (dt != 0)) {
_integrator += ((float)error * _ki) * dt;
// set dt and calculate the input filter alpha
_dt = dt;
calc_filt_alpha();
}
// set_filt_hz - set input filter hz
void AC_PID::set_filt_hz(float hz)
{
_filt_hz.set(hz);
// calculate the input filter alpha
calc_filt_alpha();
}
// set_input_filter_all - 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::set_input_filter_all(float input)
{
// reset input filter to value received
if (_flags._reset_filter) {
_flags._reset_filter = false;
_input = input;
_derivative = 0.0f;
}
// update filter and calculate derivative
float input_filt_change = _filt_alpha * (input - _input);
_input = _input + input_filt_change;
if (_dt > 0.0f) {
_derivative = input_filt_change / _dt;
}
}
// 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::set_input_filter_d(float input)
{
// reset input filter to value received
if (_flags._reset_filter) {
_flags._reset_filter = false;
_derivative = 0.0f;
}
// update filter and calculate derivative
if (_dt > 0.0f) {
float derivative = (input - _input) / _dt;
_derivative = _derivative + _filt_alpha * (derivative-_derivative);
}
_input = input;
}
float AC_PID::get_p() const
{
return (_input * _kp);
}
float AC_PID::get_i()
{
if((_ki != 0) && (_dt != 0)) {
_integrator += ((float)_input * _ki) * _dt;
if (_integrator < -_imax) {
_integrator = -_imax;
} else if (_integrator > _imax) {
@ -45,51 +137,27 @@ float AC_PID::get_i(float error, float dt)
return 0;
}
float AC_PID::get_d(float input, float dt)
float AC_PID::get_d() const
{
if ((_kd != 0) && (dt != 0)) {
float derivative;
if (isnan(_last_derivative)) {
// we've just done a reset, suppress the first derivative
// term as we don't want a sudden change in input to cause
// a large D output change
derivative = 0;
_last_derivative = 0;
} else {
// calculate instantaneous derivative
derivative = (input - _last_input) / dt;
}
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
derivative = _last_derivative + _d_lpf_alpha * (derivative - _last_derivative);
// update state
_last_input = input;
_last_derivative = derivative;
// add in derivative component
return _kd * derivative;
}
return 0;
// add in derivative component
return (_kd * _derivative);
}
float AC_PID::get_pi(float error, float dt)
float AC_PID::get_pi()
{
return get_p(error) + get_i(error, dt);
return get_p() + get_i();
}
float AC_PID::get_pid(float error, float dt)
float AC_PID::get_pid()
{
return get_p(error) + get_i(error, dt) + get_d(error, dt);
return get_p() + get_i() + get_d();
}
void AC_PID::reset_I()
{
_integrator = 0;
// mark derivative as invalid
_last_derivative = NAN;
}
void AC_PID::load_gains()
@ -98,20 +166,40 @@ void AC_PID::load_gains()
_ki.load();
_kd.load();
_imax.load();
_imax = abs(_imax);
_imax = fabs(_imax);
_filt_hz.load();
// calculate the input filter alpha
calc_filt_alpha();
}
// save_gains - save gains to eeprom
void AC_PID::save_gains()
{
_kp.save();
_ki.save();
_kd.save();
_imax.save();
_filt_hz.save();
}
void AC_PID::set_d_lpf_alpha(int16_t cutoff_frequency, float time_step)
/// Overload the function call operator to permit easy initialisation
void AC_PID::operator() (float p, float i, float d, float imaxval, float input_filt_hz, float dt)
{
_kp = p;
_ki = i;
_kd = d;
_imax = fabs(imaxval);
_filt_hz = input_filt_hz;
_dt = dt;
// calculate the input filter alpha
calc_filt_alpha();
}
// calc_filt_alpha - recalculate the input filter alpha
void AC_PID::calc_filt_alpha()
{
// calculate alpha
float rc = 1/(2*PI*cutoff_frequency);
_d_lpf_alpha = time_step / (time_step + rc);
float rc = 1/(2*PI*_filt_hz);
_filt_alpha = _dt / (_dt + rc);
}

143
libraries/AC_PID/AC_PID.h
View File

@ -9,121 +9,100 @@
#include <AP_Common.h>
#include <AP_Param.h>
#include <stdlib.h>
#include <math.h> // for fabs()
#include <math.h>
// Examples for _filter:
// f_cut = 10 Hz -> _alpha = 0.385869
// f_cut = 15 Hz -> _alpha = 0.485194
// f_cut = 20 Hz -> _alpha = 0.556864
// f_cut = 25 Hz -> _alpha = 0.611015
// f_cut = 30 Hz -> _alpha = 0.653373
#define AC_PID_D_TERM_FILTER 0.556864f // Default 100Hz Filter Rate with 20Hz Cutoff Frequency
#define AC_PID_FILT_HZ_DEFAULT 20.0f // default input filter frequency
/// @class AC_PID
/// @brief Object managing one PID control
/// @brief Copter PID control class
class AC_PID {
public:
/// Constructor for PID that saves its settings to EEPROM
///
/// @note PIDs must be named to avoid either multiple parameters with the
/// same name, or an overly complex constructor.
///
/// @param initial_p Initial value for the P term.
/// @param initial_i Initial value for the I term.
/// @param initial_d Initial value for the D term.
/// @param initial_imax Initial value for the imax term.4
///
AC_PID(
const float & initial_p = 0.0,
const float & initial_i = 0.0,
const float & initial_d = 0.0,
const int16_t & initial_imax = 0.0):
_integrator(0),
_last_input(0),
_last_derivative(0),
_d_lpf_alpha(AC_PID_D_TERM_FILTER)
{
AP_Param::setup_object_defaults(this, var_info);
// Constructor for PID
AC_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt);
_kp = initial_p;
_ki = initial_i;
_kd = initial_d;
_imax = abs(initial_imax);
// set_dt - set time step in seconds
void set_dt(float dt);
// derivative is invalid on startup
_last_derivative = NAN;
}
// set_filt_hz - set input filter hz
void set_filt_hz(float hz);
/// Iterate the PID, return the new control value
///
/// Positive error produces positive output.
///
/// @param error The measured error value
/// @param dt The time delta in milliseconds (note
/// that update interval cannot be more
/// than 65.535 seconds due to limited range
/// of the data type).
/// @param scaler An arbitrary scale factor
///
/// @returns The updated control output.
///
float get_pid(float error, float dt);
float get_pi(float error, float dt);
float get_p(float error) const;
float get_i(float error, float dt);
float get_d(float error, float dt);
// set_input_filter_all - 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_filter_all(float input);
/// Reset the PID integrator
///
// 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(float input);
// get_pid - get results from pid controller
float get_pid();
float get_pi();
float get_p() const;
float get_i();
float get_d() const;
// reset_I - reset the integrator
void reset_I();
/// Load gain properties
///
// reset_filter - input filter will be reset to the next value provided to set_input()
void reset_filter();
// load gain from eeprom
void load_gains();
/// Save gain properties
///
// save gain to eeprom
void save_gains();
/// Sets filter Alpha for D-term LPF
void set_d_lpf_alpha(int16_t cutoff_frequency, float time_step);
/// @name parameter accessors
//@{
/// operator function call for easy initialisation
void operator() (float p, float i, float d, float imaxval, float input_filt_hz, float dt );
/// Overload the function call operator to permit relatively easy initialisation
void operator () (const float p,
const float i,
const float d,
const int16_t imaxval) {
_kp = p; _ki = i; _kd = d; _imax = abs(imaxval);
}
// accessors
// get accessors
float kP() const { return _kp.get(); }
float kI() const { return _ki.get(); }
float kD() const { return _kd.get(); }
int16_t imax() const { return _imax.get(); }
float imax() const { return _imax.get(); }
float filt_hz() const { return _filt_hz.get(); }
float get_filt_alpha() const { return _filt_alpha; }
// 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 int16_t v) { _imax.set(abs(v)); }
void imax(const float v) { _imax.set(fabs(v)); }
void filt_hz(const float v) { _filt_hz.set(fabs(v)); }
float get_integrator() const { return _integrator; }
void set_integrator(float i) { _integrator = i; }
// parameter var table
static const struct AP_Param::GroupInfo var_info[];
protected:
// calc_filt_alpha - recalculate the input filter alpha
void calc_filt_alpha();
// parameters
AP_Float _kp;
AP_Float _ki;
AP_Float _kd;
AP_Int16 _imax;
AP_Float _imax;
AP_Float _filt_hz; // PID Input filter frequency in Hz
float _integrator; ///< integrator value
float _last_input; ///< last input for derivative
float _last_derivative; ///< last derivative for low-pass filter
float _d_lpf_alpha; ///< alpha used in D-term LPF
// 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
float _dt; // timestep in seconds
float _integrator; // integrator value
float _input; // last input for derivative
float _derivative; // last derivative for low-pass filter
float _filt_alpha; // input filter alpha
};
#endif // __AC_PID_H__