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 =
|
||||
LDLIBS = -lm -lrt
|
||||
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)
|
||||
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