GlobalTimeSyncMaster

libuavcan/include/uavcan/protocol/global_time_sync_master.hpp

@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/node/publisher.hpp>
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/util/lazy_constructor.hpp>
+#include <uavcan/protocol/GlobalTimeSync.hpp>
+
+namespace uavcan
+{
+/**
+ * TODO: Enforce max one master per node
+ */
+class GlobalTimeSyncMaster : protected LoopbackFrameListenerBase
+{
+    class IfaceMaster
+    {
+        Publisher<protocol::GlobalTimeSync> pub_;
+        MonotonicTime prev_pub_mono_;
+        UtcTime prev_tx_utc_;
+        const uint8_t iface_index_;
+
+    public:
+        IfaceMaster(INode& node, uint8_t iface_index)
+        : pub_(node)
+        , iface_index_(iface_index)
+        {
+            assert(iface_index < MaxCanIfaces);
+        }
+
+        int init();
+
+        void setTxTimestamp(UtcTime ts);
+
+        int publish();
+    };
+
+    INode& node_;
+    LazyConstructor<IfaceMaster> iface_masters_[MaxCanIfaces];
+    DataTypeID dtid_;
+    bool initialized_;
+
+    void handleLoopbackFrame(const RxFrame& frame);
+
+public:
+    GlobalTimeSyncMaster(INode& node)
+    : LoopbackFrameListenerBase(node.getDispatcher())
+    , node_(node)
+    , initialized_(false)
+    { }
+
+    int init();
+
+    bool isInitialized() const { return initialized_; }
+
+    int publish();
+};
+
+}

libuavcan/src/protocol/global_time_sync_master.cpp

@@ -0,0 +1,160 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cstdlib>
+#include <cassert>
+#include <uavcan/protocol/global_time_sync_master.hpp>
+#include <uavcan/debug.hpp>
+
+namespace uavcan
+{
+/*
+ * GlobalTimeSyncMaster::IfaceMaster
+ */
+int GlobalTimeSyncMaster::IfaceMaster::init()
+{
+    const int res = pub_.init();
+    if (res >= 0)
+    {
+        TransferSender* const ts = pub_.getTransferSender();
+        assert(ts != NULL);
+        ts->setIfaceMask(1 << iface_index_);
+        ts->setCanIOFlags(CanIOFlagLoopback);
+    }
+    return res;
+}
+
+void GlobalTimeSyncMaster::IfaceMaster::setTxTimestamp(UtcTime ts)
+{
+    prev_tx_utc_ = UtcTime();
+    if (ts.isZero())
+    {
+        assert(0);
+        pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster got zero UTC TX timestamp");
+        return;
+    }
+    if (!prev_tx_utc_.isZero())
+    {
+        assert(0);
+        pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster publication conflict");
+        return;
+    }
+    prev_tx_utc_ = ts;
+}
+
+int GlobalTimeSyncMaster::IfaceMaster::publish()
+{
+    assert(pub_.getTransferSender()->getCanIOFlags() == CanIOFlagLoopback);
+    assert(pub_.getTransferSender()->getIfaceMask() == (1 << iface_index_));
+
+    const MonotonicTime ts_mono = pub_.getNode().getMonotonicTime();
+    const MonotonicDuration since_prev_pub = ts_mono - prev_pub_mono_;
+    assert(!since_prev_pub.isNegative());
+
+    if (since_prev_pub.toMSec() > protocol::GlobalTimeSync::MIN_PUBLICATION_PERIOD_MS)
+    {
+        prev_pub_mono_ = ts_mono;
+        protocol::GlobalTimeSync msg;
+        if (since_prev_pub.toMSec() < protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS)
+        {
+            msg.prev_utc_usec = prev_tx_utc_.toUSec();
+        }
+        else
+        {
+            msg.prev_utc_usec = 0;
+        }
+        prev_tx_utc_ = UtcTime();
+        UAVCAN_TRACE("GlobalTimeSyncMaster", "Publishing %llu", static_cast<unsigned long long>(msg.prev_utc_usec));
+        return pub_.broadcast(msg);
+    }
+    else
+    {
+        UAVCAN_TRACE("GlobalTimeSyncMaster", "Publication skipped");
+        return 0;
+    }
+}
+
+/*
+ * GlobalTimeSyncMaster
+ */
+void GlobalTimeSyncMaster::handleLoopbackFrame(const RxFrame& frame)
+{
+    const uint8_t iface = frame.getIfaceIndex();
+    if (initialized_ && iface < MaxCanIfaces)
+    {
+        if (frame.getDataTypeID() == dtid_ &&
+            frame.getTransferType() == TransferTypeMessageBroadcast &&
+            frame.isLast() && frame.isFirst())
+        {
+            iface_masters_[iface]->setTxTimestamp(frame.getUtcTimestamp());
+        }
+    }
+    else
+    {
+        assert(0);
+    }
+}
+
+int GlobalTimeSyncMaster::init()
+{
+    if (initialized_)
+    {
+        return 0;
+    }
+
+    // Data type ID
+    const DataTypeDescriptor* const desc = GlobalDataTypeRegistry::instance().find(DataTypeKindMessage,
+        protocol::GlobalTimeSync::getDataTypeFullName());
+    if (desc == NULL)
+    {
+        return -1;
+    }
+    dtid_ = desc->getID();
+
+    // Iface master array
+    int res = -1;
+    for (uint8_t i = 0; i < MaxCanIfaces; i++)
+    {
+        if (!iface_masters_[i].isConstructed())
+        {
+            iface_masters_[i].construct<INode&, uint8_t>(node_, i);
+        }
+        res = iface_masters_[i]->init();
+        if (res < 0)
+        {
+            break;
+        }
+    }
+
+    // Loopback listener
+    initialized_ = res >= 0;
+    if (initialized_)
+    {
+        LoopbackFrameListenerBase::startListening();
+    }
+    return res;
+}
+
+int GlobalTimeSyncMaster::publish()
+{
+    if (!initialized_)
+    {
+        const int res = init();
+        if (res < 0)
+        {
+            return res;
+        }
+    }
+    for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++)
+    {
+        const int res = iface_masters_[i]->publish();
+        if (res < 0)
+        {
+            return res;
+        }
+    }
+    return 0;
+}
+
+}

libuavcan/test/clock.hpp

@@ -62,6 +62,8 @@ public:
 class SystemClockDriver : public uavcan::ISystemClock
 {
 public:
+    uavcan::UtcDuration utc_adjustment;
+
     uavcan::MonotonicTime getMonotonic() const
     {
         struct timespec ts;
@@ -83,12 +85,12 @@ public:
             assert(0);
             return uavcan::UtcTime();
         }
-        return uavcan::UtcTime::fromUSec(uint64_t(tv.tv_sec) * 1000000UL + tv.tv_usec);
+        return uavcan::UtcTime::fromUSec(uint64_t(tv.tv_sec) * 1000000UL + tv.tv_usec) + utc_adjustment;
     }
 
-    void adjustUtc(uavcan::UtcDuration)
+    void adjustUtc(uavcan::UtcDuration adjustment)
     {
-        assert(0);
+        utc_adjustment += adjustment;
     }
 };
 

libuavcan/test/protocol/global_time_sync_master.cpp

@@ -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());
+}