From c17a2bbd5bcb3de45abefe4e813c2c7890c7c6c2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 17 Apr 2014 16:14:39 +0400 Subject: [PATCH] Fixed Linux driver: SocketCan driver adds time offset from the provided clock instance, which fixes time synchronization in PerDriverPrivate clock adjustment mode --- .../linux/include/uavcan_linux/helpers.hpp | 6 +-- .../linux/include/uavcan_linux/socketcan.hpp | 17 +++++---- libuavcan_drivers/linux/test/test_socket.cpp | 37 +++++++++++++++---- 3 files changed, 42 insertions(+), 18 deletions(-) diff --git a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp index 11ebdab444..e9a0411226 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp @@ -31,12 +31,12 @@ class DefaultLogSink : public uavcan::ILogSink */ struct DriverPack { - SocketCanDriver can; SystemClock clock; + SocketCanDriver can; DriverPack(ClockAdjustmentMode clock_adjustment_mode) - : can() - , clock(clock_adjustment_mode) + : clock(clock_adjustment_mode) + , can(clock) { } }; diff --git a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp index dc8c8f2e56..24b68f7c00 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp @@ -116,7 +116,7 @@ class SocketCanIface : public uavcan::ICanIface { } }; - const SystemClock clock_; + const SystemClock& clock_; const int fd_; const unsigned max_frames_in_socket_tx_queue_; @@ -280,6 +280,7 @@ class SocketCanIface : public uavcan::ICanIface } if (accept) { + rx.ts_utc += clock_.getPrivateAdjustment(); rx_queue_.push(rx); } } @@ -298,8 +299,9 @@ public: /** * Takes ownership of socket's file descriptor. */ - explicit SocketCanIface(int socket_fd, int max_frames_in_socket_tx_queue = 3) - : fd_(socket_fd) + SocketCanIface(const SystemClock& clock, int socket_fd, int max_frames_in_socket_tx_queue = 3) + : clock_(clock) + , fd_(socket_fd) , max_frames_in_socket_tx_queue_(max_frames_in_socket_tx_queue) , frames_in_socket_tx_queue_(0) { @@ -487,14 +489,15 @@ public: static constexpr unsigned MaxIfaces = uavcan::MaxCanIfaces; private: - const SystemClock clock_; + const SystemClock& clock_; uavcan::LazyConstructor ifaces_[MaxIfaces]; ::pollfd pollfds_[MaxIfaces]; std::uint8_t num_ifaces_; public: - SocketCanDriver() - : num_ifaces_(0) + explicit SocketCanDriver(const SystemClock& clock) + : clock_(clock) + , num_ifaces_(0) { for (auto& p : pollfds_) { @@ -601,7 +604,7 @@ public: // Construct the iface - upon successful construction the iface will take ownership of the fd. try { - ifaces_[num_ifaces_].construct(fd); + ifaces_[num_ifaces_].construct(clock_, fd); } catch (...) { diff --git a/libuavcan_drivers/linux/test/test_socket.cpp b/libuavcan_drivers/linux/test/test_socket.cpp index c0af15b181..168a52a7d7 100644 --- a/libuavcan_drivers/linux/test/test_socket.cpp +++ b/libuavcan_drivers/linux/test/test_socket.cpp @@ -32,8 +32,16 @@ static void testSocketRxTx(const std::string& iface_name) const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name); ENFORCE(sock1 >= 0 && sock2 >= 0); - uavcan_linux::SocketCanIface if1(sock1); - uavcan_linux::SocketCanIface if2(sock2); + /* + * Clocks will have some offset from the true system time + * SocketCAN driver must handle this correctly + */ + uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate); + clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(100000)); + const uavcan_linux::SystemClock& clock = clock_impl; + + uavcan_linux::SocketCanIface if1(clock, sock1); + uavcan_linux::SocketCanIface if2(clock, sock2); /* * Sending two frames, one of which must be returned back @@ -69,8 +77,6 @@ static void testSocketRxTx(const std::string& iface_name) uavcan::UtcTime ts_utc; uavcan::CanIOFlags flags = 0; - const uavcan_linux::SystemClock clock(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate); - /* * Read first */ @@ -132,8 +138,16 @@ static void testSocketFilters(const std::string& iface_name) const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name); ENFORCE(sock1 >= 0 && sock2 >= 0); - uavcan_linux::SocketCanIface if1(sock1); - uavcan_linux::SocketCanIface if2(sock2); + /* + * Clocks will have some offset from the true system time + * SocketCAN driver must handle this correctly + */ + uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate); + clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(-1000)); + const uavcan_linux::SystemClock& clock = clock_impl; + + uavcan_linux::SocketCanIface if1(clock, sock1); + uavcan_linux::SocketCanIface if2(clock, sock2); /* * Configuring filters @@ -199,7 +213,15 @@ static void testSocketFilters(const std::string& iface_name) static void testDriver(const std::vector& iface_names) { - uavcan_linux::SocketCanDriver driver; + /* + * Clocks will have some offset from the true system time + * SocketCAN driver must handle this correctly + */ + uavcan_linux::SystemClock clock_impl(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate); + clock_impl.adjustUtc(uavcan::UtcDuration::fromMSec(9000000)); + const uavcan_linux::SystemClock& clock = clock_impl; + + uavcan_linux::SocketCanDriver driver(clock); for (auto ifn : iface_names) { std::cout << "Adding iface " << ifn << std::endl; @@ -254,7 +276,6 @@ static void testDriver(const std::vector& iface_names) /* * Receive loopback */ - const uavcan_linux::SystemClock clock(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate); for (int i = 0; i < driver.getNumIfaces(); i++) { uavcan::CanFrame frame;