From 438340ae9851948e36d535bbee639eb3b036d061 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Mon, 24 Mar 2014 10:53:41 +0400
Subject: [PATCH] GlobalTimeSyncSlave: methods isActive(), getMasterNodeID()

---
 .../protocol/global_time_sync_slave.hpp       |  6 ++++
 .../src/protocol/global_time_sync_slave.cpp   | 12 +++++++
 .../test/protocol/global_time_sync_slave.cpp  | 31 +++++++++++++++++++
 3 files changed, 49 insertions(+)

diff --git a/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp
index ba51b2b795..04a84877ca 100644
--- a/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp
+++ b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp
@@ -21,11 +21,13 @@ class GlobalTimeSyncSlave : Noncopyable
 
     UtcTime prev_ts_utc_;
     MonotonicTime prev_ts_mono_;
+    MonotonicTime last_adjustment_ts_;
     enum State { Update, Adjust } state;
     NodeID master_nid_;
     uint8_t prev_iface_index_;
 
     ISystemClock& getSystemClock() { return sub_.getNode().getSystemClock(); }
+    const ISystemClock& getSystemClock() const { return sub_.getNode().getSystemClock(); }
 
     void adjustFromMsg(const ReceivedDataStructure<protocol::GlobalTimeSync>& msg);
 
@@ -43,6 +45,10 @@ public:
     { }
 
     int start();
+
+    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 639680fef3..5badb1348c 100644
--- a/libuavcan/src/protocol/global_time_sync_slave.cpp
+++ b/libuavcan/src/protocol/global_time_sync_slave.cpp
@@ -19,6 +19,7 @@ void GlobalTimeSyncSlave::adjustFromMsg(const ReceivedDataStructure<protocol::Gl
                  int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()));
 
     getSystemClock().adjustUtc(adjustment);
+    last_adjustment_ts_ = msg.getMonotonicTimestamp();
     state = Update;
 }
 
@@ -84,4 +85,15 @@ int GlobalTimeSyncSlave::start()
     return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync));
 }
 
+bool GlobalTimeSyncSlave::isActive() const
+{
+    const MonotonicDuration since_prev_adj = getSystemClock().getMonotonic() - last_adjustment_ts_;
+    return !last_adjustment_ts_.isZero() && since_prev_adj.toMSec() <= protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS;
+}
+
+NodeID GlobalTimeSyncSlave::getMasterNodeID() const
+{
+    return isActive() ? master_nid_ : NodeID();
+}
+
 }
diff --git a/libuavcan/test/protocol/global_time_sync_slave.cpp b/libuavcan/test/protocol/global_time_sync_slave.cpp
index 340b772c4f..69ec4fc7b5 100644
--- a/libuavcan/test/protocol/global_time_sync_slave.cpp
+++ b/libuavcan/test/protocol/global_time_sync_slave.cpp
@@ -29,6 +29,8 @@ TEST(GlobalTimeSyncSlave, Basic)
     uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub(nodes.b);
 
     ASSERT_LE(0, gtss.start());
+    ASSERT_FALSE(gtss.isActive());
+    ASSERT_FALSE(gtss.getMasterNodeID().isValid());
 
     /*
      * Empty broadcast
@@ -44,6 +46,9 @@ TEST(GlobalTimeSyncSlave, Basic)
     std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl;
     std::cout << "Slave  mono=" << slave_clock.monotonic  << " utc=" << slave_clock.utc << std::endl;
 
+    ASSERT_FALSE(gtss.isActive());
+    ASSERT_FALSE(gtss.getMasterNodeID().isValid());
+
     /*
      * Follow-up broadcast with proper time
      * Slave must adjust now
@@ -59,6 +64,9 @@ TEST(GlobalTimeSyncSlave, Basic)
     master_clock.utc += 1000000;
     slave_clock.utc += 1000000;
 
+    ASSERT_TRUE(gtss.isActive());
+    ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
+
     /*
      * Next follow-up, slave is synchronized now
      * Will update
@@ -72,6 +80,9 @@ TEST(GlobalTimeSyncSlave, Basic)
     master_clock.utc += 1000000;
     slave_clock.utc += 1000000;
 
+    ASSERT_TRUE(gtss.isActive());
+    ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
+
     /*
      * Next follow-up, slave is synchronized now
      * Will adjust
@@ -87,6 +98,9 @@ TEST(GlobalTimeSyncSlave, Basic)
     ASSERT_EQ(4000000, slave_clock.utc);
     ASSERT_EQ(4000000, master_clock.utc);
 
+    ASSERT_TRUE(gtss.isActive());
+    ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
+
     /*
      * Another master
      * This one has higher priority, so it will be preferred
@@ -112,6 +126,9 @@ TEST(GlobalTimeSyncSlave, Basic)
 
     master2_clock.utc += 1000000;
 
+    ASSERT_TRUE(gtss.isActive());
+    ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
+
     /*
      * Adjustment
      */
@@ -119,6 +136,9 @@ TEST(GlobalTimeSyncSlave, Basic)
     nodes.spinBoth(uavcan::MonotonicDuration());
     ASSERT_EQ(100, slave_clock.utc);
 
+    ASSERT_TRUE(gtss.isActive());
+    ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
+
     /*
      * Another master will be ignored now
      */
@@ -131,4 +151,15 @@ TEST(GlobalTimeSyncSlave, Basic)
     gts_pub.broadcast(gts);
     nodes.spinBoth(uavcan::MonotonicDuration());
     ASSERT_EQ(100, slave_clock.utc);
+
+    ASSERT_TRUE(gtss.isActive());
+    ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
+
+    /*
+     * Timeout
+     */
+    slave_clock.advance(100000000);
+
+    ASSERT_FALSE(gtss.isActive());
+    ASSERT_FALSE(gtss.getMasterNodeID().isValid());
 }