From fa9a616b4ccf4be6dcbb1fc6efea3ce46bcb3b60 Mon Sep 17 00:00:00 2001
From: James Cotton <peabody124@gmail.com>
Date: Mon, 10 Sep 2012 03:10:41 -0500
Subject: [PATCH] PID: Add the 20 Hz low pass filter to the derivative term

---
 flight/Libraries/math/pid.c | 12 +++++++++++-
 flight/Libraries/math/pid.h |  1 +
 2 files changed, 12 insertions(+), 1 deletion(-)

diff --git a/flight/Libraries/math/pid.c b/flight/Libraries/math/pid.c
index 58f15ad1f..8bb0e6013 100644
--- a/flight/Libraries/math/pid.c
+++ b/flight/Libraries/math/pid.c
@@ -37,12 +37,21 @@ static float bound(float val, float range);
 float pid_apply(struct pid *pid, const float err, float dT)
 {
 	float diff = (err - pid->lastErr);
+	float dterm = 0;
 	pid->lastErr = err;
 	
 	// Scale up accumulator by 1000 while computing to avoid losing precision
 	pid->iAccumulator += err * (pid->i * dT * 1000.0f);
 	pid->iAccumulator = bound(pid->iAccumulator, pid->iLim * 1000.0f);
-	return ((err * pid->p) + pid->iAccumulator / 1000.0f + (diff * pid->d / dT));
+
+	// Calculate DT1 term, fixed T1 timeconstant
+	if(pid->d && dT)
+	{
+		dterm = pid->lastDer +  dT / ( dT + 7.9577e-3f) * ((diff * pid->d / dT) - pid->lastDer);
+		pid->lastDer = dterm;            //   ^ set constant to 1/(2*pi*f_cutoff)
+	}	                                 //   7.9577e-3  means 20 Hz f_cutoff
+ 
+	return ((err * pid->p) + pid->iAccumulator / 1000.0f + dterm);
 }
 
 /**
@@ -56,6 +65,7 @@ void pid_zero(struct pid *pid)
 
 	pid->iAccumulator = 0;
 	pid->lastErr = 0;
+	pid->lastDer = 0;
 }
 
 /**
diff --git a/flight/Libraries/math/pid.h b/flight/Libraries/math/pid.h
index 71f9eb56e..e817cab57 100644
--- a/flight/Libraries/math/pid.h
+++ b/flight/Libraries/math/pid.h
@@ -39,6 +39,7 @@ struct pid {
 	float iLim;
 	float iAccumulator;
 	float lastErr;
+	float lastDer;
 };
 
 //! Methods to use the pid structures