From 5e7ca5b0f8cd900734c284099d0120936f11444f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 27 Nov 2012 18:45:49 +1100
Subject: [PATCH] AC_PID: suppress the derivative immediately after reset

use _last_derivative == NAN to flag that the derivative is invalid
---
 libraries/AC_PID/AC_PID.cpp | 16 +++++++++++++---
 libraries/AC_PID/AC_PID.h   |  3 +++
 2 files changed, 16 insertions(+), 3 deletions(-)

diff --git a/libraries/AC_PID/AC_PID.cpp b/libraries/AC_PID/AC_PID.cpp
index 1f1a8cf544..ab5cfd6929 100644
--- a/libraries/AC_PID/AC_PID.cpp
+++ b/libraries/AC_PID/AC_PID.cpp
@@ -55,7 +55,17 @@ int32_t AC_PID::get_leaky_i(int32_t error, float dt, float leak_rate)
 int32_t AC_PID::get_d(int32_t input, float dt)
 {
     if ((_kd != 0) && (dt != 0)) {
-        float derivative = (input - _last_input) / dt;
+        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 {
+			// calculate instantaneous derivative
+			derivative = (input - _last_input) / dt;
+		}
 
         // discrete low pass filter, cuts out the
         // high frequency noise that can drive the controller crazy
@@ -87,8 +97,8 @@ void
 AC_PID::reset_I()
 {
     _integrator = 0;
-    _last_input = 0;
-    _last_derivative = 0;
+	// mark derivative as invalid
+    _last_derivative = NAN;
 }
 
 void
diff --git a/libraries/AC_PID/AC_PID.h b/libraries/AC_PID/AC_PID.h
index 4261256ad3..8d9f3a0170 100644
--- a/libraries/AC_PID/AC_PID.h
+++ b/libraries/AC_PID/AC_PID.h
@@ -34,6 +34,9 @@ public:
         _ki = initial_i;
         _kd = initial_d;
         _imax = abs(initial_imax);
+
+		// derivative is invalid on startup
+		_last_derivative = NAN;
     }
 
     /// Iterate the PID, return the new control value