From 52abef58d5c538ad4e2e7917e64de579723ea173 Mon Sep 17 00:00:00 2001
From: "DrZiplok@gmail.com"
 <DrZiplok@gmail.com@f9c3cf11-9bcb-44bc-f272-b75c42450872>
Date: Fri, 26 Nov 2010 01:53:12 +0000
Subject: [PATCH] More syntax, etc. fixes.  Compiles OK now.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@932 f9c3cf11-9bcb-44bc-f272-b75c42450872
---
 libraries/PID/PID.cpp              | 13 ++++++++-----
 libraries/PID/PID.h                | 17 +++++++++++------
 libraries/PID/examples/pid/pid.pde |  8 ++++----
 3 files changed, 23 insertions(+), 15 deletions(-)

diff --git a/libraries/PID/PID.cpp b/libraries/PID/PID.cpp
index 330e950049..152ee174a8 100644
--- a/libraries/PID/PID.cpp
+++ b/libraries/PID/PID.cpp
@@ -3,6 +3,7 @@
 /// @file	PID.cpp
 /// @brief	Generic PID algorithm, with EEPROM-backed storage of constants.
 
+#include <math.h>
 #include "PID.h"
 
 long
@@ -37,7 +38,11 @@ PID::get_pid(long error, long dt, float scaler)
 	// Compute integral component if time has elapsed
 	if (_ki && (dt > 0)) {
 		_integrator 		+= (error * _ki) * scaler * delta_time; 
-		_integrator 		= constrain(_integrator, -_imax, _imax);
+		if (_integrator < -_imax) {
+			_integrator = -_imax;
+		} else if (_integrator > _imax) {
+			_integrator = _imax;
+		}
 		output 				+= _integrator;
 	}
 	
@@ -45,11 +50,9 @@ PID::get_pid(long error, long dt, float scaler)
 }
 
 void
-PID::reset_I(void)
+PID::imax(float v)
 {
-	_integrator = 0;
-	_last_error = 0;
-	_last_error_derivative = 0;
+	_imax = fabs(v); 
 }
 
 void
diff --git a/libraries/PID/PID.h b/libraries/PID/PID.h
index 82298c9d03..99c25f75ca 100644
--- a/libraries/PID/PID.h
+++ b/libraries/PID/PID.h
@@ -24,7 +24,7 @@ public:
 		_address(address),
 		_integrator(0),
 		_last_error(0),
-		_last_error_derivative(0)
+		_last_derivative(0)
 	{}
 
 	/// Constructor
@@ -38,7 +38,7 @@ public:
 		_address(0),
 		_integrator(0),
 		_last_error(0),
-		_last_error_derivative(0)
+		_last_derivative(0)
 	{}
 
 	/// Iterate the PID, return the new control value
@@ -55,7 +55,11 @@ public:
 
 	/// Reset the PID integrator
 	///
-	void	reset_I() { _integrator = 0; _last_error = 0; }
+	void	reset_I() {	
+		_integrator = 0; 
+		_last_error = 0; 
+		_last_derivative = 0;
+	}
 
 	/// Load gain properties
 	///
@@ -75,11 +79,11 @@ public:
 	void	kP(float v)		{ _kp = v; }
 	void	kI(float v)		{ _ki = v; }
 	void	kD(float v)		{ _kd = v; }
-	void	imax(float v)	{ _imax = v; }
+	void	imax(float v);
 	//@}
 
 private:
-	uint16_t	_address		///< EEPROM address for save/restore of P/I/D
+	uint16_t	_address;		///< EEPROM address for save/restore of P/I/D
 	float		*_gain_array; 	///< pointer to the gains for this pid
 
 	float		_kp;			///< proportional gain
@@ -89,13 +93,14 @@ private:
 
 	float		_integrator;	///< integrator value
 	int32_t		_last_error;	///< last error for derivative
+	float		_last_derivative; ///< last derivative for low-pass filter
 
 	/// 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
-	static uint8_t _RC = 20; 
+	static const uint8_t _RC = 20; 
 };
 
 #endif
diff --git a/libraries/PID/examples/pid/pid.pde b/libraries/PID/examples/pid/pid.pde
index 249c05d653..a86d8fcad1 100644
--- a/libraries/PID/examples/pid/pid.pde
+++ b/libraries/PID/examples/pid/pid.pde
@@ -20,10 +20,10 @@ void setup()
 	//rc.trim();
 	radio_trim = APM_RC.InputCh(0);
 
-	pid.kP() = 1;
-	pid.kI() = 0;
-	pid.kD() = 0.5;
-	pid.imax() = 50;
+	pid.kP(1);
+	pid.kI(0);
+	pid.kD(0.5);
+	pid.imax(50);
 }
 
 void loop()