diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp index f82a873242..8e8499ea14 100644 --- a/libraries/PID/PID.cpp +++ b/libraries/PID/PID.cpp @@ -30,7 +30,7 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler) // discrete low pass filter, cuts out the // high frequency noise that can drive the controller crazy - float RC = 1/(2*M_PI*20); + float RC = 1/(2*M_PI*_fCut); derivative = _last_derivative + (delta_time / (RC + delta_time)) * (derivative - _last_derivative); diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h index 543fff9c4e..09f8b0e8db 100644 --- a/libraries/PID/PID.h +++ b/libraries/PID/PID.h @@ -82,6 +82,13 @@ private: float _integrator; ///< integrator value int32_t _last_error; ///< last error for derivative float _last_derivative; ///< last derivative for low-pass filter + + /// Low pass filter cut frequency for derivative calculation. + /// + /// 20 Hz becasue anything over that is probably noise, see + /// http://en.wikipedia.org/wiki/Low-pass_filter. + /// + static const uint8_t _fCut = 20; }; #endif