From fc7e2421008f76dea2633dd28840059f41880e61 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 12:10:04 +0300 Subject: [PATCH] Global time sync slave moved to header --- .../protocol/global_time_sync_slave.hpp | 104 ++++++++++++++-- .../protocol/uc_global_time_sync_slave.cpp | 115 ------------------ 2 files changed, 97 insertions(+), 122 deletions(-) delete mode 100644 libuavcan/src/protocol/uc_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 index 3da6532998..0490fe0e84 100644 --- a/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp +++ b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp @@ -8,6 +8,8 @@ #include #include #include +#include +#include namespace uavcan { @@ -43,13 +45,93 @@ class UAVCAN_EXPORT GlobalTimeSyncSlave : Noncopyable ISystemClock& getSystemClock() const { return sub_.getNode().getSystemClock(); } - void adjustFromMsg(const ReceivedDataStructure& msg); + void adjustFromMsg(const ReceivedDataStructure& msg) + { + UAVCAN_ASSERT(msg.previous_transmission_timestamp_usec > 0); + const UtcDuration adjustment = UtcTime::fromUSec(msg.previous_transmission_timestamp_usec) - prev_ts_utc_; - void updateFromMsg(const ReceivedDataStructure& msg); + UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment: usec=%lli snid=%i iface=%i suppress=%i", + static_cast(adjustment.toUSec()), + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()), int(suppressed_)); - void processMsg(const ReceivedDataStructure& msg); + if (!suppressed_) + { + getSystemClock().adjustUtc(adjustment); + } + last_adjustment_ts_ = msg.getMonotonicTimestamp(); + state_ = Update; + } - void handleGlobalTimeSync(const ReceivedDataStructure& msg); + void 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(); + prev_tid_ = msg.getTransferID(); + state_ = Adjust; + } + + void processMsg(const ReceivedDataStructure& msg) + { + const MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_; + UAVCAN_ASSERT(!since_prev_msg.isNegative()); + + const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero(); + const bool switch_master = msg.getSrcNodeID() < master_nid_; + const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS; + + if (switch_master || pub_timeout || needs_init) + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i pub_timeout=%i", + int(needs_init), int(switch_master), int(pub_timeout)); + updateFromMsg(msg); + } + else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_) + { + if (state_ == Adjust) + { + const bool msg_invalid = msg.previous_transmission_timestamp_usec == 0; + const bool wrong_tid = prev_tid_.computeForwardDistance(msg.getTransferID()) != 1; + const bool wrong_timing = since_prev_msg.toMSec() > protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS; + if (msg_invalid || wrong_tid || wrong_timing) + { + UAVCAN_TRACE("GlobalTimeSyncSlave", + "Adjustment skipped: msg_invalid=%i wrong_tid=%i wrong_timing=%i", + int(msg_invalid), int(wrong_tid), int(wrong_timing)); + state_ = Update; + } + } + if (state_ == Adjust) + { + adjustFromMsg(msg); + } + else + { + updateFromMsg(msg); + } + } + else + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Ignored: snid=%i iface=%i", + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex())); + } + } + + void handleGlobalTimeSync(const ReceivedDataStructure& msg) + { + if (msg.getTransferType() == TransferTypeMessageBroadcast) + { + processMsg(msg); + } + else + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType())); + } + } public: explicit GlobalTimeSyncSlave(INode& node) @@ -64,7 +146,10 @@ public: * attention from the application, other than to handle a clock adjustment request occasionally. * Returns negative error code. */ - int start(); + int start() + { + return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync)); + } /** * Enable or disable the suppressed mode. @@ -89,13 +174,18 @@ public: * Note that immediately after start up the slave will be INACTIVE until it finds a master. * Please read the specs to learn more. */ - bool isActive() const; + bool isActive() const + { + const MonotonicDuration since_prev_adj = getSystemClock().getMonotonic() - last_adjustment_ts_; + return !last_adjustment_ts_.isZero() && + (since_prev_adj.toMSec() <= protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS); + } /** * Node ID of the master the slave is currently locked on. * Returns an invalid Node ID if there's no active master. */ - NodeID getMasterNodeID() const; + NodeID getMasterNodeID() const { return isActive() ? master_nid_ : NodeID(); } /** * Last time when the local clock adjustment was performed. diff --git a/libuavcan/src/protocol/uc_global_time_sync_slave.cpp b/libuavcan/src/protocol/uc_global_time_sync_slave.cpp deleted file mode 100644 index 5a52ee29ae..0000000000 --- a/libuavcan/src/protocol/uc_global_time_sync_slave.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#include -#include - -namespace uavcan -{ - -void GlobalTimeSyncSlave::adjustFromMsg(const ReceivedDataStructure& msg) -{ - UAVCAN_ASSERT(msg.previous_transmission_timestamp_usec > 0); - const UtcDuration adjustment = UtcTime::fromUSec(msg.previous_transmission_timestamp_usec) - prev_ts_utc_; - - UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment: usec=%lli snid=%i iface=%i suppress=%i", - static_cast(adjustment.toUSec()), - int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()), int(suppressed_)); - - if (!suppressed_) - { - getSystemClock().adjustUtc(adjustment); - } - last_adjustment_ts_ = msg.getMonotonicTimestamp(); - 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(); - prev_tid_ = msg.getTransferID(); - state_ = Adjust; -} - -void GlobalTimeSyncSlave::processMsg(const ReceivedDataStructure& msg) -{ - const MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_; - UAVCAN_ASSERT(!since_prev_msg.isNegative()); - - const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero(); - const bool switch_master = msg.getSrcNodeID() < master_nid_; - const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS; - - if (switch_master || pub_timeout || needs_init) - { - UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i pub_timeout=%i", - int(needs_init), int(switch_master), int(pub_timeout)); - updateFromMsg(msg); - } - else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_) - { - if (state_ == Adjust) - { - const bool msg_invalid = msg.previous_transmission_timestamp_usec == 0; - const bool wrong_tid = prev_tid_.computeForwardDistance(msg.getTransferID()) != 1; - const bool wrong_timing = since_prev_msg.toMSec() > protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS; - if (msg_invalid || wrong_tid || wrong_timing) - { - UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment skipped: msg_invalid=%i wrong_tid=%i wrong_timing=%i", - int(msg_invalid), int(wrong_tid), int(wrong_timing)); - state_ = Update; - } - } - if (state_ == Adjust) - { - 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)); -} - -bool GlobalTimeSyncSlave::isActive() const -{ - const MonotonicDuration since_prev_adj = getSystemClock().getMonotonic() - last_adjustment_ts_; - return !last_adjustment_ts_.isZero() && since_prev_adj.toMSec() <= protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS; -} - -NodeID GlobalTimeSyncSlave::getMasterNodeID() const -{ - return isActive() ? master_nid_ : NodeID(); -} - -}