diff --git a/dsdl/uavcan/protocol/0.GlobalTimeSync.uavcan b/dsdl/uavcan/protocol/0.GlobalTimeSync.uavcan index 803c00f0ab..d85fd3232b 100644 --- a/dsdl/uavcan/protocol/0.GlobalTimeSync.uavcan +++ b/dsdl/uavcan/protocol/0.GlobalTimeSync.uavcan @@ -5,8 +5,9 @@ # This message is not intended for unicast transfers. # -uint16 MAX_PUBLICATION_PERIOD_MS = 2000 -uint16 DEFAULT_PUBLICATION_PERIOD_MS = 1000 +uint16 MAX_PUBLICATION_PERIOD_MS = 1100 +uint16 MIN_PUBLICATION_PERIOD_MS = 350 + uint16 PUBLISHER_TIMEOUT_MS = 10000 # UTC time in microseconds when the PREVIOUS GlobalTimeSync message was transmitted. diff --git a/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp index ba2b9e766f..ae1128205b 100644 --- a/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp +++ b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp @@ -24,6 +24,7 @@ class GlobalTimeSyncSlave : Noncopyable MonotonicTime last_adjustment_ts_; enum State { Update, Adjust } state_; NodeID master_nid_; + TransferID prev_tid_; uint8_t prev_iface_index_; ISystemClock& getSystemClock() const { return sub_.getNode().getSystemClock(); } @@ -48,6 +49,8 @@ public: bool isActive() const; NodeID getMasterNodeID() const; + + MonotonicTime getLastAdjustmentTime() const { return last_adjustment_ts_; } }; } diff --git a/libuavcan/src/protocol/global_time_sync_slave.cpp b/libuavcan/src/protocol/global_time_sync_slave.cpp index 3143a90856..08671eaeb9 100644 --- a/libuavcan/src/protocol/global_time_sync_slave.cpp +++ b/libuavcan/src/protocol/global_time_sync_slave.cpp @@ -32,27 +32,40 @@ void GlobalTimeSyncSlave::updateFromMsg(const ReceivedDataStructure& msg) { - const MonotonicDuration time_since_prev_sync = msg.getMonotonicTimestamp() - prev_ts_mono_; - assert(!time_since_prev_sync.isNegative()); + const MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_; + 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 timeout = time_since_prev_sync.toMSec() > protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS; + const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS; - if (switch_master || timeout || needs_init) + if (switch_master || pub_timeout || needs_init) { - UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i timeout=%i", - int(needs_init), int(switch_master), int(timeout)); + 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 && msg.prev_utc_usec > 0) + if (state_ == Adjust) + { + const bool msg_invalid = msg.prev_utc_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); }