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

Changed the PID derivative filter to a moving average with 4 samples. Having great luck with it and High kD when using noisy sensors. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2957 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-07-29 21:52:37 +00:00
parent 1dc0063e67
commit 7026405951
2 changed files with 25 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/PID/PID.cpp
View File

@ -18,17 +18,30 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler)
// Compute derivative component if time has elapsed // Compute derivative component if time has elapsed
if ((fabs(_kd) > 0) && (dt > 0)) { if ((fabs(_kd) > 0) && (dt > 0)) {
float derivative = (error - _last_error) / delta_time; float derivative;
// add in
_filter[_filter_index] = (error - _last_error) / delta_time;
_filter_index++;
if(_filter_index >= PID_FILTER_SIZE)
_filter_index = 0;
// sum our filter
for(uint8_t i = 0; i < PID_FILTER_SIZE; i++){
derivative += _filter[i];
}
// grab result
derivative /= PID_FILTER_SIZE;
// discrete low pass filter, cuts out the // discrete low pass filter, cuts out the
// high frequency noise that can drive the controller crazy // high frequency noise that can drive the controller crazy
float RC = 1/(2*M_PI*_fCut); //float RC = 1/(2*M_PI*_fCut);
derivative = _last_derivative + //derivative = _last_derivative + (delta_time / (RC + delta_time)) * (derivative - _last_derivative);
(delta_time / (RC + delta_time)) * (derivative - _last_derivative);
// update state // update state
_last_error = error; _last_error = error;
_last_derivative = derivative; //_last_derivative = derivative;
// add in derivative component // add in derivative component
output += _kd * derivative; output += _kd * derivative;
@ -83,7 +96,7 @@ PID::reset_I()
{ {
_integrator = 0; _integrator = 0;
_last_error = 0; _last_error = 0;
_last_derivative = 0; //_last_derivative = 0;
} }
void void

7
libraries/PID/PID.h
View File

@ -8,6 +8,7 @@
#include <AP_Common.h> #include <AP_Common.h>
#include <math.h> // for fabs() #include <math.h> // for fabs()
#define PID_FILTER_SIZE 4
/// @class PID /// @class PID
/// @brief Object managing one PID control /// @brief Object managing one PID control
@ -120,6 +121,7 @@ public:
void kI(const float v) { _ki.set(v); } void kI(const float v) { _ki.set(v); }
void kD(const float v) { _kd.set(v); } void kD(const float v) { _kd.set(v); }
void imax(const int16_t v) { _imax.set(abs(v)); } void imax(const int16_t v) { _imax.set(abs(v)); }
//void filter_size(uint8_t v) { _filter_size = min(v, PID_FILTER_SIZE); }
float get_integrator() const { return _integrator; } float get_integrator() const { return _integrator; }
@ -129,10 +131,13 @@ private:
AP_Float16 _ki; AP_Float16 _ki;
AP_Float16 _kd; AP_Float16 _kd;
AP_Int16 _imax; AP_Int16 _imax;
//uint8_t _filter_size;
float _filter[PID_FILTER_SIZE];
uint8_t _filter_index;
float _integrator; ///< integrator value float _integrator; ///< integrator value
int32_t _last_error; ///< last error for derivative int32_t _last_error; ///< last error for derivative
float _last_derivative; ///< last derivative for low-pass filter //float _last_derivative; ///< last derivative for low-pass filter
/// Low pass filter cut frequency for derivative calculation. /// Low pass filter cut frequency for derivative calculation.
/// ///