From 4ec8805216a65ca4e86ee9b63dc59f51adeb5df6 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sun, 4 Sep 2011 17:47:07 +0000 Subject: [PATCH] Reverted to older PID. Restored the older filter, and removed PI command. I am no longer using PID for the quad and have switched to PIPI loops which perform much better. The D term in this class is not that great. git-svn-id: https://arducopter.googlecode.com/svn/trunk@3241 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/PID/PID.cpp | 52 +++++-------------------------------------- libraries/PID/PID.h | 9 +------- 2 files changed, 7 insertions(+), 54 deletions(-) diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index 69132c8a02..a44d5f3e13 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -18,30 +18,17 @@ 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 = 0; - // 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; + float derivative = (error - _last_error) / delta_time; // 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; @@ -64,39 +51,12 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler) return output; } - -long -PID::get_pi(int32_t error, uint16_t dt, float scaler) -{ - float output = 0; - float delta_time = (float)dt / 1000.0; - - // Compute proportional component - output += error * _kp; - - // scale the P components - output *= scaler; - - // Compute integral component if time has elapsed - if ((fabs(_ki) > 0) && (dt > 0)) { - _integrator += (error * _ki) * scaler * delta_time; - if (_integrator < -_imax) { - _integrator = -_imax; - } else if (_integrator > _imax) { - _integrator = _imax; - } - output += _integrator; - } - - return output; -} - void 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 8def9e8d0e..6cf9919688 100644 --- a/libraries/PID/PID.h +++ b/libraries/PID/PID.h @@ -8,7 +8,6 @@ #include #include // for fabs() -#define PID_FILTER_SIZE 6 /// @class PID /// @brief Object managing one PID control @@ -86,9 +85,6 @@ public: /// long get_pid(int32_t error, uint16_t dt, float scaler = 1.0); - long get_pi(int32_t error, uint16_t dt, float scaler = 1.0); - - /// Reset the PID integrator /// void reset_I(); @@ -121,7 +117,6 @@ 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; } @@ -131,12 +126,10 @@ private: AP_Float16 _ki; AP_Float16 _kd; AP_Int16 _imax; - 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. ///