From f5c24c6e71bc918ac1b92df88f4ba8f3cdecd57a Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sun, 15 Dec 2013 12:34:56 +0400
Subject: pid library fix

---
 src/modules/systemlib/pid/pid.c | 23 ++++++++++++++---------
 1 file changed, 14 insertions(+), 9 deletions(-)

(limited to 'src/modules/systemlib/pid')

diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 0bec72495..6a4e9392a 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -143,17 +143,22 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	/* calculate PD output */
 	float output = (error * pid->kp) + (d * pid->kd);
 
-	/* check for saturation */
-	if (isfinite(i)) {
-		if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
-		    fabsf(i) <= pid->integral_limit) {
-			/* not saturated, use new integral value */
-			pid->integral = i;
+	if (pid->ki > SIGMA) {
+		// Calculate the error integral and check for saturation
+		i = pid->integral + (error * dt);
+
+		/* check for saturation */
+		if (isfinite(i)) {
+			if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
+			    fabsf(i) <= pid->integral_limit) {
+				/* not saturated, use new integral value */
+				pid->integral = i;
+			}
 		}
-	}
 
-	/* add I component to output */
-	output += pid->integral * pid-> ki;
+		/* add I component to output */
+		output += pid->integral * pid->ki;
+	}
 
 	/* limit output */
 	if (isfinite(output)) {
-- 
cgit v1.2.3