diff options
Diffstat (limited to 'ctrl')
| -rw-r--r-- | ctrl/flight-control.vala | 5 | ||||
| -rw-r--r-- | ctrl/pid-controller.vala | 16 |
2 files changed, 14 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(); } } |