Improve extras/sntp -- Issue #562 (#563)

extras/sntp/component.mk

@@ -5,4 +5,9 @@ INC_DIRS += $(sntp_ROOT)
 # args for passing into compile rule generation
 sntp_SRC_DIR =  $(sntp_ROOT)
 
+# For SNTP logging, either supply own SNTP_LOGD
+# or define SNTP_LOGD_WITH_PRINTF (see sntp_fun.c)
+
+# sntp_CFLAGS = $(CFLAGS) -DSNTP_LOGD_WITH_PRINTF
+
 $(eval $(call component_compile_rules,sntp))

extras/sntp/sntp_fun.c

@@ -8,11 +8,22 @@
 #include <sys/types.h>
 #include <sys/errno.h>
 #include <stdio.h>
+#include <sys/time.h>
 #include <espressif/esp_common.h>
 #include <esp/timer.h>
 #include <esp/rtc_regs.h>
 #include <sntp.h>
 
+
+#ifdef  SNTP_LOGD_WITH_PRINTF
+#define SNTP_LOGD(FMT, ...) printf(FMT "\n", ##__VA_ARGS__)
+#endif
+
+#ifndef SNTP_LOGD
+#define SNTP_LOGD(...)
+#define SKIP_DIAGNOSTICS
+#endif
+
 #define TIMER_COUNT			RTC.COUNTER
 
 // daylight settings
@@ -20,9 +31,14 @@
 #define sntp_base	(*((uint64_t*)RTC.SCRATCH))
 // Timer value when base was obtained
 #define tim_ref 	(RTC.SCRATCH[2])
-// Calibration value
+// Calibration value -- ( microseconds / RTC tick ) * 2^12
 #define cal 		(RTC.SCRATCH[3])
 
+#ifndef SKIP_DIAGNOSTICS
+// Keep the last time SNTP updated the time
+static struct timeval last_update_time = {0, 0};
+#endif
+
 // Timezone related data.
 static struct timezone stz;
 
@@ -50,7 +66,7 @@ void sntp_initialize(const struct timezone *tz) {
 	}
 	sntp_base = 0;
 	// To avoid div by 0 exceptions if requesting time before SNTP config
-	cal = 1;
+	cal = sdk_system_rtc_clock_cali_proc();
 	tim_ref = TIMER_COUNT;
 	sntp_init();
 }
@@ -63,7 +79,7 @@ static inline void sntp_check_timer_wrap(uint32_t current_value) {
 		// Timer wrap has occurred, compensate by subtracting 2^32 to ref.
 		sntp_base -= 1LLU<<32;
 		// DEBUG
-		printf("\nTIMER WRAPPED!\n");
+		SNTP_LOGD("SNTP RTC counter wrapped");
 	}
 }
 
@@ -77,9 +93,9 @@ inline time_t sntp_get_rtc_time(int32_t *us) {
 	// Check for timer wrap
 	sntp_check_timer_wrap(tim);
 	base = sntp_base + tim - tim_ref;
-	secs = base * cal / (1000000U<<12);
+	secs = (base * cal) / (1000000U<<12);
 	if (us) {
-		*us = base * cal % (1000000U<<12);
+		*us = ((base * cal) % (1000000U<<12)) >>12;
 	}
 	return secs;
 }
@@ -97,16 +113,53 @@ int _gettimeofday_r(struct _reent *r, struct timeval *tp, void *tzp) {
 
 // Update RTC timer. Called by SNTP module each time it receives an update.
 void sntp_update_rtc(time_t t, uint32_t us) {
+
+    uint32_t now_rtc = TIMER_COUNT;
+
     // Apply daylight and timezone correction
     t += (stz.tz_minuteswest + stz.tz_dsttime * 60) * 60;
-	// DEBUG: Compute and print drift
-	int64_t sntp_current = sntp_base + TIMER_COUNT - tim_ref;
-	int64_t sntp_correct = (((uint64_t)us + (uint64_t)t * 1000000U)<<12) / cal;
-	printf("\nRTC Adjust: drift = %lld ticks, cal = %d\n", (time_t)(sntp_correct - sntp_current), (uint32_t)cal);
 
-	tim_ref = TIMER_COUNT;
-	cal = sdk_system_rtc_clock_cali_proc();
+#ifndef SKIP_DIAGNOSTICS
+
+    int64_t sntp_reference_time, local_clock_time, clock_difference;
+    struct timeval this_update_time, elapsed_since_update;
+    double ppm;
+
+    // Calculate in diagnostics in microseconds
+    sntp_reference_time = (uint64_t)us + (uint64_t)t * 1000000U;
+    local_clock_time = ((sntp_base + now_rtc - tim_ref) * cal) / (1U<<12);
+    clock_difference = sntp_reference_time - local_clock_time;
+
+    this_update_time.tv_sec  = t;
+    this_update_time.tv_usec = us;
+
+    timersub(&this_update_time, &last_update_time, &elapsed_since_update);
+
+    // If over a day since last update, don't trust the last_update_time
+    if (elapsed_since_update.tv_sec < 24 * 60 * 60 ) {
+        ppm = ((double)clock_difference /
+               ((double)elapsed_since_update.tv_sec * 1000000
+                        + elapsed_since_update.tv_usec))
+              * 1000000;
+        SNTP_LOGD("SNTP RTC adjust: %0.3Lf s; %0.3f ppm over %0.3f s; cal: %u\n",
+                  ((long double)clock_difference)/1000000,
+                  ppm,
+                  (double)elapsed_since_update.tv_sec
+                  + ((double)elapsed_since_update.tv_usec)/1000000,
+                  cal);
+    } else {
+        SNTP_LOGD("SNTP RTC adjust: %0.3Lf s; cal: %u\n",
+                  ((long double)clock_difference)/1000000, cal);
+    }
+
+    last_update_time.tv_sec  = t;
+    last_update_time.tv_usec = us;
+
+#endif // SKIP_DIAGNOSTICS
+
+	cal = sdk_system_rtc_clock_cali_proc();
+    tim_ref = now_rtc;
+	sntp_base = (((uint64_t)us + (uint64_t)t * 1000000U) <<12) / cal;
 
-	sntp_base = (((uint64_t)us + (uint64_t)t * 1000000U)<<12) / cal;
 }