From 915a564b8f177bdcf22530000c946bd642935a97 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Fri, 29 Jul 2011 21:52:37 +0000 Subject: [PATCH] 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 --- libraries/PID/PID.cpp | 25 +++++++++++++++++++------ libraries/PID/PID.h | 7 ++++++- 2 files changed, 25 insertions(+), 7 deletions(-) diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index f5bc689026..fb6cb4c536 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -18,17 +18,30 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler) // Compute derivative component if time has elapsed 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 // high frequency noise that can drive the controller crazy - float RC = 1/(2*M_PI*_fCut); - derivative = _last_derivative + - (delta_time / (RC + delta_time)) * (derivative - _last_derivative); + //float RC = 1/(2*M_PI*_fCut); + //derivative = _last_derivative + (delta_time / (RC + delta_time)) * (derivative - _last_derivative); // update state _last_error = error; - _last_derivative = derivative; + //_last_derivative = derivative; // add in derivative component output += _kd * derivative; @@ -83,7 +96,7 @@ PID::reset_I() { _integrator = 0; _last_error = 0; - _last_derivative = 0; + //_last_derivative = 0; } void diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h index 3d2ea1e2b0..3fb32705d9 100644 --- a/libraries/PID/PID.h +++ b/libraries/PID/PID.h @@ -8,6 +8,7 @@ #include #include // for fabs() +#define PID_FILTER_SIZE 4 /// @class PID /// @brief Object managing one PID control @@ -120,6 +121,7 @@ public: 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 filter_size(uint8_t v) { _filter_size = min(v, PID_FILTER_SIZE); } float get_integrator() const { return _integrator; } @@ -129,10 +131,13 @@ private: AP_Float16 _ki; AP_Float16 _kd; AP_Int16 _imax; + //uint8_t _filter_size; + float _filter[PID_FILTER_SIZE]; + uint8_t _filter_index; float _integrator; ///< integrator value 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. ///