From e2fa613917fca0b7e94846e0952bdd6e2056b471 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Tue, 1 Apr 2014 17:35:32 +0400
Subject: [PATCH] Added slave time sync suppression, which allows to use slave
 and master on the same node. Shall be documented later.

---
 .../protocol/global_time_sync_slave.hpp       |  5 +++
 .../src/protocol/global_time_sync_slave.cpp   |  5 ++-
 .../test/protocol/global_time_sync_slave.cpp  | 36 +++++++++++++++++++
 3 files changed, 45 insertions(+), 1 deletion(-)

diff --git a/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp
index 7ffef83960..f6a94edabb 100644
--- a/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp
+++ b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp
@@ -30,6 +30,7 @@ class GlobalTimeSyncSlave : Noncopyable
     NodeID master_nid_;
     TransferID prev_tid_;
     uint8_t prev_iface_index_;
+    bool suppressed_;
 
     ISystemClock& getSystemClock() const { return sub_.getNode().getSystemClock(); }
 
@@ -46,10 +47,14 @@ public:
         : sub_(node)
         , state_(Update)
         , prev_iface_index_(0xFF)
+        , suppressed_(false)
     { }
 
     int start();
 
+    void suppress(bool suppressed) { suppressed_ = suppressed; }
+    bool isSuppressed() const { return suppressed_; }
+
     bool isActive() const;
 
     NodeID getMasterNodeID() const;
diff --git a/libuavcan/src/protocol/global_time_sync_slave.cpp b/libuavcan/src/protocol/global_time_sync_slave.cpp
index 08671eaeb9..f6c2f477e5 100644
--- a/libuavcan/src/protocol/global_time_sync_slave.cpp
+++ b/libuavcan/src/protocol/global_time_sync_slave.cpp
@@ -18,7 +18,10 @@ void GlobalTimeSyncSlave::adjustFromMsg(const ReceivedDataStructure<protocol::Gl
                  static_cast<long long>(adjustment.toUSec()),
                  int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
 
-    getSystemClock().adjustUtc(adjustment);
+    if (!suppressed_)
+    {
+        getSystemClock().adjustUtc(adjustment);
+    }
     last_adjustment_ts_ = msg.getMonotonicTimestamp();
     state_ = Update;
 }
diff --git a/libuavcan/test/protocol/global_time_sync_slave.cpp b/libuavcan/test/protocol/global_time_sync_slave.cpp
index 3d9cbf8322..e84f1ef338 100644
--- a/libuavcan/test/protocol/global_time_sync_slave.cpp
+++ b/libuavcan/test/protocol/global_time_sync_slave.cpp
@@ -274,3 +274,39 @@ TEST(GlobalTimeSyncSlave, Validation)
     ASSERT_EQ(5000, slave_clock.utc);
     std::cout << slave_clock.monotonic << std::endl;
 }
+
+
+TEST(GlobalTimeSyncSlave, Suppression)
+{
+    SystemClockMock slave_clock;
+    slave_clock.monotonic = 1000000;
+    slave_clock.preserve_utc = true;
+
+    CanDriverMock slave_can(3, slave_clock);
+    for (int i = 0; i < slave_can.getNumIfaces(); i++)
+    {
+        slave_can.ifaces.at(i).enable_utc_timestamping = true;
+    }
+
+    TestNode node(slave_can, slave_clock, 64);
+
+    uavcan::GlobalTimeSyncSlave gtss(node);
+    uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub(node);
+
+    ASSERT_LE(0, gtss.start());
+    ASSERT_EQ(0, slave_clock.utc);
+
+    gtss.suppress(true);
+
+    broadcastSyncMsg(slave_can.ifaces.at(0), 0, 8, 0);    // Locked on this
+    broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 0); // Ignored
+    ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+
+    broadcastSyncMsg(slave_can.ifaces.at(0), 1000, 8, 1); // Adjust 1000 ahead
+    broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 1); // Ignored
+    ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
+
+    ASSERT_TRUE(gtss.isActive());
+    ASSERT_EQ(8, gtss.getMasterNodeID().get());
+    ASSERT_EQ(0, slave_clock.utc);                  // The clock shall not be asjusted
+}