From 816fe9d35a114d01a53f81d14184f9bf4c9ddb8c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 27 Nov 2012 18:41:31 +1100
Subject: [PATCH] PID: use NAN to flag a D reset

this saves a byte per PID for a flag, and gives us a safe way to
reset_I() without causing a spike in D
---
 libraries/PID/PID.cpp | 18 +++++++++++++++---
 libraries/PID/PID.h   |  5 ++++-
 2 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp
index 97741f3a98..39d87f6d11 100644
--- a/libraries/PID/PID.cpp
+++ b/libraries/PID/PID.cpp
@@ -30,7 +30,7 @@ PID::get_pid(int32_t error, float scaler)
 		// the intergator term. This prevents I buildup from a
 		// previous fight mode from causing a massive return before
 		// the integrator gets a chance to correct itself
-		_integrator = 0;
+		reset_I();
     }
     _last_t = tnow;
 
@@ -41,7 +41,17 @@ PID::get_pid(int32_t error, float scaler)
 
     // Compute derivative component if time has elapsed
     if ((fabs(_kd) > 0) && (dt > 0)) {
-        float derivative = (error - _last_error) / delta_time;
+        float derivative;
+
+		if (isnan(_last_derivative)) {
+			// we've just done a reset, suppress the first derivative
+			// term as we don't want a sudden change in input to cause
+			// a large D output change			
+			derivative = 0;
+			_last_derivative = 0;
+		} else {
+			derivative = (error - _last_error) / delta_time;
+		}
 
         // discrete low pass filter, cuts out the
         // high frequency noise that can drive the controller crazy
@@ -78,7 +88,9 @@ void
 PID::reset_I()
 {
     _integrator = 0;
-    _last_derivative = 0;
+	// we use NAN (Not A Number) to indicate that the last 
+	// derivative value is not valid
+    _last_derivative = NAN;
 }
 
 void
diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h
index 7a440ea384..71c5d7b1ce 100644
--- a/libraries/PID/PID.h
+++ b/libraries/PID/PID.h
@@ -23,6 +23,9 @@ public:
         _ki = initial_i;
         _kd = initial_d;
         _imax = initial_imax;
+
+		// set _last_derivative as invalid when we startup
+		_last_derivative = NAN;
     }
 
     /// Iterate the PID, return the new control value
@@ -104,7 +107,7 @@ private:
     float           _integrator;                                ///< integrator value
     int32_t         _last_error;                                ///< last error for derivative
     float           _last_derivative;                           ///< last derivative for low-pass filter
-    uint32_t        _last_t;
+    uint32_t        _last_t;                                    ///< last time get_pid() was called in millis
 
     int32_t         _get_pid(int32_t error, uint16_t dt, float scaler);