diff --git a/pi.c b/pi.c index 3c8a635..bd78e40 100644 --- a/pi.c +++ b/pi.c @@ -171,6 +171,13 @@ static void pi_sync_interval(struct servo *servo, double interval) interval, s->kp, s->ki); } +static void pi_reset(struct servo *servo) +{ + struct pi_servo *s = container_of(servo, struct pi_servo, servo); + + s->count = 0; +} + struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts) { struct pi_servo *s; @@ -182,6 +189,7 @@ struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts) s->servo.destroy = pi_destroy; s->servo.sample = pi_sample; s->servo.sync_interval = pi_sync_interval; + s->servo.reset = pi_reset; s->drift = fadj; s->maxppb = max_ppb; s->first_update = 1; diff --git a/servo.c b/servo.c index 45c5832..5af020a 100644 --- a/servo.c +++ b/servo.c @@ -46,3 +46,8 @@ void servo_sync_interval(struct servo *servo, double interval) { servo->sync_interval(servo, interval); } + +void servo_reset(struct servo *servo) +{ + servo->reset(servo); +} diff --git a/servo.h b/servo.h index 052740e..7a5d0d3 100644 --- a/servo.h +++ b/servo.h @@ -93,4 +93,10 @@ double servo_sample(struct servo *servo, */ void servo_sync_interval(struct servo *servo, double interval); +/** + * Reset a clock servo. + * @param servo Pointer to a servo obtained via @ref servo_create(). + */ +void servo_reset(struct servo *servo); + #endif diff --git a/servo_private.h b/servo_private.h index 98c7db2..82e2bf5 100644 --- a/servo_private.h +++ b/servo_private.h @@ -30,6 +30,8 @@ struct servo { enum servo_state *state); void (*sync_interval)(struct servo *servo, double interval); + + void (*reset)(struct servo *servo); }; #endif