From ae917f9e718f1fd5e76e5ca94a97aba5d4af95b8 Mon Sep 17 00:00:00 2001
From: Jason Short <jasonshort@mac.com>
Date: Tue, 6 Dec 2011 21:08:47 -0800
Subject: [PATCH] Split the P and I terms in PI

---
 libraries/APM_PI/APM_PI.cpp | 17 +++++++++++++----
 1 file changed, 13 insertions(+), 4 deletions(-)

diff --git a/libraries/APM_PI/APM_PI.cpp b/libraries/APM_PI/APM_PI.cpp
index d6efc6fb29..14328022fd 100644
--- a/libraries/APM_PI/APM_PI.cpp
+++ b/libraries/APM_PI/APM_PI.cpp
@@ -7,10 +7,14 @@
 
 #include "APM_PI.h"
 
-long
-APM_PI::get_pi(int32_t error, float dt, bool calc_i)
+int32_t APM_PI::get_p(int32_t error)
 {
-	if(calc_i){
+	return (float)error * _kp;
+}
+
+int32_t APM_PI::get_i(int32_t error, float dt)
+{
+	if(dt != 0){
 		_integrator += ((float)error * _ki) * dt;
 
 		if (_integrator < -_imax) {
@@ -19,7 +23,12 @@ APM_PI::get_pi(int32_t error, float dt, bool calc_i)
 			_integrator = _imax;
 		}
 	}
-	return (float)error * _kp + _integrator;
+	return _integrator;
+}
+
+int32_t APM_PI::get_pi(int32_t error, float dt)
+{
+	return get_p(error) + get_i(error, dt);
 }
 
 void