From c967e5e46cd7f14a91f60aaac8ff886b0577d196 Mon Sep 17 00:00:00 2001
From: rusEfi <russianefi@gmail.com>
Date: Tue, 16 Sep 2014 01:02:58 -0500
Subject: [PATCH] auto-sync

---
 firmware/controllers/math/pid.cpp | 14 ++++++++++++--
 firmware/controllers/math/pid.h   |  5 ++++-
 2 files changed, 16 insertions(+), 3 deletions(-)

diff --git a/firmware/controllers/math/pid.cpp b/firmware/controllers/math/pid.cpp
index a0cb789a74..37b7784408 100644
--- a/firmware/controllers/math/pid.cpp
+++ b/firmware/controllers/math/pid.cpp
@@ -13,9 +13,19 @@ Pid::Pid(float pFactor, float iFactor, float dFactor) {
 	this->pFactor = pFactor;
 	this->iFactor = iFactor;
 	this->dFactor = dFactor;
+
+	integration = 0;
+	prevError = 0;
 }
 
-float Pid::getValue(float input) {
-	return 0;
+float Pid::getValue(float target, float input, float dTime) {
+	float error = target - input;
+
+	float pTerm = pFactor * error;
+	integration += iFactor * dTime * error;
+	float dTerm = dFactor / dTime * (error - prevError);
+
+	prevError = error;
+	return pTerm + integration + dTerm;
 }
 
diff --git a/firmware/controllers/math/pid.h b/firmware/controllers/math/pid.h
index 1044aeb931..528643dc60 100644
--- a/firmware/controllers/math/pid.h
+++ b/firmware/controllers/math/pid.h
@@ -12,13 +12,16 @@ class Pid {
 
 public:
 	Pid(float pFactor, float iFactor, float dFactor);
-	float getValue(float input);
+	float getValue(float target, float input, float dTime);
 
 private:
 	float pFactor;
 	float iFactor;
 	float dFactor;
 
+	float integration;
+	float prevError;
+
 };
 
 #endif /* PID_H_ */