I am Adding A "PI" loop to the mix. This is a need for AC2. We can optimize the internals later, but I basically duped the get_PID and removed the D term internals.

git-svn-id: https://arducopter.googlecode.com/svn/trunk@2338 f9c3cf11-9bcb-44bc-f272-b75c42450872

libraries/PID/PID.cpp

@@ -51,6 +51,33 @@ PID::get_pid(int32_t error, uint16_t dt, float scaler)
 	return output;
 }
 
+
+long
+PID::get_pi(int32_t error, uint16_t dt, float scaler)
+{
+	float output		= 0;
+ 	float delta_time	= (float)dt / 1000.0;
+
+	// Compute proportional component
+	output += error * _kp;
+
+	// scale the P components
+	output *= scaler;
+
+	// Compute integral component if time has elapsed
+	if ((fabs(_ki) > 0) && (dt > 0)) {
+		_integrator 		+= (error * _ki) * scaler * delta_time;
+		if (_integrator < -_imax) {
+			_integrator = -_imax;
+		} else if (_integrator > _imax) {
+			_integrator = _imax;
+		}
+		output 				+= _integrator;
+	}
+
+	return output;
+}
+
 void
 PID::reset_I()
 {

libraries/PID/PID.h

@@ -85,6 +85,9 @@ public:
 	///
 	long 	get_pid(int32_t error, uint16_t dt, float scaler = 1.0);
 
+	long 	get_pi(int32_t error, uint16_t dt, float scaler = 1.0);
+
+
 	/// Reset the PID integrator
 	///
 	void	reset_I();