Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-01 21:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_PI_2D: 2-axis PI controller

Browse Source
This commit is contained in:
Leonard Hall 2015-01-29 15:49:30 +09:00 committed by Randy Mackay
parent 046949ea8a
commit 34a5bc8b33
2 changed files with 267 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

167
libraries/AC_PID/AC_PI_2D.cpp Normal file
View File

@ -0,0 +1,167 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file AC_PI_2D.cpp
/// @brief Generic PID algorithm
#include <AP_Math.h>
#include "AC_PI_2D.h"
const AP_Param::GroupInfo AC_PI_2D::var_info[] PROGMEM = {
// @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_PI_2D, _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_PI_2D, _ki, 0),
// @Param: IMAX
// @DisplayName: PID 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
// @Description: Input filter frequency in Hz
// @Unit: Hz
AP_GROUPINFO("FILT_HZ", 3, AC_PI_2D, _filt_hz, AC_PI_2D_FILT_HZ_DEFAULT),
AP_GROUPEND
};
// Constructor
AC_PI_2D::AC_PI_2D(float initial_p, float initial_i, float initial_imax, float initial_filt_hz, float dt) :
_dt(dt)
{
// load parameter values from eeprom
AP_Param::setup_object_defaults(this, var_info);
_kp = initial_p;
_ki = initial_i;
_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();
}
// set_dt - set time step in seconds
void AC_PI_2D::set_dt(float dt)
{
// set dt and calculate the input filter alpha
_dt = dt;
calc_filt_alpha();
}
// set_filt_hz - set input filter hz
void AC_PI_2D::set_filt_hz(float hz)
{
_filt_hz.set(hz);
// calculate the input filter alpha
calc_filt_alpha();
}
// 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_PI_2D::set_input(const Vector2f &input)
{
// reset input filter to value received
if (_flags._reset_filter) {
_flags._reset_filter = false;
_input = input;
}
// update filter and calculate derivative
Vector2f input_filt_change = (input - _input) * _filt_alpha;
_input = _input + input_filt_change;
}
Vector2f AC_PI_2D::get_p() const
{
return (_input * _kp);
}
Vector2f AC_PI_2D::get_i()
{
if((_ki != 0.0f) && (_dt != 0.0f)) {
_integrator += (_input * _ki) * _dt;
float integrator_length = _integrator.length();
if ((integrator_length > _imax) && (integrator_length > 0)) {
_integrator *= (_imax / integrator_length);
}
return _integrator;
}
return Vector2f(0,0);
}
// get_i_shrink - get_i but do not allow integrator to grow in length (it may shrink)
Vector2f AC_PI_2D::get_i_shrink()
{
if((_ki != 0.0f) && (_dt != 0.0f)) {
float integrator_length_orig = min(_integrator.length(),_imax);
_integrator += (_input * _ki) * _dt;
float integrator_length_new = _integrator.length();
if ((integrator_length_new > integrator_length_orig) && (integrator_length_new > 0)) {
_integrator *= (integrator_length_orig / integrator_length_new);
}
return _integrator;
}
return Vector2f(0,0);
}
Vector2f AC_PI_2D::get_pi()
{
return get_p() + get_i();
}
void AC_PI_2D::reset_I()
{
_integrator.zero();
}
void AC_PI_2D::load_gains()
{
_kp.load();
_ki.load();
_imax.load();
_imax = fabs(_imax);
_filt_hz.load();
// calculate the input filter alpha
calc_filt_alpha();
}
// save_gains - save gains to eeprom
void AC_PI_2D::save_gains()
{
_kp.save();
_ki.save();
_imax.save();
_filt_hz.save();
}
/// Overload the function call operator to permit easy initialisation
void AC_PI_2D::operator() (float p, float i, float imaxval, float input_filt_hz, float dt)
{
_kp = p;
_ki = i;
_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_PI_2D::calc_filt_alpha()
{
// calculate alpha
float rc = 1/(2*PI*_filt_hz);
_filt_alpha = _dt / (_dt + rc);
}

100
libraries/AC_PID/AC_PI_2D.h Normal file
View File

@ -0,0 +1,100 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/// @file AC_PI_2D.h
/// @brief Generic PID algorithm, with EEPROM-backed storage of constants.
#ifndef __AC_PI_2D_H__
#define __AC_PI_2D_H__
#include <AP_Common.h>
#include <AP_Param.h>
#include <stdlib.h>
#include <math.h>
#define AC_PI_2D_FILT_HZ_DEFAULT 20.0f // default input filter frequency
/// @class AC_PI_2D
/// @brief Copter PID control class
class AC_PI_2D {
public:
// Constructor for PID
AC_PI_2D(float initial_p, float initial_i, float initial_imax, float initial_filt_hz, float dt);
// set_dt - set time step in seconds
void set_dt(float dt);
// set_filt_hz - set input filter hz
void set_filt_hz(float hz);
// 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)); }
// get_pi - get results from pid controller
Vector2f get_pi();
Vector2f get_p() const;
Vector2f get_i();
Vector2f get_i_shrink(); // get_i but do not allow integrator to grow (it may shrink)
// reset_I - reset the integrator
void reset_I();
// 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 to eeprom
void save_gains();
/// operator function call for easy initialisation
void operator() (float p, float i, float imaxval, float input_filt_hz, float dt );
// get accessors
float kP() const { return _kp.get(); }
float kI() const { return _ki.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 imax(const float v) { _imax.set(fabs(v)); }
void filt_hz(const float v) { _filt_hz.set(fabs(v)); }
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; }
// 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 _imax;
AP_Float _filt_hz; // PID Input 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
float _dt; // timestep in seconds
Vector2f _integrator; // integrator value
Vector2f _input; // last input for derivative
float _filt_alpha; // input filter alpha
};
#endif // __AC_PI_2D_H__