diff --git a/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp index e658b33730..4b69451a0a 100644 --- a/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp +++ b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp @@ -5,11 +5,15 @@ #ifndef UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED #define UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED +#include #include #include #include #include #include +#include +#include +#include namespace uavcan { @@ -42,11 +46,61 @@ class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase UAVCAN_ASSERT(iface_index < MaxCanIfaces); } - int init(); + int init() + { + const int res = pub_.init(); + if (res >= 0) + { + TransferSender* const ts = pub_.getTransferSender(); + UAVCAN_ASSERT(ts != NULL); + ts->setIfaceMask(uint8_t(1 << iface_index_)); + ts->setCanIOFlags(CanIOFlagLoopback); - void setTxTimestamp(UtcTime ts); + const int prio_res = pub_.setPriority(TransferPriorityHigh); // Fixed priority + if (prio_res < 0) + { + return prio_res; + } + } + return res; + } - int publish(TransferID tid, MonotonicTime current_time); + void setTxTimestamp(UtcTime ts) + { + if (ts.isZero()) + { + UAVCAN_ASSERT(0); + pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster zero TX ts"); + return; + } + if (!prev_tx_utc_.isZero()) + { + prev_tx_utc_ = UtcTime(); // Reset again, because there's something broken in the driver and we don't trust it + pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster pub conflict"); + return; + } + prev_tx_utc_ = ts; + } + + int publish(TransferID tid, MonotonicTime current_time) + { + UAVCAN_ASSERT(pub_.getTransferSender()->getCanIOFlags() == CanIOFlagLoopback); + UAVCAN_ASSERT(pub_.getTransferSender()->getIfaceMask() == (1 << iface_index_)); + + const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_; + iface_prev_pub_mono_ = current_time; + UAVCAN_ASSERT(since_prev_pub.isPositive()); + const bool long_period = since_prev_pub.toMSec() >= protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS; + + protocol::GlobalTimeSync msg; + msg.previous_transmission_timestamp_usec = long_period ? 0 : prev_tx_utc_.toUSec(); + prev_tx_utc_ = UtcTime(); + + UAVCAN_TRACE("GlobalTimeSyncMaster", "Publishing %llu iface=%i tid=%i", + static_cast(msg.previous_transmission_timestamp_usec), + int(iface_index_), int(tid.get())); + return pub_.broadcast(msg, tid); + } }; INode& node_; @@ -55,9 +109,43 @@ class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase DataTypeID dtid_; bool initialized_; - virtual void handleLoopbackFrame(const RxFrame& frame); + virtual void 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() && + frame.getSrcNodeID() == node_.getNodeID()) + { + iface_masters_[iface]->setTxTimestamp(frame.getUtcTimestamp()); + } + } + else + { + UAVCAN_ASSERT(0); + } + } - int getNextTransferID(TransferID& tid); + int getNextTransferID(TransferID& tid) + { + const MonotonicDuration max_transfer_interval = + MonotonicDuration::fromMSec(protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS); + + const OutgoingTransferRegistryKey otr_key(dtid_, TransferTypeMessageBroadcast, NodeID::Broadcast); + const MonotonicTime otr_deadline = node_.getMonotonicTime() + max_transfer_interval; + TransferID* const tid_ptr = + node_.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); + if (tid_ptr == NULL) + { + return -ErrMemory; + } + + tid = *tid_ptr; + tid_ptr->increment(); + return 0; + } public: explicit GlobalTimeSyncMaster(INode& node) @@ -71,7 +159,45 @@ public: * Must be called before the master can be used. * Returns negative error code. */ - int init(); + int init() + { + if (initialized_) + { + return 0; + } + + // Data type ID + const DataTypeDescriptor* const desc = + GlobalDataTypeRegistry::instance().find(DataTypeKindMessage, protocol::GlobalTimeSync::getDataTypeFullName()); + if (desc == NULL) + { + return -ErrUnknownDataType; + } + dtid_ = desc->getID(); + + // Iface master array + int res = -ErrLogic; + 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; + } /** * Whether the master instance has been initialized. @@ -89,7 +215,54 @@ public: * * Returns negative error code. */ - int publish(); + int publish() + { + if (!initialized_) + { + const int res = init(); + if (res < 0) + { + return res; + } + } + + /* + * Enforce max frequency + */ + const MonotonicTime current_time = node_.getMonotonicTime(); + { + const MonotonicDuration since_prev_pub = current_time - prev_pub_mono_; + UAVCAN_ASSERT(since_prev_pub.isPositive()); + if (since_prev_pub.toMSec() < protocol::GlobalTimeSync::MIN_PUBLICATION_PERIOD_MS) + { + UAVCAN_TRACE("GlobalTimeSyncMaster", "Publication skipped"); + return 0; + } + prev_pub_mono_ = current_time; + } + + /* + * Obtain common Transfer ID for all masters + */ + TransferID tid; + { + const int tid_res = getNextTransferID(tid); + if (tid_res < 0) + { + return tid_res; + } + } + + for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) + { + const int res = iface_masters_[i]->publish(tid, current_time); + if (res < 0) + { + return res; + } + } + return 0; + } }; } diff --git a/libuavcan/src/protocol/uc_global_time_sync_master.cpp b/libuavcan/src/protocol/uc_global_time_sync_master.cpp deleted file mode 100644 index 6d90cf2eb6..0000000000 --- a/libuavcan/src/protocol/uc_global_time_sync_master.cpp +++ /dev/null @@ -1,206 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#if !UAVCAN_TINY - -#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(); - UAVCAN_ASSERT(ts != NULL); - ts->setIfaceMask(uint8_t(1 << iface_index_)); - ts->setCanIOFlags(CanIOFlagLoopback); - - const int prio_res = pub_.setPriority(TransferPriorityHigh); // Fixed priority - if (prio_res < 0) - { - return prio_res; - } - } - return res; -} - -void GlobalTimeSyncMaster::IfaceMaster::setTxTimestamp(UtcTime ts) -{ - if (ts.isZero()) - { - UAVCAN_ASSERT(0); - pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster zero TX ts"); - return; - } - if (!prev_tx_utc_.isZero()) - { - prev_tx_utc_ = UtcTime(); // Reset again, because there's something broken in the driver and we don't trust it - pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster pub conflict"); - return; - } - prev_tx_utc_ = ts; -} - -int GlobalTimeSyncMaster::IfaceMaster::publish(TransferID tid, MonotonicTime current_time) -{ - UAVCAN_ASSERT(pub_.getTransferSender()->getCanIOFlags() == CanIOFlagLoopback); - UAVCAN_ASSERT(pub_.getTransferSender()->getIfaceMask() == (1 << iface_index_)); - - const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_; - iface_prev_pub_mono_ = current_time; - UAVCAN_ASSERT(since_prev_pub.isPositive()); - const bool long_period = since_prev_pub.toMSec() >= protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS; - - protocol::GlobalTimeSync msg; - msg.previous_transmission_timestamp_usec = long_period ? 0 : prev_tx_utc_.toUSec(); - prev_tx_utc_ = UtcTime(); - - UAVCAN_TRACE("GlobalTimeSyncMaster", "Publishing %llu iface=%i tid=%i", - static_cast(msg.previous_transmission_timestamp_usec), - int(iface_index_), int(tid.get())); - return pub_.broadcast(msg, tid); -} - -/* - * 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() && - frame.getSrcNodeID() == node_.getNodeID()) - { - iface_masters_[iface]->setTxTimestamp(frame.getUtcTimestamp()); - } - } - else - { - UAVCAN_ASSERT(0); - } -} - -int GlobalTimeSyncMaster::getNextTransferID(TransferID& tid) -{ - const MonotonicDuration max_transfer_interval = - MonotonicDuration::fromMSec(protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS); - - const OutgoingTransferRegistryKey otr_key(dtid_, TransferTypeMessageBroadcast, NodeID::Broadcast); - const MonotonicTime otr_deadline = node_.getMonotonicTime() + max_transfer_interval; - TransferID* const tid_ptr = - node_.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); - if (tid_ptr == NULL) - { - return -ErrMemory; - } - - tid = *tid_ptr; - tid_ptr->increment(); - return 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 -ErrUnknownDataType; - } - dtid_ = desc->getID(); - - // Iface master array - int res = -ErrLogic; - 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; - } - } - - /* - * Enforce max frequency - */ - const MonotonicTime current_time = node_.getMonotonicTime(); - { - const MonotonicDuration since_prev_pub = current_time - prev_pub_mono_; - UAVCAN_ASSERT(since_prev_pub.isPositive()); - if (since_prev_pub.toMSec() < protocol::GlobalTimeSync::MIN_PUBLICATION_PERIOD_MS) - { - UAVCAN_TRACE("GlobalTimeSyncMaster", "Publication skipped"); - return 0; - } - prev_pub_mono_ = current_time; - } - - /* - * Obtain common Transfer ID for all masters - */ - TransferID tid; - { - const int tid_res = getNextTransferID(tid); - if (tid_res < 0) - { - return tid_res; - } - } - - for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) - { - const int res = iface_masters_[i]->publish(tid, current_time); - if (res < 0) - { - return res; - } - } - return 0; -} - -} - -#endif