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

AC_PID: Add method to set the D-term Filter Rate from main code.

This commit is contained in:
Robert Lefebvre 2014-04-17 17:03:53 -04:00 committed by Randy Mackay
parent f1c3f2a3d1
commit b35ec4339e
2 changed files with 20 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AC_PID/AC_PID.cpp
View File

@ -62,8 +62,7 @@ float AC_PID::get_d(float input, float dt)
// discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy
derivative = _last_derivative +
(dt / ( AC_PID_D_TERM_FILTER + dt)) * (derivative - _last_derivative);
derivative = _last_derivative + _d_lpf_alpha * (derivative - _last_derivative);
// update state
_last_input = input;
@ -109,3 +108,10 @@ void AC_PID::save_gains()
_kd.save();
_imax.save();
}
void AC_PID::set_d_lpf_alpha(int16_t cutoff_frequency, float time_step)
{
// calculate alpha
float rc = 1/(2*PI*cutoff_frequency);
_d_lpf_alpha = time_step / (time_step + rc);
}

19
libraries/AC_PID/AC_PID.h
View File

@ -12,12 +12,12 @@
#include <math.h> // for fabs()
// 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_PID_D_TERM_FILTER 0.00795770f // 20hz filter on D term
// 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
/// @class AC_PID
/// @brief Object managing one PID control
@ -38,7 +38,8 @@ public:
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 int16_t & initial_imax = 0.0):
_d_lpf_alpha(AC_PID_D_TERM_FILTER)
{
AP_Param::setup_object_defaults(this, var_info);
@ -81,6 +82,9 @@ public:
/// Save gain properties
///
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
//@{
@ -116,6 +120,7 @@ protected:
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
};
#endif // __AC_PID_H__