Sync master time on network time

examples/master/marvin/marvin.cc

@@ -167,7 +167,7 @@ int main(int argc, char *argv[])
     };
 
     Timer timer{1ms};
-    timer.start(sync_point);
+    timer.init(sync_point);
 
     for (int i = 0; i < 10; ++i)
     {
@@ -301,6 +301,7 @@ int main(int argc, char *argv[])
             bus.isDCSynchronized(1000ns);
         }
 
+        timer.apply_offset(bus.dcMasterOffset());
         timer.wait_next_tick();
     }
 

lib/CMakeLists.txt

@@ -8,6 +8,7 @@ set(KICKCAT_SOURCES
   ${CMAKE_CURRENT_SOURCE_DIR}/src/SIIParser.cc
 
   ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Time.cc
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/OS/Timer.cc
 
   ${CMAKE_CURRENT_SOURCE_DIR}/src/CoE/OD.cc
   ${CMAKE_CURRENT_SOURCE_DIR}/src/CoE/protocol.cc

lib/include/kickcat/OS/Time.h

@@ -20,6 +20,10 @@ namespace kickcat
     // return the time since since another point in time
     nanoseconds elapsed_time(nanoseconds start = since_epoch());
 
+    // return a monotonic (never going backward) time in ns.
+    // The epoch is implementation-defined; use only for duration / delta computations.
+    nanoseconds monotonic_time();
+
     // Convert an std::chrono duration to a POSIX timespec
     constexpr timespec to_timespec(nanoseconds time)
     {

lib/include/kickcat/OS/Timer.h

@@ -1,12 +1,9 @@
 #ifndef KICKCAT_OS_TIMER_H
 #define KICKCAT_OS_TIMER_H
 
-#include <string_view>
 #include <system_error>
 
 #include "kickcat/OS/Time.h"
-#include "kickcat/OS/ConditionVariable.h"
-#include "kickcat/OS/Mutex.h"
 
 namespace kickcat
 {
@@ -17,19 +14,17 @@ namespace kickcat
         Timer(Timer const &) = delete;
         void operator=(Timer const &) = delete;
 
-        /// \param  name     Name of the timer (logging)
         /// \param  period   Interval between two timer firing
         Timer(nanoseconds period);
         ~Timer() = default;
 
-        /// Start the timer.
-        void start(nanoseconds sync_point = since_epoch());
+        /// \brief  Initialize (or restart) the timer.
+        /// \details Aligns the next deadline to sync_point + N*period so that it lies in the future.
+        ///          Resets the drift compensation state. Safe to call multiple times.
+        void init(nanoseconds sync_point = since_epoch());
 
-        ///  Stop the timer.
-        void stop();
-
-        ///  Get timer status
-        bool is_stopped() const;
+        /// \deprecated Use init() instead.
+        void start(nanoseconds sync_point = since_epoch()) { init(sync_point); }
 
         ///  Change the timer values to the new provided values and reset it.
         ///  This does not take effect before the timer is restarted.
@@ -42,16 +37,23 @@ namespace kickcat
         /// Wait until the next timer tick (blocking call)
         std::error_code wait_next_tick();
 
-    protected:
+        /// \brief  Phase-lock the timer to an external time reference (e.g. EtherCAT DC).
+        /// \details Pass the master-vs-reference offset every cycle before wait_next_tick().
+        ///          Do not call when no drift compensation is desired.
+        void apply_offset(nanoseconds raw_offset);
+
+        /// \return filtered per-cycle drift estimate applied by apply_offset(). 0ns if apply_offset()
+        ///         has never been called; typically a few tens of ns at steady state.
+        nanoseconds filtered_drift() const { return filtered_drift_; }
+
     private:
-        std::string name_;
+        static constexpr int64_t DRIFT_FILTER_DEPTH = 256;
+
         nanoseconds period_;
         nanoseconds next_deadline_{};
         nanoseconds last_wakeup_{};
-
-        Mutex mutex_{};
-        ConditionVariable stop_{};
-        bool is_stopped_{true};
+        nanoseconds last_raw_offset_{0ns};
+        nanoseconds filtered_drift_{0ns};
     };
 }
 

lib/master/include/kickcat/Bus.h

@@ -113,13 +113,10 @@ namespace kickcat
         void checkMailboxes( std::function<void(DatagramState const&)> const& error);
         void processMessages(std::function<void(DatagramState const&)> const& error);
 
-        /// \brief  Send drift compensation datagrams to maintain DC synchronization
-        /// \details Writes the current master time to the DC reference clock slave's system time register (0x0910)
-        ///          using FPWR, then reads it back with FRMW so that each slave on the segment updates its
-        ///          local clock offset accordingly.
-        ///          Called cyclically during process data exchange, and repeatedly (15 000 times) during
-        ///          static drift compensation at DC initialization.
-        /// \param  error  Callback invoked when a datagram error occurs
+        /// \brief  Send drift compensation datagrams to maintain DC synchronization.
+        /// \details Called cyclically during process data exchange. enableDC() also calls it
+        ///          repeatedly during static drift compensation.
+        /// \param  error  Callback invoked when a datagram error occurs.
         void sendDriftCompensation(std::function<void(DatagramState const&)> const& error);
 
         /// \brief  Check if distributed clocks are synchronized
@@ -129,6 +126,15 @@ namespace kickcat
         /// \return true if all DC slaves are synchronized within the given threshold
         bool isDCSynchronized(nanoseconds threshold = 1000ns, bool log_all = false);
 
+        /// \brief  Signed offset between the master OS clock and the DC network time.
+        ///         Positive means the master clock is ahead of the network clock.
+        /// \return 0ns if DC is not active or not yet measured.
+        nanoseconds dcMasterOffset() const;
+
+        /// \brief  Log DC synchronization state of each DC slave.
+        /// \details Intended to be called periodically during long runs to monitor PLL health.
+        void logDCStatus(bool include_per_slave = false);
+
 
         enum Access
         {
@@ -238,6 +244,9 @@ namespace kickcat
         uint16_t irq_mask_{0};
 
         Slave* dc_slave_{nullptr};
+        nanoseconds dc_network_time_{0ns};  // last reference clock system time (EtherCAT epoch)
+        nanoseconds dc_master_time_{0ns};   // master OS time when FRMW response was captured
+        nanoseconds dc_epoch_offset_{0ns};  // since_ecat_epoch() - monotonic_time() at enableDC
 
         MailboxStatusFMMU mailbox_status_fmmu_{MailboxStatusFMMU::NONE};
     };

lib/master/src/dc.cc

@@ -25,6 +25,12 @@ namespace kickcat
         dc_info("DC reference slave is %d\n", dc_slave_->address);
 
         //-------- Apply propagation delay and system time offset -------//
+        // Capture the offset between CLOCK_REALTIME (used to calibrate slave time offsets
+        // via applyMasterTime) and the monotonic clock (used to write the cyclic FPWR).
+        // This lets us write a stable, NTP-immune value while keeping the calibrated slave
+        // offsets consistent with what the PLL will see.
+        dc_epoch_offset_ = since_ecat_epoch() - monotonic_time();
+
         // Trigger latch time on received registers whenever the frame pass by the port 0-3
         uint8_t dummy = 0;
         broadcastWrite(reg::DC_RECEIVED_TIME, &dummy, 1);
@@ -472,23 +478,46 @@ namespace kickcat
 
     void Bus::sendDriftCompensation(std::function<void(DatagramState const&)> const& error)
     {
-        auto process = [](DatagramHeader const*, uint8_t const*, uint16_t wkc)
+        // FPWR writes the master time to the reference clock's system time, then FRMW reads
+        // the reference clock's system time and writes it to all subordinate clocks.
+        // Per ETG1000.4 6.8.3: writes to 0x0910 feed the ESC's PLL. Some ESCs drop out of DC
+        // state (AL_STATUS_CODE 0x0032 PLL Error) if this write is omitted.
+        auto process = [this](DatagramHeader const*, uint8_t const* data, uint16_t wkc)
         {
             if (wkc == 0)
             {
                 dc_error("Invalid working counter:  %" PRIu16 "\n", wkc);
                 return DatagramState::INVALID_WKC;
             }
+
+            uint64_t raw_network_time = 0;
+            std::memcpy(&raw_network_time, data, sizeof(raw_network_time));
+            dc_network_time_ = nanoseconds(raw_network_time);
+            dc_master_time_  = since_epoch();
+
             return DatagramState::OK;
         };
 
-        nanoseconds now = since_ecat_epoch();
+        // Use monotonic time shifted by dc_epoch_offset_ so the value matches the ECAT-epoch
+        // domain used by applyMasterTime (keeping PLL comparisons near zero) while being
+        // immune to NTP offset adjustments that affect CLOCK_REALTIME.
+        nanoseconds now = monotonic_time() + dc_epoch_offset_;
         uint64_t raw_now = now.count();
         link_->addDatagram(Command::FPWR, createAddress(dc_slave_->address, reg::DC_SYSTEM_TIME), &raw_now, sizeof(uint64_t), process, error);
         link_->addDatagram(Command::FRMW, createAddress(dc_slave_->address, reg::DC_SYSTEM_TIME), nullptr,  sizeof(uint64_t), process, error);
     }
 
 
+    nanoseconds Bus::dcMasterOffset() const
+    {
+        if (dc_slave_ == nullptr or dc_master_time_ == 0ns)
+        {
+            return 0ns;
+        }
+        return dc_master_time_ - to_unix_epoch(dc_network_time_);
+    }
+
+
     bool Bus::isDCSynchronized(nanoseconds threshold, bool log_all)
     {
         if (dc_slave_ == nullptr)
@@ -552,4 +581,108 @@ namespace kickcat
 
         return synchronized;
     }
+
+
+    void Bus::logDCStatus(bool include_per_slave)
+    {
+        if (dc_slave_ == nullptr)
+        {
+            return;
+        }
+
+        struct DCSlaveStatus
+        {
+            Slave*   slave;
+            uint32_t time_diff_raw{0};
+            int16_t  speed_diff_raw{0};
+        };
+        std::vector<DCSlaveStatus> dc_status;
+
+        auto error = [](DatagramState const& state)
+        {
+            THROW_ERROR_DATAGRAM("Error while reading DC status", state);
+        };
+
+        for (auto& slave : slaves_)
+        {
+            if (not slave.isDCSupport() or &slave == dc_slave_)
+            {
+                continue;
+            }
+
+            dc_status.push_back({&slave, 0, 0});
+            auto& status = dc_status.back();
+
+            auto process_diff = [&status](DatagramHeader const*, uint8_t const* data, uint16_t wkc)
+            {
+                if (wkc != 1)
+                {
+                    return DatagramState::INVALID_WKC;
+                }
+                std::memcpy(&status.time_diff_raw, data, sizeof(status.time_diff_raw));
+                return DatagramState::OK;
+            };
+
+            auto process_speed = [&status](DatagramHeader const*, uint8_t const* data, uint16_t wkc)
+            {
+                if (wkc != 1)
+                {
+                    return DatagramState::INVALID_WKC;
+                }
+                std::memcpy(&status.speed_diff_raw, data, sizeof(status.speed_diff_raw));
+                return DatagramState::OK;
+            };
+
+            link_->addDatagram(Command::FPRD, createAddress(slave.address, reg::DC_SYSTEM_TIME_DIFF), nullptr, sizeof(status.time_diff_raw),  process_diff,  error);
+            link_->addDatagram(Command::FPRD, createAddress(slave.address, reg::DC_SPEED_CNT_DIFF),   nullptr, sizeof(status.speed_diff_raw), process_speed, error);
+        }
+        link_->processDatagrams();
+
+        nanoseconds max_drift = 0ns;
+        int16_t max_speed_diff_abs = 0;
+        int synchronized_count = 0;
+        for (auto const& status : dc_status)
+        {
+            // DC_SYSTEM_TIME_DIFF uses sign-magnitude encoding (bit 31 = sign, bits 30:0 = magnitude)
+            nanoseconds drift = nanoseconds(status.time_diff_raw & 0x7FFFFFFF);
+            if (drift > max_drift)
+            {
+                max_drift = drift;
+            }
+
+            int16_t speed_abs = status.speed_diff_raw;
+            if (speed_abs < 0)
+            {
+                speed_abs = static_cast<int16_t>(-speed_abs);
+            }
+            if (speed_abs > max_speed_diff_abs)
+            {
+                max_speed_diff_abs = speed_abs;
+            }
+
+            if (drift <= 1000ns)
+            {
+                ++synchronized_count;
+            }
+        }
+
+        dc_info("[DC] master_offset=%ld ns  synchronized=%d/%zu  max_drift=%ld ns  max_speed_diff=%d\n",
+                dcMasterOffset().count(),
+                synchronized_count, dc_status.size(),
+                max_drift.count(), max_speed_diff_abs);
+
+        if (include_per_slave)
+        {
+            for (auto const& status : dc_status)
+            {
+                nanoseconds drift = nanoseconds(status.time_diff_raw & 0x7FFFFFFF);
+                bool negative = (status.time_diff_raw & 0x80000000) != 0;
+                dc_info("[DC]   slave %d: time_diff=%s%ld ns  speed_diff=%d\n",
+                        status.slave->address,
+                        negative ? "-" : "",
+                        drift.count(),
+                        static_cast<int>(status.speed_diff_raw));
+            }
+        }
+    }
 }

lib/src/OS/Time.cc

@@ -23,4 +23,10 @@ namespace kickcat
     {
         return since_epoch() - start_time;
     }
+
+    nanoseconds monotonic_time()
+    {
+        auto now = time_point_cast<nanoseconds>(steady_clock::now());
+        return now.time_since_epoch();
+    }
 }

lib/src/OS/Timer.cc

@@ -0,0 +1,47 @@
+// \brief OS agnostic Timer API - shared logic
+#include "kickcat/OS/Timer.h"
+
+namespace kickcat
+{
+    Timer::Timer(nanoseconds period)
+        : period_{period}
+    {
+    }
+
+    nanoseconds Timer::period() const
+    {
+        return period_;
+    }
+
+    void Timer::init(nanoseconds sync_point)
+    {
+        nanoseconds now = since_epoch();
+        nanoseconds delta = now - sync_point;
+
+        int64_t periods_to_skip = 0;
+        if (delta >= 0ns)
+        {
+            periods_to_skip = (delta / period_) + 1;
+        }
+        next_deadline_ = sync_point + periods_to_skip * period_;
+
+        last_raw_offset_ = 0ns;
+        filtered_drift_  = 0ns;
+    }
+
+    void Timer::update_period(nanoseconds period)
+    {
+        period_ = period;
+    }
+
+    void Timer::apply_offset(nanoseconds raw_offset)
+    {
+        if (last_raw_offset_ != 0ns)
+        {
+            nanoseconds delta = raw_offset - last_raw_offset_;
+            filtered_drift_ = (filtered_drift_ * (DRIFT_FILTER_DEPTH - 1) + delta) / DRIFT_FILTER_DEPTH;
+            next_deadline_ += filtered_drift_;
+        }
+        last_raw_offset_ = raw_offset;
+    }
+}