Diff: sync.c

Revision:

17:927fc1eceb9d

Parent:

14:7ef557918bb1

Child:

18:207dd1474cd9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/sync.c	Thu Jan 11 17:39:36 2018 +0000
@@ -0,0 +1,121 @@
+#include <stdlib.h>
+
+#include   "log.h"
+#include  "tick.h"
+#include  "time.h"
+#include "clock.h"
+
+#define ONE_BILLION 1000000000LL //Make sure ONE_BILLION is 64 bit by putting LL on the end
+#define ONE_MILLION    1000000
+
+#define TRUE  1
+#define FALSE 0
+
+int SyncTrace = FALSE;
+
+int SyncedTime = FALSE;
+int SyncedRate = FALSE;
+static void setSyncedTime(int64_t diff)
+{
+    int64_t absDiff = llabs(diff);
+    int64_t limit      = ClockSyncedLimitNs;
+    int64_t hysterisis = ClockSyncedHysterisNs;
+    
+    if (absDiff < limit - hysterisis) SyncedTime = TRUE;
+    if (absDiff > limit + hysterisis) SyncedTime = FALSE;
+}
+static void setSyncedRate(int64_t diff)
+{
+    
+    int64_t absDiff = llabs(diff);
+    int64_t limit      = ClockSyncedLimitPpb;
+    int64_t hysterisis = ClockSyncedHysterisPpb;
+    
+    if (absDiff < limit - hysterisis) SyncedRate = TRUE;
+    if (absDiff > limit + hysterisis) SyncedRate = FALSE;
+}
+
+static void setSlew(int64_t diff)
+{
+    int64_t slew = -diff / ClockSlewDivisor;
+    int32_t slewMaxNs = ClockSlewMaxMs * ONE_MILLION;
+    
+    if (slew >  slewMaxNs) slew =  slewMaxNs;
+    if (slew < -slewMaxNs) slew = -slewMaxNs;
+    
+    TickSetSlew(slew);
+    
+    if (SyncTrace) LogTimeF("Sync setSlew diff %lld gives slew %lld gives TickSlew %ld\r\n", diff, slew, TickGetSlew());
+}
+static void adjustPpb(int64_t diff)
+{    
+    int64_t toAdd = diff / ClockPpbDivisor;
+    int32_t maxAdd = ClockPpbChangeMax;
+
+    if (toAdd >  maxAdd) toAdd =  maxAdd;
+    if (toAdd < -maxAdd) toAdd = -maxAdd;
+            
+    TickAddPpb(-toAdd);
+    
+    if (SyncTrace) LogTimeF("Sync setPpb diff %lld gives toAdd %lld gives TickPpb %ld\r\n", diff, toAdd, TickGetPpb());
+}
+
+static int64_t lastIntClock = -1; //-1 indicates invalid value. 0 is a valid value.
+static int64_t lastExtClock = -1;
+static void reset(int64_t thisExtClock)
+{
+    TickSet(thisExtClock);
+    TickSetPpb(0);
+    lastIntClock = 0;
+    lastExtClock = 0;
+}
+
+static void sync(int64_t thisExtClock)
+{
+    
+    if (!TickIsSet()) //Cold start - only ever true if the RTC was not set.
+    {
+        LogTimeF("Sync - cold start of clock so resetting\r\n");
+        reset(thisExtClock);
+        return;
+    }
+
+    //Get the time at the time of the interrupt
+    int64_t thisIntClock;
+    int64_t thisAbsClock;
+    TickRetrieveSnapshot(&thisIntClock, &thisAbsClock);
+
+    //Calulate the time error
+    int64_t absDiff = thisAbsClock - thisExtClock;
+    if (llabs(absDiff) > ClockMaxOffsetSecs * ONE_BILLION)
+    {
+        LogTimeF("Sync - offset is greater than %d seconds so resetting\r\n", ClockMaxOffsetSecs);
+        reset(thisExtClock);
+        return;
+    } 
+    setSlew(absDiff);
+    setSyncedTime(absDiff);
+    
+    //Calculate the rate error
+    if (lastExtClock > -1)
+    {
+        int64_t extPeriod = thisExtClock - lastExtClock;
+        
+        int64_t intPeriod = thisIntClock - lastIntClock;
+        int64_t periodDiff =   intPeriod -    extPeriod;
+        
+        int64_t ppb;
+        if (extPeriod == ONE_BILLION) ppb = periodDiff; //This saves a 64bit multiplication and division for PPS
+        else                          ppb = periodDiff * ONE_BILLION / extPeriod;
+        
+        adjustPpb(ppb);
+        setSyncedRate(ppb);
+    }
+
+    //Save last values
+    lastIntClock = thisIntClock;
+    lastExtClock = thisExtClock;
+}
+void SyncPpsI(          ) { TickSaveSnapshotI(); }
+void SyncPpsN(time_t t  ) { int64_t ns = t * ONE_BILLION; sync(ns); }
+void SyncNs  (int64_t ns) { TickSaveSnapshotI();          sync(ns); }