From 0b33dbe1735350f6e6b92faa9c8113879fbb4440 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 24 Mar 2014 20:39:59 +0400 Subject: [PATCH] GlobalTimeSyncMaster --- .../protocol/global_time_sync_master.hpp | 63 +++++++ .../src/protocol/global_time_sync_master.cpp | 160 ++++++++++++++++++ libuavcan/test/clock.hpp | 8 +- .../test/protocol/global_time_sync_master.cpp | 131 ++++++++++++++ 4 files changed, 359 insertions(+), 3 deletions(-) create mode 100644 libuavcan/include/uavcan/protocol/global_time_sync_master.hpp create mode 100644 libuavcan/src/protocol/global_time_sync_master.cpp create mode 100644 libuavcan/test/protocol/global_time_sync_master.cpp diff --git a/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp new file mode 100644 index 0000000000..a193a8de77 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * TODO: Enforce max one master per node + */ +class GlobalTimeSyncMaster : protected LoopbackFrameListenerBase +{ + class IfaceMaster + { + Publisher 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 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(); +}; + +} diff --git a/libuavcan/src/protocol/global_time_sync_master.cpp b/libuavcan/src/protocol/global_time_sync_master.cpp new file mode 100644 index 0000000000..5727dbbba9 --- /dev/null +++ b/libuavcan/src/protocol/global_time_sync_master.cpp @@ -0,0 +1,160 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + +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(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(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; +} + +} diff --git a/libuavcan/test/clock.hpp b/libuavcan/test/clock.hpp index 7ef9d94ecc..349425cb7b 100644 --- a/libuavcan/test/clock.hpp +++ b/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; } }; diff --git a/libuavcan/test/protocol/global_time_sync_master.cpp b/libuavcan/test/protocol/global_time_sync_master.cpp new file mode 100644 index 0000000000..b50d0282db --- /dev/null +++ b/libuavcan/test/protocol/global_time_sync_master.cpp @@ -0,0 +1,131 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#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 _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()); +}