global_time_sync_slave.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <gtest/gtest.h>
00006 #include <uavcan/node/publisher.hpp>
00007 #include <uavcan/protocol/global_time_sync_slave.hpp>
00008 #include "helpers.hpp"
00009 
00010 
00011 TEST(GlobalTimeSyncSlave, Basic)
00012 {
00013     InterlinkedTestNodesWithClockMock nodes(64, 65);
00014 
00015     SystemClockMock& slave_clock = nodes.clock_a;
00016     SystemClockMock& master_clock = nodes.clock_b;
00017 
00018     slave_clock.advance(1000000);
00019     master_clock.advance(1000000);
00020 
00021     master_clock.monotonic_auto_advance = slave_clock.monotonic_auto_advance = 1000;
00022     master_clock.preserve_utc = slave_clock.preserve_utc = true;
00023     slave_clock.utc = 0; // Not set yet
00024 
00025     uavcan::GlobalDataTypeRegistry::instance().reset();
00026     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalTimeSync> _reg1;
00027 
00028     uavcan::GlobalTimeSyncSlave gtss(nodes.a);
00029     uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub(nodes.b);
00030 
00031     ASSERT_LE(0, gtss.start());
00032     ASSERT_FALSE(gtss.isActive());
00033     ASSERT_FALSE(gtss.getMasterNodeID().isValid());
00034 
00035     /*
00036      * Empty broadcast
00037      * The slave must only register the timestamp and adjust nothing
00038      */
00039     uavcan::protocol::GlobalTimeSync gts;
00040     gts.previous_transmission_timestamp_usec = 0;
00041     gts_pub.broadcast(gts);
00042     gts.previous_transmission_timestamp_usec = master_clock.utc;
00043     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
00044     ASSERT_EQ(0, slave_clock.utc);
00045     ASSERT_EQ(1000000, master_clock.utc);
00046     std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl;
00047     std::cout << "Slave  mono=" << slave_clock.monotonic  << " utc=" << slave_clock.utc << std::endl;
00048 
00049     ASSERT_FALSE(gtss.isActive());
00050     ASSERT_FALSE(gtss.getMasterNodeID().isValid());
00051 
00052     /*
00053      * Follow-up broadcast with proper time
00054      * Slave must adjust now
00055      */
00056     gts_pub.broadcast(gts);
00057     gts.previous_transmission_timestamp_usec = master_clock.utc;
00058     nodes.spinBoth(uavcan::MonotonicDuration());
00059     ASSERT_EQ(1000000, slave_clock.utc);
00060     ASSERT_EQ(1000000, master_clock.utc);
00061     std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl;
00062     std::cout << "Slave  mono=" << slave_clock.monotonic  << " utc=" << slave_clock.utc << std::endl;
00063 
00064     master_clock.utc += 1000000;
00065     slave_clock.utc += 1000000;
00066 
00067     ASSERT_TRUE(gtss.isActive());
00068     ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
00069 
00070     /*
00071      * Next follow-up, slave is synchronized now
00072      * Will update
00073      */
00074     gts_pub.broadcast(gts);
00075     gts.previous_transmission_timestamp_usec = master_clock.utc;
00076     nodes.spinBoth(uavcan::MonotonicDuration());
00077     ASSERT_EQ(2000000, slave_clock.utc);
00078     ASSERT_EQ(2000000, master_clock.utc);
00079 
00080     master_clock.utc += 1000000;
00081     slave_clock.utc += 1000000;
00082 
00083     ASSERT_TRUE(gtss.isActive());
00084     ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
00085 
00086     /*
00087      * Next follow-up, slave is synchronized now
00088      * Will adjust
00089      */
00090     gts_pub.broadcast(gts);
00091     gts.previous_transmission_timestamp_usec = master_clock.utc;
00092     nodes.spinBoth(uavcan::MonotonicDuration());
00093     ASSERT_EQ(3000000, slave_clock.utc);
00094     ASSERT_EQ(3000000, master_clock.utc);
00095 
00096     master_clock.utc += 1000000;
00097     slave_clock.utc += 1000000;
00098     ASSERT_EQ(4000000, slave_clock.utc);
00099     ASSERT_EQ(4000000, master_clock.utc);
00100 
00101     ASSERT_TRUE(gtss.isActive());
00102     ASSERT_EQ(nodes.b.getNodeID(), gtss.getMasterNodeID());
00103 
00104     /*
00105      * Another master
00106      * This one has higher priority, so it will be preferred
00107      */
00108     SystemClockMock master2_clock(100);
00109     master2_clock.monotonic_auto_advance = 1000;
00110     master2_clock.preserve_utc = true;
00111     PairableCanDriver master2_can(master2_clock);
00112     master2_can.others.insert(&nodes.can_a);
00113     TestNode master2_node(master2_can, master2_clock, 8);
00114 
00115     uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub2(master2_node);
00116 
00117     /*
00118      * Update step, no adjustment yet
00119      */
00120     gts.previous_transmission_timestamp_usec = 0;
00121     gts_pub2.broadcast(gts);
00122     gts.previous_transmission_timestamp_usec = master2_clock.utc;
00123     nodes.spinBoth(uavcan::MonotonicDuration());
00124     ASSERT_EQ(4000000, slave_clock.utc);
00125     ASSERT_EQ(100, master2_clock.utc);
00126 
00127     master2_clock.utc += 1000000;
00128 
00129     ASSERT_TRUE(gtss.isActive());
00130     ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
00131 
00132     /*
00133      * Adjustment
00134      */
00135     gts_pub2.broadcast(gts);
00136     nodes.spinBoth(uavcan::MonotonicDuration());
00137     ASSERT_EQ(100, slave_clock.utc);
00138 
00139     ASSERT_TRUE(gtss.isActive());
00140     ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
00141 
00142     /*
00143      * Another master will be ignored now
00144      */
00145     gts.previous_transmission_timestamp_usec = 99999999;
00146     // Update
00147     gts_pub.broadcast(gts);
00148     nodes.spinBoth(uavcan::MonotonicDuration());
00149     ASSERT_EQ(100, slave_clock.utc);
00150     // Adjust
00151     gts_pub.broadcast(gts);
00152     nodes.spinBoth(uavcan::MonotonicDuration());
00153     ASSERT_EQ(100, slave_clock.utc);
00154 
00155     ASSERT_TRUE(gtss.isActive());
00156     ASSERT_EQ(master2_node.getNodeID(), gtss.getMasterNodeID());
00157 
00158     /*
00159      * Timeout
00160      */
00161     slave_clock.advance(100000000);
00162 
00163     ASSERT_FALSE(gtss.isActive());
00164     ASSERT_FALSE(gtss.getMasterNodeID().isValid());
00165 }
00166 
00167 
00168 #if !defined(BYTE_ORDER) || !defined(LITTLE_ENDIAN) || (BYTE_ORDER != LITTLE_ENDIAN)
00169 # error "This test cannot be executed on this platform"
00170 #endif
00171 
00172 static uavcan::Frame makeSyncMsg(uavcan::uint64_t usec, uavcan::NodeID snid, uavcan::TransferID tid)
00173 {
00174     uavcan::Frame frame(uavcan::protocol::GlobalTimeSync::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
00175                         snid, uavcan::NodeID::Broadcast, tid);
00176     frame.setStartOfTransfer(true);
00177     frame.setEndOfTransfer(true);
00178     EXPECT_EQ(7, frame.setPayload(reinterpret_cast<uint8_t*>(&usec), 7)); // Assuming little endian!!!
00179     return frame;
00180 }
00181 
00182 static void broadcastSyncMsg(CanIfaceMock& iface, uavcan::uint64_t usec, uavcan::NodeID snid, uavcan::TransferID tid)
00183 {
00184     const uavcan::Frame frame = makeSyncMsg(usec, snid, tid);
00185     uavcan::CanFrame can_frame;
00186     ASSERT_TRUE(frame.compile(can_frame));
00187     iface.pushRx(can_frame);
00188 }
00189 
00190 
00191 TEST(GlobalTimeSyncSlave, Validation)
00192 {
00193     SystemClockMock slave_clock;
00194     slave_clock.monotonic = 1000000;
00195     slave_clock.preserve_utc = true;
00196 
00197     CanDriverMock slave_can(3, slave_clock);
00198     for (uint8_t i = 0; i < slave_can.getNumIfaces(); i++)
00199     {
00200         slave_can.ifaces.at(i).enable_utc_timestamping = true;
00201     }
00202 
00203     TestNode node(slave_can, slave_clock, 64);
00204 
00205     uavcan::GlobalTimeSyncSlave gtss(node);
00206     uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub(node);
00207 
00208     ASSERT_LE(0, gtss.start());
00209     ASSERT_FALSE(gtss.isActive());
00210     ASSERT_FALSE(gtss.getMasterNodeID().isValid());
00211     ASSERT_EQ(0, slave_clock.utc);
00212 
00213     /*
00214      * Update/adjust/update
00215      */
00216     broadcastSyncMsg(slave_can.ifaces.at(0), 0, 8, 0);    // Locked on this
00217     broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 0); // Ignored
00218     ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
00219 
00220     broadcastSyncMsg(slave_can.ifaces.at(0), 1000, 8, 1); // Adjust 1000 ahead
00221     broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 1); // Ignored
00222     ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
00223 
00224     ASSERT_TRUE(gtss.isActive());
00225     ASSERT_EQ(8, gtss.getMasterNodeID().get());
00226     ASSERT_EQ(1000, slave_clock.utc);
00227 
00228     broadcastSyncMsg(slave_can.ifaces.at(0), 2000, 8, 2); // Update
00229     ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
00230 
00231     ASSERT_EQ(1000, slave_clock.utc);
00232     std::cout << slave_clock.monotonic << std::endl;
00233 
00234     /*
00235      * TID jump simulates a frame loss
00236      */
00237     broadcastSyncMsg(slave_can.ifaces.at(0), 3000, 8, 4); // Adjustment skipped - expected TID 3, update instead
00238     ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
00239 
00240     ASSERT_TRUE(gtss.isActive());
00241     ASSERT_EQ(8, gtss.getMasterNodeID().get());
00242     ASSERT_EQ(1000, slave_clock.utc);
00243     std::cout << slave_clock.monotonic << std::endl;
00244 
00245     /*
00246      * Valid adjustment - continuing from TID 4
00247      */
00248     broadcastSyncMsg(slave_can.ifaces.at(0), 3000, 8, 5);  // Slave UTC was 1000, master reports 3000 --> shift ahead
00249     broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 5);
00250     ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
00251 
00252     ASSERT_TRUE(gtss.isActive());
00253     ASSERT_EQ(8, gtss.getMasterNodeID().get());
00254     ASSERT_EQ(3000, slave_clock.utc);
00255     std::cout << slave_clock.monotonic << std::endl;
00256 
00257     /*
00258      * Update, then very long delay with correct TID
00259      */
00260     broadcastSyncMsg(slave_can.ifaces.at(0), 2000, 8, 6); // Valid update, slave UTC is 3000
00261     broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 6);
00262     ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
00263 
00264     slave_clock.monotonic += 5000000;
00265 
00266     broadcastSyncMsg(slave_can.ifaces.at(0), 5000, 8, 7); // Adjustment skipped
00267     broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 7);
00268     ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
00269 
00270     broadcastSyncMsg(slave_can.ifaces.at(0), 5000, 8, 8); // Valid adjustment now
00271     broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 8);
00272     ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
00273 
00274     ASSERT_TRUE(gtss.isActive());
00275     ASSERT_EQ(8, gtss.getMasterNodeID().get());
00276     ASSERT_EQ(5000, slave_clock.utc);
00277     std::cout << slave_clock.monotonic << std::endl;
00278 }
00279 
00280 
00281 TEST(GlobalTimeSyncSlave, Suppression)
00282 {
00283     SystemClockMock slave_clock;
00284     slave_clock.monotonic = 1000000;
00285     slave_clock.preserve_utc = true;
00286 
00287     CanDriverMock slave_can(3, slave_clock);
00288     for (uint8_t i = 0; i < slave_can.getNumIfaces(); i++)
00289     {
00290         slave_can.ifaces.at(i).enable_utc_timestamping = true;
00291     }
00292 
00293     TestNode node(slave_can, slave_clock, 64);
00294 
00295     uavcan::GlobalTimeSyncSlave gtss(node);
00296     uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub(node);
00297 
00298     ASSERT_LE(0, gtss.start());
00299     ASSERT_EQ(0, slave_clock.utc);
00300 
00301     gtss.suppress(true);
00302 
00303     broadcastSyncMsg(slave_can.ifaces.at(0), 0, 8, 0);    // Locked on this
00304     broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 0); // Ignored
00305     ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
00306 
00307     broadcastSyncMsg(slave_can.ifaces.at(0), 1000, 8, 1); // Adjust 1000 ahead
00308     broadcastSyncMsg(slave_can.ifaces.at(1), 2000, 8, 1); // Ignored
00309     ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
00310 
00311     ASSERT_TRUE(gtss.isActive());
00312     ASSERT_EQ(8, gtss.getMasterNodeID().get());
00313     ASSERT_EQ(0, slave_clock.utc);                  // The clock shall not be asjusted
00314 }