Let the clock servo know about the expected time stamp quality.
If software time stamping is to be used, then the servo will want to have appropriate filtering. Signed-off-by: Richard Cochran <richardcochran@gmail.com>master
parent
01691df47b
commit
87e61f9770
11
clock.c
11
clock.c
|
@ -158,7 +158,7 @@ UInteger8 clock_class(struct clock *c)
|
||||||
struct clock *clock_create(char *phc, struct interface *iface, int count,
|
struct clock *clock_create(char *phc, struct interface *iface, int count,
|
||||||
struct defaultDS *ds)
|
struct defaultDS *ds)
|
||||||
{
|
{
|
||||||
int i, max_adj;
|
int i, max_adj, sw_ts = 0;
|
||||||
struct clock *c = &the_clock;
|
struct clock *c = &the_clock;
|
||||||
|
|
||||||
srandom(time(NULL));
|
srandom(time(NULL));
|
||||||
|
@ -182,7 +182,14 @@ struct clock *clock_create(char *phc, struct interface *iface, int count,
|
||||||
max_adj = 512000;
|
max_adj = 512000;
|
||||||
}
|
}
|
||||||
|
|
||||||
c->servo = servo_create("pi", max_adj);
|
for (i = 0; i < count; i++) {
|
||||||
|
if (iface[i].timestamping == TS_SOFTWARE) {
|
||||||
|
sw_ts = 1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
c->servo = servo_create("pi", 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;
|
||||||
|
|
23
pi.c
23
pi.c
|
@ -22,8 +22,11 @@
|
||||||
#include "pi.h"
|
#include "pi.h"
|
||||||
#include "servo_private.h"
|
#include "servo_private.h"
|
||||||
|
|
||||||
#define KP 0.7
|
#define HWTS_KP 0.7
|
||||||
#define KI 0.3
|
#define HWTS_KI 0.3
|
||||||
|
|
||||||
|
#define SWTS_KP 0.1
|
||||||
|
#define SWTS_KI 0.001
|
||||||
|
|
||||||
struct pi_servo {
|
struct pi_servo {
|
||||||
struct servo servo;
|
struct servo servo;
|
||||||
|
@ -31,6 +34,8 @@ struct pi_servo {
|
||||||
double local[2];
|
double local[2];
|
||||||
double drift;
|
double drift;
|
||||||
double maxppb;
|
double maxppb;
|
||||||
|
double kp;
|
||||||
|
double ki;
|
||||||
int count;
|
int count;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -72,8 +77,8 @@ static double pi_sample(struct servo *servo,
|
||||||
s->count = 4;
|
s->count = 4;
|
||||||
break;
|
break;
|
||||||
case 4:
|
case 4:
|
||||||
ki_term = KI * offset;
|
ki_term = s->ki * offset;
|
||||||
ppb = KP * offset + s->drift + ki_term;
|
ppb = s->kp * offset + s->drift + ki_term;
|
||||||
if (ppb < -s->maxppb) {
|
if (ppb < -s->maxppb) {
|
||||||
ppb = -s->maxppb;
|
ppb = -s->maxppb;
|
||||||
} else if (ppb > s->maxppb) {
|
} else if (ppb > s->maxppb) {
|
||||||
|
@ -88,7 +93,7 @@ static double pi_sample(struct servo *servo,
|
||||||
return ppb;
|
return ppb;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct servo *pi_servo_create(int max_ppb)
|
struct servo *pi_servo_create(int max_ppb, int sw_ts)
|
||||||
{
|
{
|
||||||
struct pi_servo *s;
|
struct pi_servo *s;
|
||||||
|
|
||||||
|
@ -100,5 +105,13 @@ struct servo *pi_servo_create(int max_ppb)
|
||||||
s->servo.sample = pi_sample;
|
s->servo.sample = pi_sample;
|
||||||
s->maxppb = max_ppb;
|
s->maxppb = max_ppb;
|
||||||
|
|
||||||
|
if (sw_ts) {
|
||||||
|
s->kp = SWTS_KP;
|
||||||
|
s->ki = SWTS_KI;
|
||||||
|
} else {
|
||||||
|
s->kp = HWTS_KP;
|
||||||
|
s->ki = HWTS_KI;
|
||||||
|
}
|
||||||
|
|
||||||
return &s->servo;
|
return &s->servo;
|
||||||
}
|
}
|
||||||
|
|
2
pi.h
2
pi.h
|
@ -21,6 +21,6 @@
|
||||||
|
|
||||||
#include "servo.h"
|
#include "servo.h"
|
||||||
|
|
||||||
struct servo *pi_servo_create(int max_ppb);
|
struct servo *pi_servo_create(int max_ppb, int sw_ts);
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
4
servo.c
4
servo.c
|
@ -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)
|
struct servo *servo_create(char *name, int max_ppb, int sw_ts)
|
||||||
{
|
{
|
||||||
if (!strncmp(name, "pi", 2)) {
|
if (!strncmp(name, "pi", 2)) {
|
||||||
return pi_servo_create(max_ppb);
|
return pi_servo_create(max_ppb, sw_ts);
|
||||||
}
|
}
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
4
servo.h
4
servo.h
|
@ -51,9 +51,11 @@ enum servo_state {
|
||||||
* @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.
|
||||||
|
* @param sw_ts Indicates that software time stamping will be used,
|
||||||
|
* 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);
|
struct servo *servo_create(char *name, int max_ppb, int sw_ts);
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Destroy an instance of a clock servo.
|
* Destroy an instance of a clock servo.
|
||||||
|
|
Loading…
Reference in New Issue