diff options
| -rw-r--r-- | ctrl/flight-control.vala | 5 | ||||
| -rw-r--r-- | ctrl/pid-controller.vala | 16 | ||||
| -rw-r--r-- | timer.vala | 39 |
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; + } +} |