Fixed PID error for low pass filter.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1295 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/PID/PID.cpp

@@ -29,8 +29,9 @@ 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*_fCut);
 		derivative = _last_derivative + 
-			((delta_time / (_RC + delta_time)) * (derivative - _last_derivative));
+			(delta_time / (RC + delta_time)) * (derivative - _last_derivative);
 
 		// update state
 		_last_error 		= error;

libraries/PID/PID.h

@@ -133,7 +133,7 @@ private:
 	/// 20 Hz becasue anything over that is probably noise, see
 	/// http://en.wikipedia.org/wiki/Low-pass_filter.
 	///
-	static const uint8_t _RC = 20; 
+	static const uint8_t _fCut = 20; 
 };
 
 #endif