Add a modular clock servo interface with a PI controller.
Signed-off-by: Richard Cochran <richardcochran@gmail.com>master
Richard Cochran
parent
edcb731cc2
commit
5c010351c0
|
|
@ -0,0 +1,33 @@
|
||||||
|
/**
|
||||||
|
* @file contain.h
|
||||||
|
* @brief Implements pseudo object oriented features.
|
||||||
|
* @note Copyright (C) 2011 Richard Cochran <richardcochran@gmail.com>
|
||||||
|
*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||||
|
*/
|
||||||
|
#ifndef HAVE_CONTAIN_H
|
||||||
|
#define HAVE_CONTAIN_H
|
||||||
|
|
||||||
|
#include <stddef.h>
|
||||||
|
|
||||||
|
/*
|
||||||
|
* This macro boroughed from the Linux kernel.
|
||||||
|
*/
|
||||||
|
#define container_of(ptr, type, member) ({ \
|
||||||
|
const typeof( ((type *)0)->member ) *__mptr = (ptr); \
|
||||||
|
(type *)( (char *)__mptr - offsetof(type, member) ); \
|
||||||
|
})
|
||||||
|
|
||||||
|
#endif
|
||||||
2
makefile
2
makefile
|
|
@ -24,7 +24,7 @@ CFLAGS = -Wall $(INC) $(DEBUG)
|
||||||
LDFLAGS =
|
LDFLAGS =
|
||||||
LDLIBS = -lm -lrt
|
LDLIBS = -lm -lrt
|
||||||
PRG = linuxptp
|
PRG = linuxptp
|
||||||
OBJ = bmc.o fsm.o msg.o phc.o print.o transport.o udp.o util.o
|
OBJ = bmc.o fsm.o msg.o phc.o pi.o print.o servo.o transport.o udp.o util.o
|
||||||
|
|
||||||
SRC = $(OBJ:.o=.c)
|
SRC = $(OBJ:.o=.c)
|
||||||
DEPEND = $(OBJ:.o=.d)
|
DEPEND = $(OBJ:.o=.d)
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,104 @@
|
||||||
|
/**
|
||||||
|
* @file pi.c
|
||||||
|
* @brief Implements a Proportional Integral clock servo.
|
||||||
|
* @note Copyright (C) 2011 Richard Cochran <richardcochran@gmail.com>
|
||||||
|
*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||||
|
*/
|
||||||
|
#include <stdlib.h>
|
||||||
|
|
||||||
|
#include "pi.h"
|
||||||
|
#include "servo_private.h"
|
||||||
|
|
||||||
|
#define KP 0.7
|
||||||
|
#define KI 0.3
|
||||||
|
|
||||||
|
struct pi_servo {
|
||||||
|
struct servo servo;
|
||||||
|
double offset[2];
|
||||||
|
double local[2];
|
||||||
|
double drift;
|
||||||
|
double maxppb;
|
||||||
|
int count;
|
||||||
|
};
|
||||||
|
|
||||||
|
static void pi_destroy(struct servo *servo)
|
||||||
|
{
|
||||||
|
struct pi_servo *s = container_of(servo, struct pi_servo, servo);
|
||||||
|
free(s);
|
||||||
|
}
|
||||||
|
|
||||||
|
static double pi_sample(struct servo *servo,
|
||||||
|
double offset,
|
||||||
|
double local_ts,
|
||||||
|
enum servo_state *state)
|
||||||
|
{
|
||||||
|
double ki_term, ppb = 0.0;
|
||||||
|
struct pi_servo *s = container_of(servo, struct pi_servo, servo);
|
||||||
|
|
||||||
|
switch (s->count) {
|
||||||
|
case 0:
|
||||||
|
s->offset[0] = offset;
|
||||||
|
s->local[0] = local_ts;
|
||||||
|
*state = SERVO_UNLOCKED;
|
||||||
|
s->count = 1;
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
s->offset[1] = offset;
|
||||||
|
s->local[1] = local_ts;
|
||||||
|
*state = SERVO_UNLOCKED;
|
||||||
|
s->count = 2;
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
s->drift = (s->offset[1] - s->offset[0]) /
|
||||||
|
(s->local[1] - s->local[0]);
|
||||||
|
*state = SERVO_UNLOCKED;
|
||||||
|
s->count = 3;
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
*state = SERVO_JUMP;
|
||||||
|
s->count = 4;
|
||||||
|
break;
|
||||||
|
case 4:
|
||||||
|
ki_term = KI * offset;
|
||||||
|
ppb = KP * offset + s->drift + ki_term;
|
||||||
|
if (ppb < -s->maxppb) {
|
||||||
|
ppb = -s->maxppb;
|
||||||
|
} else if (ppb > s->maxppb) {
|
||||||
|
ppb = s->maxppb;
|
||||||
|
} else {
|
||||||
|
s->drift += ki_term;
|
||||||
|
}
|
||||||
|
*state = SERVO_LOCKED;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
return ppb;
|
||||||
|
}
|
||||||
|
|
||||||
|
struct servo *pi_servo_create(int max_ppb)
|
||||||
|
{
|
||||||
|
struct pi_servo *s;
|
||||||
|
|
||||||
|
s = calloc(1, sizeof(*s));
|
||||||
|
if (!s)
|
||||||
|
return NULL;
|
||||||
|
|
||||||
|
s->servo.destroy = pi_destroy;
|
||||||
|
s->servo.sample = pi_sample;
|
||||||
|
s->maxppb = max_ppb;
|
||||||
|
|
||||||
|
return &s->servo;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,26 @@
|
||||||
|
/**
|
||||||
|
* @file pi.h
|
||||||
|
* @note Copyright (C) 2011 Richard Cochran <richardcochran@gmail.com>
|
||||||
|
*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||||
|
*/
|
||||||
|
#ifndef HAVE_PI_H
|
||||||
|
#define HAVE_PI_H
|
||||||
|
|
||||||
|
#include "servo.h"
|
||||||
|
|
||||||
|
struct servo *pi_servo_create(int max_ppb);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,43 @@
|
||||||
|
/**
|
||||||
|
* @file servo.c
|
||||||
|
* @note Copyright (C) 2011 Richard Cochran <richardcochran@gmail.com>
|
||||||
|
*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||||
|
*/
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
#include "pi.h"
|
||||||
|
#include "servo_private.h"
|
||||||
|
|
||||||
|
struct servo *servo_create(char *name, int max_ppb)
|
||||||
|
{
|
||||||
|
if (!strncmp(name, "pi", 2)) {
|
||||||
|
return pi_servo_create(max_ppb);
|
||||||
|
}
|
||||||
|
return NULL;
|
||||||
|
}
|
||||||
|
|
||||||
|
void servo_destroy(struct servo *servo)
|
||||||
|
{
|
||||||
|
servo->destroy(servo);
|
||||||
|
}
|
||||||
|
|
||||||
|
double servo_sample(struct servo *servo,
|
||||||
|
double offset,
|
||||||
|
double local_ts,
|
||||||
|
enum servo_state *state)
|
||||||
|
{
|
||||||
|
return servo->sample(servo, offset, local_ts, state);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,76 @@
|
||||||
|
/**
|
||||||
|
* @file servo.h
|
||||||
|
* @brief Implements a generic clock servo interface.
|
||||||
|
* @note Copyright (C) 2011 Richard Cochran <richardcochran@gmail.com>
|
||||||
|
*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||||
|
*/
|
||||||
|
#ifndef HAVE_SERVO_H
|
||||||
|
#define HAVE_SERVO_H
|
||||||
|
|
||||||
|
struct servo; /** Opaque type */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Defines the caller visible states of a clock servo.
|
||||||
|
*/
|
||||||
|
enum servo_state {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* The servo is not yet ready to track the master clock.
|
||||||
|
*/
|
||||||
|
SERVO_UNLOCKED,
|
||||||
|
|
||||||
|
/**
|
||||||
|
* The is ready to track and requests a clock jump to
|
||||||
|
* immediately correct the estimated offset.
|
||||||
|
*/
|
||||||
|
SERVO_JUMP,
|
||||||
|
|
||||||
|
/**
|
||||||
|
* The servo is tracking the master clock.
|
||||||
|
*/
|
||||||
|
SERVO_LOCKED,
|
||||||
|
};
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Create a new instance of a clock servo.
|
||||||
|
* @param name The name of the servo flavor to create.
|
||||||
|
* @param max_ppb The absolute maxinum adjustment allowed by the clock
|
||||||
|
* in parts per billion. The clock servo will clamp its
|
||||||
|
* output according to this limit.
|
||||||
|
* @return A pointer to a new servo on success, NULL otherwise.
|
||||||
|
*/
|
||||||
|
struct servo *servo_create(char *name, int max_ppb);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Destroy an instance of a clock servo.
|
||||||
|
* @param servo Pointer to a servo obtained via @ref servo_create().
|
||||||
|
*/
|
||||||
|
void servo_destroy(struct servo *servo);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Feed a sample into a clock servo.
|
||||||
|
* @param servo Pointer to a servo obtained via @ref servo_create().
|
||||||
|
* @param offset The estimated clock offset in nanoseconds.
|
||||||
|
* @param local_ts The local time stamp of the sample in nanoseconds.
|
||||||
|
* @param state Returns the servo's state.
|
||||||
|
* @return The clock adjustment in parts per billion.
|
||||||
|
*/
|
||||||
|
double servo_sample(struct servo *servo,
|
||||||
|
double offset,
|
||||||
|
double local_ts,
|
||||||
|
enum servo_state *state);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,33 @@
|
||||||
|
/**
|
||||||
|
* @file servo_private.h
|
||||||
|
* @note Copyright (C) 2011 Richard Cochran <richardcochran@gmail.com>
|
||||||
|
*
|
||||||
|
* This program is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* This program is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License along
|
||||||
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||||
|
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||||
|
*/
|
||||||
|
#ifndef HAVE_SERVO_PRIVATE_H
|
||||||
|
#define HAVE_SERVO_PRIVATE_H
|
||||||
|
|
||||||
|
#include "contain.h"
|
||||||
|
|
||||||
|
struct servo {
|
||||||
|
|
||||||
|
void (*destroy)(struct servo *servo);
|
||||||
|
|
||||||
|
double (*sample)(struct servo *servo,
|
||||||
|
double offset, double local_ts,
|
||||||
|
enum servo_state *state);
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
Loading…
Reference in New Issue