STM32: clock sync: Simple PI controller for speed adjustments; converges to +-100 usec in few minutes

libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp

@@ -50,6 +50,13 @@ uavcan::int32_t getUtcSpeedCorrectionPPM();
  */
 uavcan::uint32_t getUtcAjdustmentJumpCount();
 
+/**
+ * Returns clock error sampled at previous UTC adjustment.
+ * Positive if the hardware timer is slower than reference time.
+ * This function is thread safe.
+ */
+uavcan::UtcDuration getPrevUtcAdjustment();
+
 }
 
 /**

libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp

@@ -39,6 +39,7 @@ bool utc_set = false;
 
 uavcan::uint32_t utc_jump_cnt = 0;
 uavcan::int32_t utc_correction_usec_per_overflow_x16 = 0;
+uavcan::int64_t prev_adjustment = 0;
 
 uavcan::uint64_t time_mono = 0;
 uavcan::uint64_t time_utc = 0;
@@ -147,43 +148,25 @@ void adjustUtc(uavcan::UtcDuration adjustment)
     MutexLocker mlocker(mutex);
 
     assert(initialized);
-    if (adjustment.isZero() && utc_set)
-    {
-        return; // Perfect sync
-    }
 
     /*
-     * Naive speed adjustment (proof of concept)
-     * TODO: Reliable clock speed adjustment algorithm
+     * Naive speed adjustment - discrete PI controller.
+     * TODO: More reliable clock speed adjustment algorithm
      */
-    if (adjustment.isPositive())
-    {
-        if (utc_correction_usec_per_overflow_x16 < 0)
-        {
-            utc_correction_usec_per_overflow_x16 += 4;
-        }
-        else if (utc_correction_usec_per_overflow_x16 < MaxUtcSpeedCorrectionX16)
-        {
-            utc_correction_usec_per_overflow_x16 += 1;
-        }
-    }
-    else
-    {
-        if (utc_correction_usec_per_overflow_x16 > 0)
-        {
-            utc_correction_usec_per_overflow_x16 -= 4;
-        }
-        else if (utc_correction_usec_per_overflow_x16 > -MaxUtcSpeedCorrectionX16)
-        {
-            utc_correction_usec_per_overflow_x16 -= 1;
-        }
-    }
+    const uavcan::int64_t adj_delta = adjustment.toUSec() - prev_adjustment;  // This is the P term
+    prev_adjustment = adjustment.toUSec();
+
+    utc_correction_usec_per_overflow_x16 += adjustment.isPositive() ? 1 : -1; // I
+    utc_correction_usec_per_overflow_x16 += (adj_delta > 0) ? 1 : -1;         // P
+
+    utc_correction_usec_per_overflow_x16 = std::max(utc_correction_usec_per_overflow_x16, -MaxUtcSpeedCorrectionX16);
+    utc_correction_usec_per_overflow_x16 = std::min(utc_correction_usec_per_overflow_x16,  MaxUtcSpeedCorrectionX16);
 
     /*
      * Clock value adjustment
-     * For small adjustments we will rely only on speed change
+     * For small adjustments (less than 3 msec) we will rely only on speed change
      */
-    if (adjustment.getAbs().toMSec() > 1 || !utc_set)
+    if (adjustment.getAbs().toMSec() > 2 || !utc_set)
     {
         const uavcan::int64_t adj_usec = adjustment.toUSec();
 
@@ -223,6 +206,12 @@ uavcan::uint32_t getUtcAjdustmentJumpCount()
     return utc_jump_cnt;
 }
 
+uavcan::UtcDuration getPrevUtcAdjustment()
+{
+    MutexLocker mlocker(mutex);
+    return uavcan::UtcDuration::fromUSec(prev_adjustment);
+}
+
 } // namespace clock
 
 SystemClock& SystemClock::instance()

libuavcan_drivers/stm32/test_stm32f107/src/main.cpp

@@ -166,8 +166,9 @@ int main()
         }
 
         const uavcan::UtcTime utc = uavcan_stm32::clock::getUtc();
-        lowsyslog("UTC %lu sec, %li corr, %lu jumps\n",
+        lowsyslog("UTC %lu sec   Absolute error: %li usec   Speed correction: %liPPM   Jumps: %lu\n",
                   static_cast<unsigned long>(utc.toMSec() / 1000),
+                  static_cast<long>(uavcan_stm32::clock::getPrevUtcAdjustment().toUSec()),
                   uavcan_stm32::clock::getUtcSpeedCorrectionPPM(),
                   uavcan_stm32::clock::getUtcAjdustmentJumpCount());
     }