feat(radio): Automatic clock drift compensation

radio/src/cli.cpp

@@ -1102,6 +1102,23 @@ int cliSet(const char **argv)
       return -1;
     }
   }
+#if defined(DEBUG)
+  else if (!strcmp(argv[1], "rtccal")) {
+    int ppm = 0;
+    if (!strcmp(argv[2], "reset")) {
+      rtcResetCalibration();
+    } else if (toInt(argv, 2, &ppm) > 0) {
+      rtcSetCalibration(rtcCalibrationUnits(ppm));
+    } else {
+      cliSerialPrint("%s: expected \"reset\" or a ppm value", argv[0]);
+      return -1;
+    }
+    int32_t units = rtcGetCalibration();
+    cliSerialPrint("rtc calibration = %d units (%d ppm x10), reference = %u",
+                   (int)units, (int)rtcCalibrationPpm10(units),
+                   (unsigned)rtcGetCalibrationRef());
+  }
+#endif
 #if !defined(SOFTWARE_VOLUME) && defined(AUDIO)
   else if (!strcmp(argv[1], "volume")) {
     int level = 0;
@@ -1496,6 +1513,10 @@ int cliDisplay(const char ** argv)
     struct gtm utm;
     gettime(&utm);
     cliSerialPrint("rtc = %4d-%02d-%02d %02d:%02d:%02d.%02d0", utm.tm_year+TM_YEAR_BASE, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100);
+    int32_t units = rtcGetCalibration();
+    cliSerialPrint("rtc calibration = %d units (%d ppm x10), reference = %u",
+                   (int)units, (int)rtcCalibrationPpm10(units),
+                   (unsigned)rtcGetCalibrationRef());
   }
 #if defined(VOLUME_I2C_ADDRESS)
   else if (!strcmp(argv[1], "volume")) {

radio/src/rtc.cpp

@@ -22,8 +22,6 @@
 #include <limits.h>
 #include "edgetx.h"
 
-extern void rtcdriver_settime(struct gtm * t);
-
 #define LEAP_SECONDS_POSSIBLE 0
 
 /* Shift A right by B bits portably, by dividing A by 2**B and
@@ -445,8 +443,6 @@ void gettime(struct gtm * tm)
   filltm(&g_rtcTime, tm); // create a struct tm date/time structure from global unix time stamp
 }
 
-void rtcGetTime(struct gtm * t);
-
 #define RTC_ADJUST_PERIOD      60   // how often RTC is checked for accuracy [seconds]
 #define RTC_ADJUST_TRESHOLD    20   // how much clock must differ before adjustment is made [seconds]
 /*
@@ -494,6 +490,121 @@ uint8_t rtcAdjust(uint16_t year, uint8_t mon, uint8_t day, uint8_t hour, uint8_t
   return 0;
 }
 
+// Crystal drift measured between two clock settings, compensated by the RTC
+// smooth calibration hardware. Reference kept in the RTC backup domain.
+
+#define RTC_CALIB_MIN_ELAPSED   SECS_PER_DAY              // no reliable measurement below that
+#define RTC_CALIB_MAX_ELAPSED   (10 * 365 * SECS_PER_DAY) // nonsense reference
+#define RTC_CALIB_MIN_ERROR     4                         // [s] below that it is input noise
+#define RTC_CALIB_SET_ACCURACY  2                         // [s] hand setting accuracy
+#define RTC_CALIB_MAX_PPM       200                       // beyond any crystal: clock was set
+#define RTC_CALIB_TZ_STEP       900                       // [s] time zone granularity
+#define RTC_CALIB_TZ_BAND       60                        // [s] tolerance around it
+#define RTC_CALIB_MIN_YEAR      101                       // 2001, RTC resets to 2000
+#define RTC_CALIB_SESSION_GAP   (5 * 60 * 100)            // [10ms] same edit session
+
+// Sampled before the clock got changed
+static bool rtcCalibSession = false;
+static tmr10ms_t rtcCalibLastSet = 0;
+static gtime_t rtcCalibSessionRef = 0;
+static gtime_t rtcCalibSessionRtc = 0;
+static int32_t rtcCalibSessionUnits = 0;
+static bool rtcCalibSessionValid = false;
+
+int32_t rtcCalibrationPpm10(int32_t units)
+{
+  return (int32_t)(((int64_t)units * 10000000) / RTC_CALIB_UNITS_PER_SECOND);
+}
+
+int32_t rtcCalibrationUnits(int32_t ppm)
+{
+  int64_t v = (int64_t)ppm * RTC_CALIB_UNITS_PER_SECOND;
+  v += (v < 0) ? -500000 : 500000;
+  return (int32_t)(v / 1000000);
+}
+
+static void rtcCalibrationStart()
+{
+  struct gtm before = {};
+  rtcGetTime(&before);
+
+  rtcCalibSessionValid = (before.tm_year >= RTC_CALIB_MIN_YEAR);
+  rtcCalibSessionRtc = rtcCalibSessionValid ? gmktime(&before) : 0;
+  rtcCalibSessionRef = rtcGetCalibrationRef();
+  rtcCalibSessionUnits = rtcGetCalibration();
+  rtcCalibSession = true;
+}
+
+// Recomputed from the calibration in use at session start, so that successive
+// edits converge instead of piling up
+static void rtcCalibrationUpdate(gtime_t newTime)
+{
+  // Initial setting, or clock that lost power
+  if (rtcCalibSessionRef == 0 || !rtcCalibSessionValid) return;
+
+  gtime_t elapsed = newTime - rtcCalibSessionRef;
+  if (elapsed < (gtime_t)RTC_CALIB_MIN_ELAPSED || elapsed > (gtime_t)RTC_CALIB_MAX_ELAPSED) return;
+
+  gtime_t error = rtcCalibSessionRtc - newTime; // > 0 when the clock runs fast
+  int64_t absError = (error < 0) ? -(int64_t)error : (int64_t)error;
+
+  // Too small to tell from input noise
+  if (absError < RTC_CALIB_MIN_ERROR) return;
+
+  // Faster than any crystal: the clock was moved
+  if (absError * 1000000 > (int64_t)elapsed * RTC_CALIB_MAX_PPM) return;
+
+  // A whole time zone step is a time zone or DST change
+  if (absError >= RTC_CALIB_TZ_STEP - RTC_CALIB_TZ_BAND) {
+    int64_t rem = absError % RTC_CALIB_TZ_STEP;
+    if (rem <= RTC_CALIB_TZ_BAND || rem >= RTC_CALIB_TZ_STEP - RTC_CALIB_TZ_BAND) return;
+  }
+
+  // Do not over correct on a short measurement
+  absError -= RTC_CALIB_SET_ACCURACY;
+
+  int32_t delta = (int32_t)((absError * RTC_CALIB_UNITS_PER_SECOND + elapsed / 2) / elapsed);
+  if (error < 0) delta = -delta;
+
+  int32_t units = rtcCalibSessionUnits - delta;
+  if (units > RTC_CALIB_UNIT_MAX) units = RTC_CALIB_UNIT_MAX;
+  if (units < RTC_CALIB_UNIT_MIN) units = RTC_CALIB_UNIT_MIN;
+
+  rtcSetCalibration(units);
+
+  TRACE("RTC drift %d s over %d s, calibration %d -> %d units (%d -> %d ppm x10)",
+        (int)error, (int)elapsed, (int)rtcCalibSessionUnits, (int)units,
+        (int)rtcCalibrationPpm10(rtcCalibSessionUnits),
+        (int)rtcCalibrationPpm10(units));
+}
+
+// Clears the hardware trim and the stored reference, so the next two settings
+// start a fresh measurement
+void rtcResetCalibration()
+{
+  rtcSetCalibration(0);
+  rtcClearCalibrationRef();
+  rtcCalibSession = false;
+  rtcCalibLastSet = 0;
+}
+
+void rtcSetTime(const struct gtm * t)
+{
+  struct gtm tm = *t;
+  gtime_t newTime = gmktime(&tm);
+
+  tmr10ms_t now = get_tmr10ms();
+  if (!rtcCalibSession || (now - rtcCalibLastSet) > RTC_CALIB_SESSION_GAP) {
+    rtcCalibrationStart();
+  }
+  rtcCalibLastSet = now;
+
+  rtcDriverSetTime(t);
+
+  rtcCalibrationUpdate(newTime);
+  rtcSetCalibrationRef(newTime);
+}
+
 bool rtcIsValid()
 {
   struct gtm t;

radio/src/rtc.h

@@ -55,6 +55,28 @@ void rtcSetTime(const struct gtm * tm);
 gtime_t gmktime (struct gtm *tm);
 uint8_t rtcAdjust(uint16_t year, uint8_t mon, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec);
 
+// Driver interface, rtcSetTime() wraps rtcDriverSetTime()
+void rtcDriverSetTime(const struct gtm * tm);
+void rtcGetTime(struct gtm * tm);
+
+// Smooth calibration, one unit is one clock pulse out of 2^20 (~0.954 ppm)
+#define RTC_CALIB_UNITS_PER_SECOND  1048576
+#define RTC_CALIB_UNIT_MAX          512
+#define RTC_CALIB_UNIT_MIN          (-511)
+
+int32_t rtcGetCalibration();
+void rtcSetCalibration(int32_t units);
+int32_t rtcCalibrationPpm10(int32_t units);
+int32_t rtcCalibrationUnits(int32_t ppm);
+
+// Time of the last known good setting, 0 when unknown
+gtime_t rtcGetCalibrationRef();
+void rtcSetCalibrationRef(gtime_t t);
+void rtcClearCalibrationRef();
+
+// Back to a factory-fresh state, as if the backup domain had been lost
+void rtcResetCalibration();
+
 #if defined(__cplusplus) && !defined(SIMU)
 extern "C" {
 #endif

radio/src/targets/common/arm/stm32/f4/CMakeLists.txt

@@ -41,6 +41,7 @@ if(NOT NATIVE_BUILD)
     ${STM32CUBE_DIR}/Src/stm32f4xx_hal_rcc.c
     ${STM32CUBE_DIR}/Src/stm32f4xx_hal_rcc_ex.c
     ${STM32CUBE_DIR}/Src/stm32f4xx_hal_rtc.c
+    ${STM32CUBE_DIR}/Src/stm32f4xx_hal_rtc_ex.c
     ${STM32CUBE_DIR}/Src/stm32f4xx_hal_i2c.c
     ${STM32CUBE_DIR}/Src/stm32f4xx_hal_i2c_ex.c
     ${STM32CUBE_DIR}/Src/stm32f4xx_hal_sd.c

radio/src/targets/common/arm/stm32/h7/CMakeLists.txt

@@ -44,6 +44,7 @@ if(NOT NATIVE_BUILD)
       ${STM32CUBE_SRC_PREFIX}_hal_rcc.c
       ${STM32CUBE_SRC_PREFIX}_hal_rcc_ex.c
       ${STM32CUBE_SRC_PREFIX}_hal_rtc.c
+      ${STM32CUBE_SRC_PREFIX}_hal_rtc_ex.c
       ${STM32CUBE_SRC_PREFIX}_hal_i2c.c
       ${STM32CUBE_SRC_PREFIX}_hal_i2c_ex.c
       ${STM32CUBE_SRC_PREFIX}_hal_i2s.c
@@ -85,6 +86,7 @@ if(NOT NATIVE_BUILD)
       ${STM32CUBE_SRC_PREFIX}_hal_rcc.c
       ${STM32CUBE_SRC_PREFIX}_hal_rcc_ex.c
       ${STM32CUBE_SRC_PREFIX}_hal_rtc.c
+      ${STM32CUBE_SRC_PREFIX}_hal_rtc_ex.c
       ${STM32CUBE_SRC_PREFIX}_hal_i2c.c
       ${STM32CUBE_SRC_PREFIX}_hal_i2c_ex.c
       ${STM32CUBE_SRC_PREFIX}_hal_i2s.c

radio/src/targets/common/arm/stm32/rtc_driver.cpp

@@ -24,7 +24,7 @@
 
 RTC_HandleTypeDef rtc = {};
 
-void rtcSetTime(const struct gtm * t)
+void rtcDriverSetTime(const struct gtm * t)
 {
   g_ms100 = 0; // start of next second begins now
 
@@ -57,6 +57,63 @@ void rtcGetTime(struct gtm * t)
   t->tm_mday = RTC_DateStruct.Date;
 }
 
+#if defined(RTC_CALR_CALM) && !defined(BOOT)
+
+// DR0 is left alone, legacy code uses it for shutdown/soft reset requests
+#define RTC_CALIB_BKP_MAGIC_REG   RTC_BKP_DR1
+#define RTC_CALIB_BKP_REF_REG     RTC_BKP_DR2
+#define RTC_CALIB_BKP_MAGIC       0x52544301
+
+int32_t rtcGetCalibration()
+{
+  uint32_t calr = READ_REG(rtc.Instance->CALR);
+  int32_t units = -(int32_t)(calr & RTC_CALR_CALM);
+  if (calr & RTC_CALR_CALP) units += 512;
+  return units;
+}
+
+void rtcSetCalibration(int32_t units)
+{
+  if (units > RTC_CALIB_UNIT_MAX) units = RTC_CALIB_UNIT_MAX;
+  if (units < RTC_CALIB_UNIT_MIN) units = RTC_CALIB_UNIT_MIN;
+
+  // CALP adds 512 pulses, CALM removes up to 511
+  uint32_t plus = (units > 0) ? RTC_SMOOTHCALIB_PLUSPULSES_SET
+                              : RTC_SMOOTHCALIB_PLUSPULSES_RESET;
+  uint32_t minus = (units > 0) ? (512 - units) : -units;
+
+  HAL_RTCEx_SetSmoothCalib(&rtc, RTC_SMOOTHCALIB_PERIOD_32SEC, plus, minus);
+}
+
+gtime_t rtcGetCalibrationRef()
+{
+  if (HAL_RTCEx_BKUPRead(&rtc, RTC_CALIB_BKP_MAGIC_REG) != RTC_CALIB_BKP_MAGIC)
+    return 0;
+  return (gtime_t)HAL_RTCEx_BKUPRead(&rtc, RTC_CALIB_BKP_REF_REG);
+}
+
+void rtcSetCalibrationRef(gtime_t t)
+{
+  HAL_RTCEx_BKUPWrite(&rtc, RTC_CALIB_BKP_REF_REG, (uint32_t)t);
+  HAL_RTCEx_BKUPWrite(&rtc, RTC_CALIB_BKP_MAGIC_REG, RTC_CALIB_BKP_MAGIC);
+}
+
+void rtcClearCalibrationRef()
+{
+  HAL_RTCEx_BKUPWrite(&rtc, RTC_CALIB_BKP_MAGIC_REG, 0);
+  HAL_RTCEx_BKUPWrite(&rtc, RTC_CALIB_BKP_REF_REG, 0);
+}
+
+#else // no smooth calibration hardware
+
+int32_t rtcGetCalibration() { return 0; }
+void rtcSetCalibration(int32_t units) { (void)units; }
+gtime_t rtcGetCalibrationRef() { return 0; }
+void rtcSetCalibrationRef(gtime_t t) { (void)t; }
+void rtcClearCalibrationRef() {}
+
+#endif
+
 void rtcInit()
 {
   rtc.Instance = RTC;

radio/src/targets/simu/simulib.cpp

@@ -447,10 +447,16 @@ void rtcGetTime(struct gtm * t)
 {
 }
 
-void rtcSetTime(const struct gtm * t)
+void rtcDriverSetTime(const struct gtm * t)
 {
 }
 
+int32_t rtcGetCalibration() { return 0; }
+void rtcSetCalibration(int32_t units) { (void)units; }
+gtime_t rtcGetCalibrationRef() { return 0; }
+void rtcSetCalibrationRef(gtime_t t) { (void)t; }
+void rtcClearCalibrationRef() {}
+
 #if defined(PCBTARANIS)
 void sdPoll10ms() {}
 #endif