Skip to content

Commit

Permalink
Simplify time adjust, use average drift to avoid one-off cases
Browse files Browse the repository at this point in the history
  • Loading branch information
@pascal-fb-martin
pascal-fb-martin committed Oct 27, 2019
1 parent 5b76dd7 commit 3149ae9
Show file tree
Hide file tree
Showing 4 changed files with 111 additions and 81 deletions.
157 changes: 88 additions & 69 deletions hc_clock.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,11 +53,14 @@
*/

#include <time.h>
#include <errno.h>

#include "houseclock.h"
#include "hc_clock.h"
#include "hc_db.h"

#define HC_CLOCK_LEARNING_PERIOD 10

static int clockPrecision;
static int clockShowDrift = 0;

Expand All @@ -77,10 +80,15 @@ const char *hc_clock_help (int level) {
return clockHelp[level];
}

static void hc_clock_start_learning (void) {
hc_clock_status_db->count = 0;
hc_clock_status_db->accumulator = 0;
}

void hc_clock_initialize (int argc, const char **argv) {

int i;
const char *precision_option = "50"; // ms
const char *precision_option = "10"; // ms

clockShowDrift = 0;

Expand Down Expand Up @@ -110,96 +118,107 @@ void hc_clock_initialize (int argc, const char **argv) {
hc_clock_status_db->synchronized = 0;
hc_clock_status_db->precision = clockPrecision;
hc_clock_status_db->drift = 0;
hc_clock_start_learning ();
}

static void hc_clock_force (const struct timeval *gps,
const struct timeval *local) {

struct timeval now;
struct timeval corrected = *gps;

gettimeofday (&now, NULL);

// Correct the GPS time from the time of the fix to this
// current time, as estimated using the local clock.
//
corrected.tv_sec += (now.tv_sec - local->tv_sec);
corrected.tv_usec += (now.tv_usec - local->tv_usec);
if (corrected.tv_usec > 1000000) {
corrected.tv_sec += 1;
corrected.tv_usec -= 1000000;
} else if (corrected.tv_usec < 0) {
corrected.tv_sec -= 1;
corrected.tv_usec += 1000000;
}

DEBUG {
printf ("Forcing time to %ld.%03.3d seconds\n",
(long)(corrected.tv_sec), (int)(corrected.tv_usec/1000));
}
if (settimeofday (&corrected, NULL) != 0) {
printf ("settimeofday() error %d\n", errno);
}
}

static void hc_clock_adjust (time_t drift) {

struct timeval delta;

delta.tv_sec = (drift / 1000);
delta.tv_usec = (drift % 1000) * 1000;
if (adjtime (&delta, NULL) != 0) {
printf ("adjtime() error %d\n", errno);
}
}

void hc_clock_synchronize(const struct timeval *gps,
const struct timeval *local, int latency) {

if (hc_clock_drift_db == 0) return;
if (hc_clock_status_db == 0) return;

time_t drift = ((gps->tv_sec - local->tv_sec) * 1000)
+ ((gps->tv_usec - local->tv_usec) / 1000) + latency;

time_t absdrift = (drift < 0)? (0 - drift) : drift;

if (hc_clock_drift_db) hc_clock_drift_db[local->tv_sec%120] = (int)drift;
if (hc_clock_status_db) {
hc_clock_status_db->drift = (int)drift;
hc_clock_status_db->timestamp = *local;
}
hc_clock_drift_db[gps->tv_sec%120] = (int)drift;
hc_clock_status_db->drift = (int)drift;
hc_clock_status_db->timestamp = *local;

if (clockShowDrift || hc_test_mode()) {
printf ("[%d]=%8.3f\n", local->tv_sec%120, drift/1000.0);
return;
}

if (absdrift < clockPrecision) {
clockSynchronized = 1;
if (hc_clock_status_db) hc_clock_status_db->synchronized = 1;
return;
}

// GPS and local system time have drifted apart.

DEBUG {
printf ("Detected drift of %s%ld.%03.3d seconds\n",
(drift < 0)?"-":"",
(long)(absdrift/1000), (int)(absdrift%1000));
printf (" GPS = %ld.%3.3d, System = %ld.%3.3d\n",
gps->tv_sec, gps->tv_usec, local->tv_sec, local->tv_usec);
printf ("[%d] %8.3f\n", local->tv_sec%120, drift/1000.0);
if (hc_test_mode()) return;
}

if (absdrift >= 10000) {

// Too much of a difference: force system time.
//
struct timeval now;
struct timeval adjusted = *gps;

gettimeofday (&now, NULL);

// Adjust the GMT time from the time of the fix to this
// current time, as estimated using the local clock.
//
adjusted.tv_sec += (now.tv_sec - local->tv_sec);
adjusted.tv_usec += (now.tv_usec - local->tv_usec);
if (adjusted.tv_usec > 1000000) {
adjusted.tv_sec += 1;
adjusted.tv_usec -= 1000000;
} else if (adjusted.tv_usec < 0) {
adjusted.tv_sec -= 1;
adjusted.tv_usec += 1000000;
}
hc_clock_force (gps, local);
hc_clock_start_learning();
return;
}

DEBUG {
printf ("Forcing time to %ld.%03.3d seconds\n",
(long)(adjusted.tv_sec), (int)(adjusted.tv_usec/1000));
}
settimeofday (&adjusted, NULL);
// Accumulate an average drift, to eliminate one-time issues.
//
hc_clock_status_db->accumulator += (int)drift;
hc_clock_status_db->count += 1;
if (hc_clock_status_db->count < HC_CLOCK_LEARNING_PERIOD) return;

// We reached the end of a learning period.
// At this point we consider only the average drift
// calculated over the past learning period.
//
drift = hc_clock_status_db->accumulator / hc_clock_status_db->count;
absdrift = (drift < 0)? (0 - drift) : drift;
if (clockShowDrift)
printf ("Average drift: %d ms\n", drift);

if (absdrift < clockPrecision) {
clockSynchronized = 1;
hc_clock_status_db->synchronized = 1;
} else {

// Small difference: adjust the time progressively.
// GPS and local system time have drifted apart
// by a small difference: adjust the time progressively.
//
time_t leftover;
struct timeval delta;

// What is the current situation in the kernel?
// We do not want to interrupt an adjustment that
// is meant to correct the current drift, however
// if the drift has reversed sign, we need to reverse
// direction of the adjustment. (Remember that the drift
// value must be significant to have gotten here.)

adjtime (NULL, &delta);
leftover = (delta.tv_sec * 1000) + (delta.tv_usec / 1000);

if (leftover * drift <= 0) { // Different adjustment
DEBUG printf ("New adjustment: %d replaces %d\n",
drift, leftover);
delta.tv_sec = (drift / 1000);
delta.tv_usec = (drift % 1000) * 1000;
adjtime (&delta, NULL);
DEBUG {
printf ("Time adjust at GPS = %ld.%3.3d, System = %ld.%3.3d\n",
(long)gps->tv_sec, (int)gps->tv_usec/1000,
(long)local->tv_sec, (int)local->tv_usec/1000);
}
hc_clock_adjust (drift);
}
hc_clock_start_learning();
}

int hc_clock_synchronized (void) {
Expand Down
8 changes: 5 additions & 3 deletions hc_clock.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,9 +34,11 @@ int hc_clock_synchronized (void);
#define HC_CLOCK_STATUS "ClockStatus"

typedef struct {
int synchronized;
int precision;
int drift;
struct timeval timestamp;
int drift;
short precision;
char synchronized;
char count;
int accumulator;
} hc_clock_status;

7 changes: 5 additions & 2 deletions hc_http.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,8 @@ static const char *hc_http_status (const char *method, const char *uri,
",\"time\":[%c%c,%c%c,%c%c],\"date\":[%d,%c%c,%c%c]"
",\"latitude\":%s,\"longitude\":%s}"
",\"clock\":{\"synchronized\":%s"
",\"precision\":%d,\"drift\":%d,\"timestamp\":%zd.%03d}}",
",\"precision\":%d,\"drift\":%d,\"timestamp\":%zd.%03d}"
",\"learn\":{\"count\":%d,\"accumulator\":%d}}",
nmea_db->fix?"true":"false",
nmea_db->time[0], nmea_db->time[1],
nmea_db->time[2], nmea_db->time[3],
Expand All @@ -120,7 +121,9 @@ static const char *hc_http_status (const char *method, const char *uri,
clock_db->precision,
clock_db->drift,
(size_t) (clock_db->timestamp.tv_sec),
clock_db->timestamp.tv_usec/1000);
clock_db->timestamp.tv_usec/1000,
clock_db->count, clock_db->accumulator);

echttp_content_type_json();
return JsonBuffer;
}
Expand Down
20 changes: 13 additions & 7 deletions hc_nmea.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,7 @@ static int gpsTty = 0;

static int gpsUseBurst = 0;
static int gpsPrivacy = 0;
static int gpsShowNmea = 0;

static hc_nmea_status *hc_nmea_status_db = 0;

Expand All @@ -156,6 +157,7 @@ const char *hc_nmea_help (int level) {
" [-gps=DEV] [-latency=N] [-burst] [-privacy]",
"-gps=DEV: TTY device from which to read the NMEA data.\n"
"-latency=N: delay between the GPS fix and the 1st NMEA sentence.\n"
"-show-nmea: trace NMEA sentences.\n"
"-burst: Use burst start as the GPS timing reference\n"
"-privacy: do not export location",
NULL
Expand Down Expand Up @@ -190,6 +192,7 @@ int hc_nmea_initialize (int argc, const char **argv) {
hc_match ("-latency=", argv[i], &latency_option);
if (hc_present ("-burst", argv[i])) gpsUseBurst = 1;
if (hc_present ("-privacy", argv[i])) gpsPrivacy = 1;
if (hc_present ("-show-nmea", argv[i])) gpsShowNmea = 1;
}
gpsLatency = atoi(latency_option);

Expand Down Expand Up @@ -400,7 +403,7 @@ static int hc_nmea_ready (int flags) {
if (flags & GPSFLAGS_NEWBURST) burstinfo = "new";

if (flags) {
DEBUG printf ("(%s fix, %s burst)\n", fixinfo, burstinfo);
if(gpsShowNmea) printf ("(%s fix, %s burst)\n", fixinfo, burstinfo);
}
return (flags == GPSFLAGS_NEWFIX+GPSFLAGS_NEWBURST);
}
Expand Down Expand Up @@ -458,17 +461,20 @@ int hc_nmea_process (const struct timeval *received) {
// We multiply the speed by 1000 to get some precision.
// The other 1000 is because gpsDuration is in milliseconds.
speed = (1000 * 1000 * gpsTotal) / gpsDuration;
DEBUG printf ("Calculated speed: %d.%03d Bytes/s\n",
speed/1000, speed%1000);
if(gpsShowNmea)
printf ("Calculated speed: %d.%03d Bytes/s\n",
speed/1000, speed%1000);
} else {
speed = 115000; // Arbitrary speed at the beginning.
}

if (previous.tv_usec > 0 && interval > 500) {
hc_nmea_timing (received, &bursttiming, speed, gpsCount);
DEBUG printf ("Data received at %d.%03d, burst started at %d.%03d\n",
received->tv_sec, received->tv_usec/1000,
bursttiming.tv_sec, bursttiming.tv_usec/1000);
if (gpsShowNmea) {
printf ("Data received at %d.%03d, burst started at %d.%03d\n",
received->tv_sec, received->tv_usec/1000,
bursttiming.tv_sec, bursttiming.tv_usec/1000);
}
// Whatever GPS time we got before is now old.
gpsTime[0] = 0;
flags = GPSFLAGS_NEWBURST;
Expand All @@ -488,7 +494,7 @@ int hc_nmea_process (const struct timeval *received) {

if (gpsBuffer[start++] != '$') continue; // Skip invalid sentence.

DEBUG {
if (gpsShowNmea) {
printf ("%11d.%03.3d: %s\n",
timing.tv_sec, timing.tv_usec/1000, gpsBuffer+start);
}
Expand Down

0 comments on commit 3149ae9

Please sign in to comment.