make PID timing more precise, setup P, I and DHEADmaster

3 files changed, 53 insertions, 7 deletions

diff --git a/ctrl/flight-control.vala b/ctrl/flight-control.vala
index f0e1345..964c449 100644
--- a/ctrl/flight-control.vala
+++ b/ctrl/flight-control.vala
@@ -27,8 +27,11 @@ public class FlightControl {
 
 	public FlightControl() throws Error {
 		pid = new PID[PIDEntry.length];
-		for(int i=0; i<PIDEntry.length; i++)
+		for(int i=0; i<PIDEntry.length; i++) {
 			pid[i] = new PID();
+			pid[i].calibrate(100.0, 0, -300.0);
+			pid[i].clear();
+		}
 	}
 
 	/**
diff --git a/ctrl/pid-controller.vala b/ctrl/pid-controller.vala
index 5866fe5..ae2add0 100644
--- a/ctrl/pid-controller.vala
+++ b/ctrl/pid-controller.vala
@@ -22,16 +22,20 @@ public class PID {
 	double I;
 	double D;
 
+	Timer  timer;
 	double lastPosition;
-	uint64 previousPIDTime;
 	double integratedError;
 	double windupGuard;
 
+	public void calibrate(double P, double I, double D) {
+		this.P = P;
+		this.I = I;
+		this.D = D;
+	}
+
 	public double update(double targetPosition, double currentPosition) {
-		/* calculate time delta */
-		uint64 currentTime = time_t();
-		uint64 deltaPIDTime = currentTime - previousPIDTime;
-		previousPIDTime = currentTime;
+		/* delta time in seconds */
+		double deltaPIDTime = timer.ellapsed() / 1000000.0;
 
 		/* calculate error */
 		double error = targetPosition - currentPosition;
@@ -46,6 +50,6 @@ public class PID {
 
 	public void clear() {
 		integratedError = 0;
-		previousPIDTime = time_t();
+		timer.start();
 	}
 }
diff --git a/timer.vala b/timer.vala
new file mode 100644
index 0000000..8999bfc
--- /dev/null
+++ b/timer.vala
@@ -0,0 +1,39 @@
+/* Copyright 2012, Sebastian Reichel <sre@ring0.de>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+public struct Timer {
+	private Posix.timespec time_old;
+
+	public void start() {
+		Posix.clock_gettime(Posix.CLOCK_MONOTONIC, out time_old);
+	}
+
+	public ulong ellapsed() {
+		ulong result;
+		Posix.timespec time_new;
+
+		Posix.clock_gettime(Posix.CLOCK_MONOTONIC, out time_new);
+
+		if(time_new.tv_sec < time_old.tv_sec)
+			result = 0;
+		else if(time_new.tv_sec == time_old.tv_sec)
+			result = (time_new.tv_nsec - time_old.tv_nsec) / 1000;
+		else
+			result = (time_new.tv_sec - time_old.tv_sec) * 1000 - (time_old.tv_nsec/1000) + (time_new.tv_nsec/1000);
+
+		time_old = time_new;
+		return result;
+	}
+}