diff --git a/contain.h b/contain.h new file mode 100644 index 0000000..4c2822d --- /dev/null +++ b/contain.h @@ -0,0 +1,33 @@ +/** + * @file contain.h + * @brief Implements pseudo object oriented features. + * @note Copyright (C) 2011 Richard Cochran + * + * 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 + +/* + * 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 diff --git a/makefile b/makefile index 901e10a..6c04669 100644 --- a/makefile +++ b/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) diff --git a/pi.c b/pi.c new file mode 100644 index 0000000..2bab166 --- /dev/null +++ b/pi.c @@ -0,0 +1,104 @@ +/** + * @file pi.c + * @brief Implements a Proportional Integral clock servo. + * @note Copyright (C) 2011 Richard Cochran + * + * 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 + +#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; +} diff --git a/pi.h b/pi.h new file mode 100644 index 0000000..4782543 --- /dev/null +++ b/pi.h @@ -0,0 +1,26 @@ +/** + * @file pi.h + * @note Copyright (C) 2011 Richard Cochran + * + * 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 diff --git a/servo.c b/servo.c new file mode 100644 index 0000000..a976af8 --- /dev/null +++ b/servo.c @@ -0,0 +1,43 @@ +/** + * @file servo.c + * @note Copyright (C) 2011 Richard Cochran + * + * 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 + +#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); +} diff --git a/servo.h b/servo.h new file mode 100644 index 0000000..5310f28 --- /dev/null +++ b/servo.h @@ -0,0 +1,76 @@ +/** + * @file servo.h + * @brief Implements a generic clock servo interface. + * @note Copyright (C) 2011 Richard Cochran + * + * 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 diff --git a/servo_private.h b/servo_private.h new file mode 100644 index 0000000..2c710e5 --- /dev/null +++ b/servo_private.h @@ -0,0 +1,33 @@ +/** + * @file servo_private.h + * @note Copyright (C) 2011 Richard Cochran + * + * 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