From 00e7f9b6d04beb17c801623fac53976449c19959 Mon Sep 17 00:00:00 2001
From: "james.goppert" <james.goppert@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Thu, 25 Nov 2010 23:14:37 +0000
Subject: [PATCH] Added lowpass filter on PID lib derivative calc

git-svn-id: https://arducopter.googlecode.com/svn/trunk@927 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 libraries/PID/PID.cpp | 19 +++++++++++++++----
 libraries/PID/PID.h   |  8 +++++++-
 2 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp
index ed9f5a53d6..948d978bd8 100644
--- a/libraries/PID/PID.cpp
+++ b/libraries/PID/PID.cpp
@@ -20,9 +20,19 @@ PID::get_pid(long err, long dt, float scaler)
 
 	if(_kd != 0){
 		// Compute derivative component
-		//derivative = (error - previous_error)/dt
-		float derivative 	= 1000 * (error - _last_error) / dt;
+		//derivative = (error - previous_error)/delta_time
+		float derivative 	= (error - _last_error) / delta_time;
+
+		// discrete low pass filter, cuts out the 
+		// high frequency noise that can drive the controller crazy
+		derivative = _last_derivative + delta_time/
+			(RC + delta_time)*(derivative - _last_derivative);
+		
+		//Serial.print("d: ");
+		//Serial.println(derivative,DEC);
+
 		_last_error 		= error;
+		_last_derivative    = derivative;
 		output 				+= _kd * derivative;			// Sum the derivative component
 	}
 	
@@ -30,7 +40,7 @@ PID::get_pid(long err, long dt, float scaler)
 	
 	// Compute integral component
 	if(_ki != 0){
-		_integrator 		+= (error * _ki) * scaler * dt *.001; 
+		_integrator 		+= (error * _ki) * scaler * delta_time; 
 		_integrator 		= constrain(_integrator, -_imax, _imax);
 		output 				+= _integrator;
 	}
@@ -44,6 +54,7 @@ PID::reset_I(void)
 {
 	_integrator = 0;
 	_last_error = 0;
+	_last_error_derivative = 0;
 }
 
 
@@ -179,4 +190,4 @@ PID::test(void)
 	address += 8;
 	Serial.println(eeprom_read_word((uint16_t *) address));
 	*/
-}
\ No newline at end of file
+}
diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h
index 4e304283fb..f45be8cb74 100644
--- a/libraries/PID/PID.h
+++ b/libraries/PID/PID.h
@@ -32,6 +32,12 @@ public:
 	
 
 private:
+	static uint8_t RC = 20; // low pass filter cut frequency
+	// for derivative calculation,
+	// set to 20 Hz becasue anything over that
+	// is probably noise, see
+	// http://en.wikipedia.org/wiki/Low-pass_filter
+
 };
 
-#endif
\ No newline at end of file
+#endif