global_time_sync_master.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <gtest/gtest.h>
00006 #include <uavcan/protocol/global_time_sync_master.hpp>
00007 #include <uavcan/protocol/global_time_sync_slave.hpp>
00008 #include "helpers.hpp"
00009 
00010 struct GlobalTimeSyncMasterTestNode
00011 {
00012     SystemClockDriver clock;
00013     PairableCanDriver can;
00014     TestNode node;
00015 
00016     GlobalTimeSyncMasterTestNode(uavcan::NodeID nid)
00017         : can(clock)
00018         , node(can, clock, nid)
00019     { }
00020 };
00021 
00022 struct GlobalTimeSyncTestNetwork
00023 {
00024     GlobalTimeSyncMasterTestNode slave;
00025     GlobalTimeSyncMasterTestNode master_low;
00026     GlobalTimeSyncMasterTestNode master_high;
00027 
00028     GlobalTimeSyncTestNetwork()
00029         : slave(64)
00030         , master_low(120)
00031         , master_high(8)
00032     {
00033         slave.can.others.insert(&master_low.can);
00034         master_low.can.others.insert(&slave.can);
00035         master_high.can.others.insert(&slave.can);
00036     }
00037 
00038     void spinAll(uavcan::MonotonicDuration duration = uavcan::MonotonicDuration::fromMSec(9))
00039     {
00040         assert(!duration.isNegative());
00041         unsigned nspins3 = unsigned(duration.toMSec() / 3);
00042         nspins3 = nspins3 ? nspins3 : 2;
00043         while (nspins3 --> 0)
00044         {
00045             ASSERT_LE(0, slave.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
00046             ASSERT_LE(0, master_low.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
00047             ASSERT_LE(0, master_high.node.spin(uavcan::MonotonicDuration::fromMSec(1)));
00048         }
00049     }
00050 };
00051 
00052 TEST(GlobalTimeSyncMaster, Basic)
00053 {
00054     GlobalTimeSyncTestNetwork nwk;
00055 
00056     uavcan::GlobalDataTypeRegistry::instance().reset();
00057     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalTimeSync> _reg1;
00058 
00059     uavcan::GlobalTimeSyncSlave slave(nwk.slave.node);
00060     uavcan::GlobalTimeSyncMaster master_low(nwk.master_low.node);
00061     uavcan::GlobalTimeSyncMaster master_high(nwk.master_high.node);
00062 
00063     ASSERT_FALSE(master_low.isInitialized());
00064 
00065     ASSERT_LE(0, slave.start());
00066     ASSERT_LE(0, master_low.init());
00067     ASSERT_LE(0, master_high.init());
00068 
00069     ASSERT_TRUE(master_low.isInitialized());
00070     ASSERT_FALSE(slave.isActive());
00071 
00072     /*
00073      * Simple synchronization
00074      */
00075     ASSERT_LE(0, master_low.publish());  // Update
00076     nwk.spinAll();
00077 
00078     usleep(400000);
00079     ASSERT_LE(0, master_low.publish());  // Adjustment
00080     nwk.spinAll();
00081 
00082     // Synchronization complete.
00083     ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc()));
00084     ASSERT_TRUE(slave.isActive());
00085     ASSERT_EQ(nwk.master_low.node.getNodeID(), slave.getMasterNodeID());
00086 
00087     /*
00088      * Moving clocks forward and re-syncing with another master
00089      */
00090     static const uavcan::UtcDuration OneDay = uavcan::UtcDuration::fromMSec(24 * 3600 * 1000);
00091     nwk.master_high.clock.utc_adjustment = OneDay;
00092 
00093     usleep(400000);
00094     ASSERT_LE(0, master_low.publish());   // Update from the old master
00095     nwk.spinAll();
00096 
00097     ASSERT_LE(0, master_high.publish());  // Update from the new master
00098     nwk.spinAll();
00099 
00100     usleep(400000);
00101     ASSERT_LE(0, master_low.publish());   // Adjustment from the old master (ignored now)
00102     ASSERT_LE(0, master_high.publish());  // Adjustment from the new master (accepted)
00103     nwk.spinAll();
00104 
00105     // Synchronization complete.
00106     ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_high.clock.getUtc()));
00107     ASSERT_FALSE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc()));
00108     ASSERT_TRUE(slave.isActive());
00109     ASSERT_EQ(nwk.master_high.node.getNodeID(), slave.getMasterNodeID());
00110 
00111     /*
00112      * Frequent calls to publish()
00113      */
00114     ASSERT_LE(0, master_low.publish()); // Dropped
00115     ASSERT_LE(0, master_low.publish()); // Dropped
00116     ASSERT_LE(0, master_low.publish()); // Dropped
00117 
00118     ASSERT_TRUE(nwk.slave.can.read_queue.empty());
00119 
00120     usleep(400000);
00121     ASSERT_LE(0, master_low.publish()); // Accepted
00122     ASSERT_FALSE(nwk.slave.can.read_queue.empty());
00123 
00124     nwk.spinAll();
00125 
00126     // Synchronization did not change
00127     ASSERT_TRUE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_high.clock.getUtc()));
00128     ASSERT_FALSE(areTimestampsClose(nwk.slave.clock.getUtc(), nwk.master_low.clock.getUtc()));
00129     ASSERT_TRUE(slave.isActive());
00130     ASSERT_EQ(nwk.master_high.node.getNodeID(), slave.getMasterNodeID());
00131 }