summaryrefslogtreecommitdiffstats
diff options
context:
space:
mode:
-rw-r--r--ctrl/flight-control.vala5
-rw-r--r--ctrl/pid-controller.vala16
-rw-r--r--timer.vala39
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;
+ }
+}