From cee9397b664b3eaed528283ac4555c600fc2b93e Mon Sep 17 00:00:00 2001 From: Miroslav Lichvar Date: Wed, 8 Jan 2014 14:23:46 +0100 Subject: [PATCH] Print current frequency when PI servo is in unlocked state. Store the current frequency and return it when the servo is in unlocked state instead of zero. This fixes values printed in log messages. Signed-off-by: Miroslav Lichvar --- pi.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/pi.c b/pi.c index 689b232..171ff55 100644 --- a/pi.c +++ b/pi.c @@ -58,6 +58,7 @@ struct pi_servo { double ki; double max_offset; double max_f_offset; + double last_freq; int count; int first_update; }; @@ -73,9 +74,9 @@ static double pi_sample(struct servo *servo, uint64_t local_ts, enum servo_state *state) { - double ki_term, ppb = 0.0; - double freq_est_interval, localdiff; struct pi_servo *s = container_of(servo, struct pi_servo, servo); + double ki_term, ppb = s->last_freq; + double freq_est_interval, localdiff; switch (s->count) { case 0: @@ -153,6 +154,7 @@ static double pi_sample(struct servo *servo, break; } + s->last_freq = ppb; return ppb; } @@ -192,6 +194,7 @@ struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts) s->servo.sync_interval = pi_sync_interval; s->servo.reset = pi_reset; s->drift = fadj; + s->last_freq = fadj; s->maxppb = max_ppb; s->first_update = 1; s->kp = 0.0;