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

AC_HELI_PID: add input filter and restructure 

Also removed unused initial_ff from construtor
This commit is contained in:
Leonard Hall 2015-01-23 19:05:40 +09:00 committed by Randy Mackay
parent 517448e536
commit 046949ea8a
2 changed files with 28 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AC_PID/AC_HELI_PID.cpp
View File

@ -11,25 +11,41 @@ const AP_Param::GroupInfo AC_HELI_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_HELI_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_HELI_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_HELI_PID, _kd, 0),
// @Param: IMAX
// @DisplayName: PID Integral Maximum
// @Description: The maximum/minimum value that the I term can output
AP_GROUPINFO("IMAX", 3, AC_HELI_PID, _imax, 0),
// @Param: FC
// @DisplayName: PID+FF FeedForward Gain
// @Description: FF Gain which produces an output value that is proportional to the current error value
AP_GROUPINFO("FF", 4, AC_HELI_PID, _ff, 0),
// @Param: IMAX
// @DisplayName: PID Integral Maximum
// @Description: The maximum/minimum value that the I term can output
AP_GROUPINFO("IMAX", 5, AC_HELI_PID, _imax, 0),
// @Param: FILT_HZ
// @DisplayName: PID Input filter frequency in Hz
// @Description:
AP_GROUPINFO("FILT_HZ", 6, AC_HELI_PID, _filt_hz, AC_PID_FILT_HZ_DEFAULT),
AP_GROUPEND
};
/// Constructor for PID
AC_HELI_PID::AC_HELI_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt) :
AC_PID(initial_p, initial_i, initial_d, initial_imax, initial_filt_hz, dt)
{
_ff = 0.0f;
}
float AC_HELI_PID::get_ff(float requested_rate) const
{
return (float)requested_rate * _ff;
@ -38,11 +54,11 @@ float AC_HELI_PID::get_ff(float requested_rate) const
// This is an integrator which tends to decay to zero naturally
// if the error is zero.
float AC_HELI_PID::get_leaky_i(float error, float dt, float leak_rate)
float AC_HELI_PID::get_leaky_i(float leak_rate)
{
if((_ki != 0) && (dt != 0)){
if((_ki != 0) && (_dt != 0)){
_integrator -= (float)_integrator * leak_rate;
_integrator += ((float)error * _ki) * dt;
_integrator += ((float)_input * _ki) * _dt;
if (_integrator < -_imax) {
_integrator = -_imax;
} else if (_integrator > _imax) {

36
libraries/AC_PID/AC_HELI_PID.h
View File

@ -12,45 +12,19 @@
#include <math.h>
#include <AC_PID.h>
// Examples for _filter:
// f_cut = 10 Hz -> _filter = 15.9155e-3
// f_cut = 15 Hz -> _filter = 10.6103e-3
// f_cut = 20 Hz -> _filter = 7.9577e-3
// f_cut = 25 Hz -> _filter = 6.3662e-3
// f_cut = 30 Hz -> _filter = 5.3052e-3
#define AC_HELI_PID_D_TERM_FILTER 0.00795770f // 20hz filter on D term
/// @class AC_HELI_PID
/// @brief Object managing one PID control
/// @brief Heli PID control class
class AC_HELI_PID : public 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_HELI_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,
const float & initial_ff = 0.0) :
AC_PID(initial_p, initial_i, initial_d, initial_imax)
{
_ff = initial_ff;
}
/// Constructor for PID
AC_HELI_PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt);
/// get_ff - return FeedForward Term
float get_ff(float requested_rate) const;
/// get_leaky_i - replacement for get_i but output is leaded at leak_rate
float get_leaky_i(float error, float dt, float leak_rate);
float get_leaky_i(float leak_rate);
// accessors
float ff() const { return _ff.get(); }