Discover and utilize the initial clock frequency adjustment.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>
master
Richard Cochran 2012-09-21 09:27:53 +02:00
parent 671358b21d
commit 8f00d29265
5 changed files with 25 additions and 8 deletions

19
clock.c
View File

@ -270,6 +270,18 @@ static void clock_ppb(clockid_t clkid, double ppb)
pr_err("failed to adjust the clock: %m"); pr_err("failed to adjust the clock: %m");
} }
static double clock_ppb_read(clockid_t clkid)
{
double f = 0.0;
struct timex tx;
memset(&tx, 0, sizeof(tx));
if (clock_adjtime(clkid, &tx) < 0)
pr_err("failed to read out the clock frequency adjustment: %m");
else
f = tx.freq / 65.536;
return f;
}
static void clock_step(clockid_t clkid, int64_t ns) static void clock_step(clockid_t clkid, int64_t ns)
{ {
struct timex tx; struct timex tx;
@ -388,7 +400,7 @@ UInteger8 clock_class(struct clock *c)
struct clock *clock_create(int phc_index, struct interface *iface, int count, struct clock *clock_create(int phc_index, struct interface *iface, int count,
enum timestamp_type timestamping, struct defaultDS *ds) enum timestamp_type timestamping, struct defaultDS *ds)
{ {
int i, max_adj, sw_ts = timestamping == TS_SOFTWARE ? 1 : 0; int i, fadj = 0, max_adj, sw_ts = timestamping == TS_SOFTWARE ? 1 : 0;
struct clock *c = &the_clock; struct clock *c = &the_clock;
char phc[32]; char phc[32];
struct interface udsif; struct interface udsif;
@ -422,7 +434,10 @@ struct clock *clock_create(int phc_index, struct interface *iface, int count,
max_adj = 512000; max_adj = 512000;
} }
c->servo = servo_create("pi", max_adj, sw_ts); if (c->clkid != CLOCK_INVALID) {
fadj = (int) clock_ppb_read(c->clkid);
}
c->servo = servo_create("pi", -fadj, max_adj, sw_ts);
if (!c->servo) { if (!c->servo) {
pr_err("Failed to create clock servo"); pr_err("Failed to create clock servo");
return NULL; return NULL;

5
pi.c
View File

@ -71,7 +71,7 @@ static double pi_sample(struct servo *servo,
s->count = 2; s->count = 2;
break; break;
case 2: case 2:
s->drift = (s->offset[1] - s->offset[0]) / s->drift += (s->offset[1] - s->offset[0]) /
(s->local[1] - s->local[0]); (s->local[1] - s->local[0]);
*state = SERVO_UNLOCKED; *state = SERVO_UNLOCKED;
s->count = 3; s->count = 3;
@ -97,7 +97,7 @@ static double pi_sample(struct servo *servo,
return ppb; return ppb;
} }
struct servo *pi_servo_create(int max_ppb, int sw_ts) struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts)
{ {
struct pi_servo *s; struct pi_servo *s;
@ -107,6 +107,7 @@ struct servo *pi_servo_create(int max_ppb, int sw_ts)
s->servo.destroy = pi_destroy; s->servo.destroy = pi_destroy;
s->servo.sample = pi_sample; s->servo.sample = pi_sample;
s->drift = fadj;
s->maxppb = max_ppb; s->maxppb = max_ppb;
if (configured_pi_kp && configured_pi_ki) { if (configured_pi_kp && configured_pi_ki) {

2
pi.h
View File

@ -33,6 +33,6 @@ extern double configured_pi_kp;
*/ */
extern double configured_pi_ki; extern double configured_pi_ki;
struct servo *pi_servo_create(int max_ppb, int sw_ts); struct servo *pi_servo_create(int fadj, int max_ppb, int sw_ts);
#endif #endif

View File

@ -21,10 +21,10 @@
#include "pi.h" #include "pi.h"
#include "servo_private.h" #include "servo_private.h"
struct servo *servo_create(char *name, int max_ppb, int sw_ts) struct servo *servo_create(char *name, int fadj, int max_ppb, int sw_ts)
{ {
if (!strncmp(name, "pi", 2)) { if (!strncmp(name, "pi", 2)) {
return pi_servo_create(max_ppb, sw_ts); return pi_servo_create(fadj, max_ppb, sw_ts);
} }
return NULL; return NULL;
} }

View File

@ -48,6 +48,7 @@ enum servo_state {
/** /**
* Create a new instance of a clock servo. * Create a new instance of a clock servo.
* @param name The name of the servo flavor to create. * @param name The name of the servo flavor to create.
* @param fadj The clock's current adjustment in parts per billion.
* @param max_ppb The absolute maxinum adjustment allowed by the clock * @param max_ppb The absolute maxinum adjustment allowed by the clock
* in parts per billion. The clock servo will clamp its * in parts per billion. The clock servo will clamp its
* output according to this limit. * output according to this limit.
@ -55,7 +56,7 @@ enum servo_state {
* and the servo should use more aggressive filtering. * and the servo should use more aggressive filtering.
* @return A pointer to a new servo on success, NULL otherwise. * @return A pointer to a new servo on success, NULL otherwise.
*/ */
struct servo *servo_create(char *name, int max_ppb, int sw_ts); struct servo *servo_create(char *name, int fadj, int max_ppb, int sw_ts);
/** /**
* Destroy an instance of a clock servo. * Destroy an instance of a clock servo.