From d8c37584c07f088ec3b44cd0474cd0c6582db4c7 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Sat, 12 Apr 2014 21:21:02 +0400
Subject: [PATCH] STM32: clock sync: Simple PI controller for speed
 adjustments; converges to +-100 usec in few minutes

---
 .../driver/include/uavcan_stm32/clock.hpp     |  7 +++
 .../stm32/driver/src/uc_stm32_clock.cpp       | 49 +++++++------------
 .../stm32/test_stm32f107/src/main.cpp         |  3 +-
 3 files changed, 28 insertions(+), 31 deletions(-)

diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp
index ebe934ff6d..6835231b59 100644
--- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp
+++ b/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();
+
 }
 
 /**
diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
index 3be68d8fc3..d5e7481eda 100644
--- a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
+++ b/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()
diff --git a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp b/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp
index efb66e3de0..a4b8329a6f 100644
--- a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp
+++ b/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());
     }