From d56d5d5dcafcd59da13f586d86672538d868eed8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 23 Mar 2014 21:35:01 +0400 Subject: [PATCH] GlobalTimeSyncSlave --- .../protocol/global_time_sync_slave.hpp | 48 +++++++ .../src/protocol/global_time_sync_slave.cpp | 87 ++++++++++++ .../test/protocol/global_time_sync_slave.cpp | 134 ++++++++++++++++++ 3 files changed, 269 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp create mode 100644 libuavcan/src/protocol/global_time_sync_slave.cpp create mode 100644 libuavcan/test/protocol/global_time_sync_slave.cpp diff --git a/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp new file mode 100644 index 0000000000..ba51b2b795 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + +namespace uavcan +{ + +class GlobalTimeSyncSlave : Noncopyable +{ + typedef MethodBinder&)> GlobalTimeSyncCallback; + + // Static buffers are explicitly disabled because time should never be unicasted. + Subscriber sub_; + + UtcTime prev_ts_utc_; + MonotonicTime prev_ts_mono_; + enum State { Update, Adjust } state; + NodeID master_nid_; + uint8_t prev_iface_index_; + + ISystemClock& getSystemClock() { return sub_.getNode().getSystemClock(); } + + void adjustFromMsg(const ReceivedDataStructure& msg); + + void updateFromMsg(const ReceivedDataStructure& msg); + + void processMsg(const ReceivedDataStructure& msg); + + void handleGlobalTimeSync(const ReceivedDataStructure& msg); + +public: + GlobalTimeSyncSlave(INode& node) + : sub_(node) + , state(Update) + , prev_iface_index_(0xFF) + { } + + int start(); +}; + +} diff --git a/libuavcan/src/protocol/global_time_sync_slave.cpp b/libuavcan/src/protocol/global_time_sync_slave.cpp new file mode 100644 index 0000000000..639680fef3 --- /dev/null +++ b/libuavcan/src/protocol/global_time_sync_slave.cpp @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ + +void GlobalTimeSyncSlave::adjustFromMsg(const ReceivedDataStructure& msg) +{ + assert(msg.prev_utc_usec > 0); + const UtcDuration adjustment = UtcTime::fromUSec(msg.prev_utc_usec) - prev_ts_utc_; + + UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment: usec=%lli snid=%i iface=%i", + static_cast(adjustment.toUSec()), + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex())); + + getSystemClock().adjustUtc(adjustment); + state = Update; +} + +void GlobalTimeSyncSlave::updateFromMsg(const ReceivedDataStructure& msg) +{ + UAVCAN_TRACE("GlobalTimeSyncSlave", "Update: snid=%i iface=%i", + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex())); + + prev_ts_utc_ = msg.getUtcTimestamp(); + prev_ts_mono_ = msg.getMonotonicTimestamp(); + master_nid_ = msg.getSrcNodeID(); + prev_iface_index_ = msg.getIfaceIndex(); + state = Adjust; +} + +void GlobalTimeSyncSlave::processMsg(const ReceivedDataStructure& msg) +{ + const MonotonicDuration time_since_prev_sync = msg.getMonotonicTimestamp() - prev_ts_mono_; + assert(!time_since_prev_sync.isNegative()); + + const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero(); + const bool switch_master = msg.getSrcNodeID() < master_nid_; + const bool timeout = time_since_prev_sync.toMSec() > protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS; + + if (switch_master || timeout || needs_init) + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i timeout=%i", + int(needs_init), int(switch_master), int(timeout)); + updateFromMsg(msg); + } + else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_) + { + if (state == Adjust && msg.prev_utc_usec > 0) + { + adjustFromMsg(msg); + } + else + { + updateFromMsg(msg); + } + } + else + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Ignored: snid=%i iface=%i", + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex())); + } +} + +void GlobalTimeSyncSlave::handleGlobalTimeSync(const ReceivedDataStructure& msg) +{ + if (msg.getTransferType() == TransferTypeMessageBroadcast) + { + processMsg(msg); + } + else + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType())); + } +} + +int GlobalTimeSyncSlave::start() +{ + return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync)); +} + +} diff --git a/libuavcan/test/protocol/global_time_sync_slave.cpp b/libuavcan/test/protocol/global_time_sync_slave.cpp new file mode 100644 index 0000000000..340b772c4f --- /dev/null +++ b/libuavcan/test/protocol/global_time_sync_slave.cpp @@ -0,0 +1,134 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + + +TEST(GlobalTimeSyncSlave, Basic) +{ + InterlinkedTestNodesWithClockMock nodes(64, 65); + + SystemClockMock& slave_clock = nodes.clock_a; + SystemClockMock& master_clock = nodes.clock_b; + + slave_clock.advance(1000000); + master_clock.advance(1000000); + + master_clock.monotonic_auto_advance = slave_clock.monotonic_auto_advance = 1000; + master_clock.preserve_utc = slave_clock.preserve_utc = true; + slave_clock.utc = 0; // Not set yet + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + uavcan::GlobalTimeSyncSlave gtss(nodes.a); + uavcan::Publisher gts_pub(nodes.b); + + ASSERT_LE(0, gtss.start()); + + /* + * Empty broadcast + * The slave must only register the timestamp and adjust nothing + */ + uavcan::protocol::GlobalTimeSync gts; + gts.prev_utc_usec = 0; + gts_pub.broadcast(gts); + gts.prev_utc_usec = master_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_EQ(0, slave_clock.utc); + ASSERT_EQ(1000000, master_clock.utc); + std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl; + std::cout << "Slave mono=" << slave_clock.monotonic << " utc=" << slave_clock.utc << std::endl; + + /* + * Follow-up broadcast with proper time + * Slave must adjust now + */ + gts_pub.broadcast(gts); + gts.prev_utc_usec = master_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(1000000, slave_clock.utc); + ASSERT_EQ(1000000, master_clock.utc); + std::cout << "Master mono=" << master_clock.monotonic << " utc=" << master_clock.utc << std::endl; + std::cout << "Slave mono=" << slave_clock.monotonic << " utc=" << slave_clock.utc << std::endl; + + master_clock.utc += 1000000; + slave_clock.utc += 1000000; + + /* + * Next follow-up, slave is synchronized now + * Will update + */ + gts_pub.broadcast(gts); + gts.prev_utc_usec = master_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(2000000, slave_clock.utc); + ASSERT_EQ(2000000, master_clock.utc); + + master_clock.utc += 1000000; + slave_clock.utc += 1000000; + + /* + * Next follow-up, slave is synchronized now + * Will adjust + */ + gts_pub.broadcast(gts); + gts.prev_utc_usec = master_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(3000000, slave_clock.utc); + ASSERT_EQ(3000000, master_clock.utc); + + master_clock.utc += 1000000; + slave_clock.utc += 1000000; + ASSERT_EQ(4000000, slave_clock.utc); + ASSERT_EQ(4000000, master_clock.utc); + + /* + * Another master + * This one has higher priority, so it will be preferred + */ + SystemClockMock master2_clock(100); + master2_clock.monotonic_auto_advance = 1000; + master2_clock.preserve_utc = true; + PairableCanDriver master2_can(master2_clock); + master2_can.other = &nodes.can_a; + TestNode master2_node(master2_can, master2_clock, 8); + + uavcan::Publisher gts_pub2(master2_node); + + /* + * Update step, no adjustment yet + */ + gts.prev_utc_usec = 0; + gts_pub2.broadcast(gts); + gts.prev_utc_usec = master2_clock.utc; + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(4000000, slave_clock.utc); + ASSERT_EQ(100, master2_clock.utc); + + master2_clock.utc += 1000000; + + /* + * Adjustment + */ + gts_pub2.broadcast(gts); + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(100, slave_clock.utc); + + /* + * Another master will be ignored now + */ + gts.prev_utc_usec = 99999999; + // Update + gts_pub.broadcast(gts); + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(100, slave_clock.utc); + // Adjust + gts_pub.broadcast(gts); + nodes.spinBoth(uavcan::MonotonicDuration()); + ASSERT_EQ(100, slave_clock.utc); +}