From cb39ef3299208fc22711dceb5c4287e81e762556 Mon Sep 17 00:00:00 2001 From: Philippe Leduc Date: Wed, 15 Apr 2026 09:31:53 +0200 Subject: [PATCH 1/4] Sync master time on network time --- examples/master/marvin/marvin.cc | 1 + lib/include/kickcat/OS/Timer.h | 11 +++++++++++ lib/master/include/kickcat/Bus.h | 13 ++++++++++--- lib/master/src/dc.cc | 27 ++++++++++++++++++++++----- lib/src/OS/Unix/Timer.cc | 6 ++++++ lib/src/OS/Windows/Timer.cc | 6 ++++++ 6 files changed, 56 insertions(+), 8 deletions(-) diff --git a/examples/master/marvin/marvin.cc b/examples/master/marvin/marvin.cc index c08e397d..963fc904 100644 --- a/examples/master/marvin/marvin.cc +++ b/examples/master/marvin/marvin.cc @@ -301,6 +301,7 @@ int main(int argc, char *argv[]) bus.isDCSynchronized(1000ns); } + timer.apply_offset(bus.dcMasterOffset()); timer.wait_next_tick(); } diff --git a/lib/include/kickcat/OS/Timer.h b/lib/include/kickcat/OS/Timer.h index b081c442..62a0a9b8 100644 --- a/lib/include/kickcat/OS/Timer.h +++ b/lib/include/kickcat/OS/Timer.h @@ -42,12 +42,23 @@ namespace kickcat /// Wait until the next timer tick (blocking call) std::error_code wait_next_tick(); + /// \brief Adjust the timer phase to track an external time reference (e.g. EtherCAT DC). + /// \details Call this every cycle before wait_next_tick() with bus.dcMasterOffset(). + /// A positive offset means the master is ahead of the reference: the timer slows down. + /// Uses a first-order IIR filter to smooth the correction. + /// Pass 0ns when no reference is available (no-op when filtered offset is zero). + void apply_offset(nanoseconds raw_offset); + protected: private: + static constexpr int64_t OFFSET_FILTER_DEPTH = 256; + static constexpr int64_t OFFSET_CORRECTION_DIVISOR = 16; + std::string name_; nanoseconds period_; nanoseconds next_deadline_{}; nanoseconds last_wakeup_{}; + nanoseconds filtered_offset_{0ns}; Mutex mutex_{}; ConditionVariable stop_{}; diff --git a/lib/master/include/kickcat/Bus.h b/lib/master/include/kickcat/Bus.h index 00dd3b06..308bc638 100644 --- a/lib/master/include/kickcat/Bus.h +++ b/lib/master/include/kickcat/Bus.h @@ -114,9 +114,8 @@ namespace kickcat void processMessages(std::function 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. + /// \details Reads the DC reference clock's system time register (0x0910) 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 @@ -129,6 +128,12 @@ namespace kickcat /// \return true if all DC slaves are synchronized within the given threshold bool isDCSynchronized(nanoseconds threshold = 1000ns, bool log_all = false); + /// \brief Return the signed offset between the master OS clock and the DC network time. + /// \details Positive means the master clock is ahead of the network clock. + /// Updated every cycle by sendDriftCompensation(). + /// \return 0ns if DC is not active or not yet measured. + nanoseconds dcMasterOffset() const; + enum Access { @@ -238,6 +243,8 @@ 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 MailboxStatusFMMU mailbox_status_fmmu_{MailboxStatusFMMU::NONE}; }; diff --git a/lib/master/src/dc.cc b/lib/master/src/dc.cc index 805411f5..d39bec1e 100644 --- a/lib/master/src/dc.cc +++ b/lib/master/src/dc.cc @@ -472,20 +472,37 @@ namespace kickcat void Bus::sendDriftCompensation(std::function const& error) { - auto process = [](DatagramHeader const*, uint8_t const*, uint16_t wkc) + // FRMW reads the reference clock's system time and writes it to all subordinate clocks. + // The reference clock free-runs on its own quartz (offset was set during enableDC). + // Using only FRMW (no FPWR) avoids injecting master clock jitter and NTP corrections + // into the network - the slave PLLs track the stable ESC oscillator instead. + 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(); - 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); + 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_); } diff --git a/lib/src/OS/Unix/Timer.cc b/lib/src/OS/Unix/Timer.cc index b060f35d..b4daff74 100644 --- a/lib/src/OS/Unix/Timer.cc +++ b/lib/src/OS/Unix/Timer.cc @@ -56,6 +56,12 @@ namespace kickcat period_ = period; } + void Timer::apply_offset(nanoseconds raw_offset) + { + filtered_offset_ = (filtered_offset_ * (OFFSET_FILTER_DEPTH - 1) + raw_offset) / OFFSET_FILTER_DEPTH; + next_deadline_ -= filtered_offset_ / OFFSET_CORRECTION_DIVISOR; + } + std::error_code Timer::wait_next_tick() { { diff --git a/lib/src/OS/Windows/Timer.cc b/lib/src/OS/Windows/Timer.cc index cd84a6b5..91c128c7 100644 --- a/lib/src/OS/Windows/Timer.cc +++ b/lib/src/OS/Windows/Timer.cc @@ -55,6 +55,12 @@ namespace kickcat period_ = period; } + void Timer::apply_offset(nanoseconds raw_offset) + { + filtered_offset_ = (filtered_offset_ * (OFFSET_FILTER_DEPTH - 1) + raw_offset) / OFFSET_FILTER_DEPTH; + next_deadline_ -= filtered_offset_ / OFFSET_CORRECTION_DIVISOR; + } + std::error_code Timer::wait_next_tick() { { From 2298f0c9b0a0091a7df51e2f102803e4759bd1fb Mon Sep 17 00:00:00 2001 From: Philippe Leduc Date: Mon, 20 Apr 2026 13:15:30 +0200 Subject: [PATCH 2/4] Fix sync offset: it shall works on the delta, not the absolute drift that will always diverge. Refactor timer class. --- examples/master/marvin/marvin.cc | 2 +- lib/CMakeLists.txt | 1 + lib/include/kickcat/OS/Timer.h | 38 +++++++------------ lib/src/OS/Timer.cc | 47 ++++++++++++++++++++++++ lib/src/OS/Unix/Timer.cc | 63 -------------------------------- lib/src/OS/Windows/Timer.cc | 61 ------------------------------- 6 files changed, 63 insertions(+), 149 deletions(-) create mode 100644 lib/src/OS/Timer.cc diff --git a/examples/master/marvin/marvin.cc b/examples/master/marvin/marvin.cc index 963fc904..6097df63 100644 --- a/examples/master/marvin/marvin.cc +++ b/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) { diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt index bab26eee..0e4f62aa 100644 --- a/lib/CMakeLists.txt +++ b/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 diff --git a/lib/include/kickcat/OS/Timer.h b/lib/include/kickcat/OS/Timer.h index 62a0a9b8..2be5fab3 100644 --- a/lib/include/kickcat/OS/Timer.h +++ b/lib/include/kickcat/OS/Timer.h @@ -1,12 +1,9 @@ #ifndef KICKCAT_OS_TIMER_H #define KICKCAT_OS_TIMER_H -#include #include #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,27 +37,22 @@ namespace kickcat /// Wait until the next timer tick (blocking call) std::error_code wait_next_tick(); - /// \brief Adjust the timer phase to track an external time reference (e.g. EtherCAT DC). + /// \brief Match the timer's firing rate to an external time reference (e.g. EtherCAT DC). /// \details Call this every cycle before wait_next_tick() with bus.dcMasterOffset(). - /// A positive offset means the master is ahead of the reference: the timer slows down. - /// Uses a first-order IIR filter to smooth the correction. - /// Pass 0ns when no reference is available (no-op when filtered offset is zero). + /// The measured offset grows linearly at the clock drift rate: the timer + /// tracks the per-cycle delta and uses it as a period correction, so the + /// timer fires at the same real-time rate as the reference clock. + /// Do not call this when no drift compensation is desired. void apply_offset(nanoseconds raw_offset); - protected: private: - static constexpr int64_t OFFSET_FILTER_DEPTH = 256; - static constexpr int64_t OFFSET_CORRECTION_DIVISOR = 16; + static constexpr int64_t DRIFT_FILTER_DEPTH = 256; - std::string name_; nanoseconds period_; nanoseconds next_deadline_{}; nanoseconds last_wakeup_{}; - nanoseconds filtered_offset_{0ns}; - - Mutex mutex_{}; - ConditionVariable stop_{}; - bool is_stopped_{true}; + nanoseconds last_raw_offset_{0ns}; + nanoseconds filtered_drift_{0ns}; }; } diff --git a/lib/src/OS/Timer.cc b/lib/src/OS/Timer.cc new file mode 100644 index 00000000..83ffc821 --- /dev/null +++ b/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; + } +} diff --git a/lib/src/OS/Unix/Timer.cc b/lib/src/OS/Unix/Timer.cc index b4daff74..433f1157 100644 --- a/lib/src/OS/Unix/Timer.cc +++ b/lib/src/OS/Unix/Timer.cc @@ -1,4 +1,3 @@ -#include #include #include @@ -8,69 +7,8 @@ namespace kickcat { - Timer::Timer(nanoseconds period) - : period_{period} - { - } - - nanoseconds Timer::period() const - { - return period_; - } - - void Timer::start(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_; - - { - LockGuard lock(mutex_); - is_stopped_ = false; - } - stop_.signal(); - } - - void Timer::stop() - { - { - LockGuard lock(mutex_); - is_stopped_ = true; - } - stop_.signal(); - } - - bool Timer::is_stopped() const - { - return is_stopped_; - } - - void Timer::update_period(nanoseconds period) - { - period_ = period; - } - - void Timer::apply_offset(nanoseconds raw_offset) - { - filtered_offset_ = (filtered_offset_ * (OFFSET_FILTER_DEPTH - 1) + raw_offset) / OFFSET_FILTER_DEPTH; - next_deadline_ -= filtered_offset_ / OFFSET_CORRECTION_DIVISOR; - } - std::error_code Timer::wait_next_tick() { - { - LockGuard lock(mutex_); - stop_.wait(mutex_, [&]() - { return not is_stopped_; }); - } - - // Wait to the next working time. timespec const deadline = to_timespec(next_deadline_); int rc = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &deadline, NULL); if (rc != 0) @@ -101,7 +39,6 @@ namespace kickcat next_deadline_ += period_; } - return ret; } } diff --git a/lib/src/OS/Windows/Timer.cc b/lib/src/OS/Windows/Timer.cc index 91c128c7..f89990a1 100644 --- a/lib/src/OS/Windows/Timer.cc +++ b/lib/src/OS/Windows/Timer.cc @@ -1,5 +1,4 @@ #include -#include #include #include "kickcat/Error.h" @@ -7,68 +6,8 @@ namespace kickcat { - Timer::Timer(nanoseconds period) - : period_{period} - { - } - - nanoseconds Timer::period() const - { - return period_; - } - - void Timer::start(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_; - - { - LockGuard lock(mutex_); - is_stopped_ = false; - } - stop_.signal(); - } - - void Timer::stop() - { - { - LockGuard lock(mutex_); - is_stopped_ = true; - } - stop_.signal(); - } - - bool Timer::is_stopped() const - { - return is_stopped_; - } - - void Timer::update_period(nanoseconds period) - { - period_ = period; - } - - void Timer::apply_offset(nanoseconds raw_offset) - { - filtered_offset_ = (filtered_offset_ * (OFFSET_FILTER_DEPTH - 1) + raw_offset) / OFFSET_FILTER_DEPTH; - next_deadline_ -= filtered_offset_ / OFFSET_CORRECTION_DIVISOR; - } - std::error_code Timer::wait_next_tick() { - { - LockGuard lock(mutex_); - stop_.wait(mutex_, [&]() - { return not is_stopped_; }); - } - nanoseconds now = since_epoch(); if (next_deadline_ > now) { From da406b8a0f687e57514d3e90270d34bf45cbb7c6 Mon Sep 17 00:00:00 2001 From: Philippe Leduc Date: Tue, 21 Apr 2026 16:01:49 +0200 Subject: [PATCH 3/4] WIP --- lib/master/src/dc.cc | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/lib/master/src/dc.cc b/lib/master/src/dc.cc index d39bec1e..c2c78ab1 100644 --- a/lib/master/src/dc.cc +++ b/lib/master/src/dc.cc @@ -472,10 +472,10 @@ namespace kickcat void Bus::sendDriftCompensation(std::function const& error) { - // FRMW reads the reference clock's system time and writes it to all subordinate clocks. - // The reference clock free-runs on its own quartz (offset was set during enableDC). - // Using only FRMW (no FPWR) avoids injecting master clock jitter and NTP corrections - // into the network - the slave PLLs track the stable ESC oscillator instead. + // 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) @@ -492,7 +492,10 @@ namespace kickcat return DatagramState::OK; }; - link_->addDatagram(Command::FRMW, createAddress(dc_slave_->address, reg::DC_SYSTEM_TIME), nullptr, sizeof(uint64_t), process, error); + nanoseconds now = since_ecat_epoch(); + 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); } From 87189b250bb0e0b4a569ade5350cf1b51d609735 Mon Sep 17 00:00:00 2001 From: Philippe Leduc Date: Thu, 23 Apr 2026 10:28:03 +0200 Subject: [PATCH 4/4] Switch to clock monotonic to inject master time in the network. Add a function to monitor the DC of the network --- lib/include/kickcat/OS/Time.h | 4 ++ lib/include/kickcat/OS/Timer.h | 13 ++-- lib/master/include/kickcat/Bus.h | 20 +++--- lib/master/src/dc.cc | 115 ++++++++++++++++++++++++++++++- lib/src/OS/Time.cc | 6 ++ 5 files changed, 142 insertions(+), 16 deletions(-) diff --git a/lib/include/kickcat/OS/Time.h b/lib/include/kickcat/OS/Time.h index ef6ec207..fa6b3142 100644 --- a/lib/include/kickcat/OS/Time.h +++ b/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) { diff --git a/lib/include/kickcat/OS/Timer.h b/lib/include/kickcat/OS/Timer.h index 2be5fab3..f0764c5c 100644 --- a/lib/include/kickcat/OS/Timer.h +++ b/lib/include/kickcat/OS/Timer.h @@ -37,14 +37,15 @@ namespace kickcat /// Wait until the next timer tick (blocking call) std::error_code wait_next_tick(); - /// \brief Match the timer's firing rate to an external time reference (e.g. EtherCAT DC). - /// \details Call this every cycle before wait_next_tick() with bus.dcMasterOffset(). - /// The measured offset grows linearly at the clock drift rate: the timer - /// tracks the per-cycle delta and uses it as a period correction, so the - /// timer fires at the same real-time rate as the reference clock. - /// Do not call this when no drift compensation is desired. + /// \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: static constexpr int64_t DRIFT_FILTER_DEPTH = 256; diff --git a/lib/master/include/kickcat/Bus.h b/lib/master/include/kickcat/Bus.h index 308bc638..83d72cf2 100644 --- a/lib/master/include/kickcat/Bus.h +++ b/lib/master/include/kickcat/Bus.h @@ -113,12 +113,10 @@ namespace kickcat void checkMailboxes( std::function const& error); void processMessages(std::function const& error); - /// \brief Send drift compensation datagrams to maintain DC synchronization - /// \details Reads the DC reference clock's system time register (0x0910) 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 const& error); /// \brief Check if distributed clocks are synchronized @@ -128,12 +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 Return the signed offset between the master OS clock and the DC network time. - /// \details Positive means the master clock is ahead of the network clock. - /// Updated every cycle by sendDriftCompensation(). + /// \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 { @@ -245,6 +246,7 @@ namespace kickcat 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}; }; diff --git a/lib/master/src/dc.cc b/lib/master/src/dc.cc index c2c78ab1..3ef33356 100644 --- a/lib/master/src/dc.cc +++ b/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); @@ -492,7 +498,10 @@ namespace kickcat 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); @@ -572,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 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(-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(status.speed_diff_raw)); + } + } + } } diff --git a/lib/src/OS/Time.cc b/lib/src/OS/Time.cc index 45c7e83c..160ba670 100644 --- a/lib/src/OS/Time.cc +++ b/lib/src/OS/Time.cc @@ -23,4 +23,10 @@ namespace kickcat { return since_epoch() - start_time; } + + nanoseconds monotonic_time() + { + auto now = time_point_cast(steady_clock::now()); + return now.time_since_epoch(); + } }