GlobalTimeSyncMaster

2026-10-17 13:33:34 +08:00 · 2014-03-24 20:39:59 +04:00
parent 6b43e55dc6
commit 0b33dbe173
4 changed files with 359 additions and 3 deletions
@@ -0,0 +1,131 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/global_time_sync_master.hpp>
+#include <uavcan/protocol/global_time_sync_slave.hpp>
+#include "helpers.hpp"
+
+struct GlobalTimeSyncMasterTestNode
+{
+    SystemClockDriver clock;
+    PairableCanDriver can;
+    TestNode node;
+
+    GlobalTimeSyncMasterTestNode(uavcan::NodeID nid)
+    : can(clock)
+    , node(can, clock, nid)
+    { }
+};
+
+struct GlobalTimeSyncTestNetwork
+{
+    GlobalTimeSyncMasterTestNode slave;
+    GlobalTimeSyncMasterTestNode master_low;
+    GlobalTimeSyncMasterTestNode master_high;
+
+    GlobalTimeSyncTestNetwork()
+    : slave(64)
+    , master_low(120)
+    , master_high(8)
+    {
+        slave.can.other = &master_low.can;
+        master_low.can.other = &slave.can;
+        master_high.can.other = &slave.can;
+    }
+
+    void spinAll(uavcan::MonotonicDuration duration = uavcan::MonotonicDuration::fromMSec(3))
+    {
+        assert(!duration.isNegative());
+        unsigned int nspins3 = duration.toMSec() / 3;
+        nspins3 = nspins3 ? nspins3 : 2;
+        while (nspins3 --> 0)
+        {
+            ASSERT_LE(0, slave.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
+            ASSERT_LE(0, master_low.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
+            ASSERT_LE(0, master_high.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
+        }
+    }
+};
+
+TEST(GlobalTimeSyncMaster, Basic)
+{
+    GlobalTimeSyncTestNetwork nwk;
+
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalTimeSync> _reg1;
+
+    uavcan::GlobalTimeSyncSlave slave(nwk.slave.node);
+    uavcan::GlobalTimeSyncMaster master_low(nwk.master_low.node);
+    uavcan::GlobalTimeSyncMaster master_high(nwk.master_high.node);
+
+    ASSERT_FALSE(master_low.isInitialized());
+
+    ASSERT_LE(0, slave.start());
+    ASSERT_LE(0, master_low.init());
+    ASSERT_LE(0, master_high.init());
+
+    ASSERT_TRUE(master_low.isInitialized());
+    ASSERT_FALSE(slave.isActive());
+
+    /*
+     * Simple synchronization
+     */
+    ASSERT_LE(0, master_low.publish());  // Update
+    nwk.spinAll();
+
+    usleep(400000);
+    ASSERT_LE(0, master_low.publish());  // Adjustment
+    nwk.spinAll();
+
+    // Synchronization complete.
+    ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc()));
+    ASSERT_TRUE(slave.isActive());
+    ASSERT_EQ(nwk.master_low.node.getNodeID(), slave.getMasterNodeID());
+
+    /*
+     * Moving clocks forward and re-syncing with another master
+     */
+    static const uavcan::UtcDuration OneDay = uavcan::UtcDuration::fromMSec(24 * 3600 * 1000);
+    nwk.master_high.clock.utc_adjustment = OneDay;
+
+    usleep(400000);
+    ASSERT_LE(0, master_low.publish());   // Update from the old master
+    nwk.spinAll();
+
+    ASSERT_LE(0, master_high.publish());  // Update from the new master
+    nwk.spinAll();
+
+    usleep(400000);
+    ASSERT_LE(0, master_low.publish());   // Adjustment from the old master (ignored now)
+    ASSERT_LE(0, master_high.publish());  // Adjustment from the new master (accepted)
+    nwk.spinAll();
+
+    // Synchronization complete.
+    ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_high.clock.getUtc()));
+    ASSERT_FALSE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc()));
+    ASSERT_TRUE(slave.isActive());
+    ASSERT_EQ(nwk.master_high.node.getNodeID(), slave.getMasterNodeID());
+
+    /*
+     * Frequent calls to publish()
+     */
+    ASSERT_LE(0, master_low.publish()); // Dropped
+    ASSERT_LE(0, master_low.publish()); // Dropped
+    ASSERT_LE(0, master_low.publish()); // Dropped
+
+    ASSERT_TRUE(nwk.slave.can.read_queue.empty());
+
+    usleep(400000);
+    ASSERT_LE(0, master_low.publish()); // Accepted
+    ASSERT_FALSE(nwk.slave.can.read_queue.empty());
+
+    nwk.spinAll();
+
+    // Synchronization did not change
+    ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_high.clock.getUtc()));
+    ASSERT_FALSE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc()));
+    ASSERT_TRUE(slave.isActive());
+    ASSERT_EQ(nwk.master_high.node.getNodeID(), slave.getMasterNodeID());
+}