From 20feaba1de3a426492e808be0e4b462cff6c6fcd Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 30 Apr 2015 14:19:52 +0300 Subject: [PATCH] Using transfer priorities in protocol:: classes --- libuavcan/include/uavcan/node/node.hpp | 11 +++++++---- libuavcan/include/uavcan/node/publisher.hpp | 14 +++++++++----- .../protocol/dynamic_node_id_allocation_client.hpp | 5 ++++- .../uavcan/protocol/node_status_provider.hpp | 2 +- .../uc_dynamic_node_id_allocation_client.cpp | 8 +++++++- .../src/protocol/uc_global_time_sync_master.cpp | 6 ++++++ libuavcan/src/protocol/uc_logger.cpp | 7 ++++++- libuavcan/src/protocol/uc_node_status_provider.cpp | 8 +++++++- 8 files changed, 47 insertions(+), 14 deletions(-) diff --git a/libuavcan/include/uavcan/node/node.hpp b/libuavcan/include/uavcan/node/node.hpp index ad6cfd5b8d..930ecf419d 100644 --- a/libuavcan/include/uavcan/node/node.hpp +++ b/libuavcan/include/uavcan/node/node.hpp @@ -145,8 +145,10 @@ public: * Once started, the node can't stop. * If the node failed to start up, it's recommended to destroy the current node instance and start over. * Returns negative error code. + * @param node_status_transfer_priority Transfer priority that will be used for outgoing NodeStatus messages. + * Normal priority is used by default. */ - int start(); + int start(const TransferPriority node_status_transfer_priority = TransferPriorityNormal); #if !UAVCAN_TINY /** @@ -260,7 +262,8 @@ public: template -int Node::start() +int Node::start( + const TransferPriority priority) { if (started_) { @@ -274,7 +277,7 @@ int Node::check(); + StaticAssert<(sizeof(checker) < 2048)>::check(); // Making sure that the code footprint doesn't explode res = checker.execute(); result = checker.getResult(); return res; diff --git a/libuavcan/include/uavcan/node/publisher.hpp b/libuavcan/include/uavcan/node/publisher.hpp index ea86714631..4b95313d24 100644 --- a/libuavcan/include/uavcan/node/publisher.hpp +++ b/libuavcan/include/uavcan/node/publisher.hpp @@ -100,22 +100,26 @@ public: /** * Allows to change the priority of outgoing transfers. * Note that only High, Normal and Low priorities can be used; Service priority is not available for messages. - * Attempt to use Service priority will result in assertion failure in debug builds, and it will have no - * effect in release builds. + * Returns negative error code if priority cannot be set, non-negative on success. */ - void setPriority(const TransferPriority prio) + int setPriority(const TransferPriority prio) { if (prio < NumTransferPriorities && prio != TransferPriorityService) { - TransferSender* const ts = getTransferSender(); + TransferSender* const ts = getTransferSender(); // TODO: Static TransferSender? if (ts != NULL) { ts->setPriority(prio); + return 0; + } + else + { + return -ErrLogic; } } else { - UAVCAN_ASSERT(0); + return -ErrInvalidParam; } } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp index 1f11f46663..0833ffe24e 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp @@ -58,11 +58,14 @@ public: * @param hardware_version Hardware version information, where unique_id must be set correctly. * @param preferred_node_id Node ID that the application would like to take; set to broadcast (zero) if * the application doesn't have any preference (this is default). + * @param transfer_priority Transfer priority, Normal by default. * @return Zero on success * Negative error code on failure * -ErrLogic if 1. the node is not in passive mode or 2. the client is already started */ - int start(const protocol::HardwareVersion& hardware_version, const NodeID preferred_node_id = NodeID::Broadcast); + int start(const protocol::HardwareVersion& hardware_version, + const NodeID preferred_node_id = NodeID::Broadcast, + const TransferPriority transfer_priority = TransferPriorityNormal); /** * Use this method to determine when allocation is complete. diff --git a/libuavcan/include/uavcan/protocol/node_status_provider.hpp b/libuavcan/include/uavcan/protocol/node_status_provider.hpp index 6b06fa76dd..107aaf91c0 100644 --- a/libuavcan/include/uavcan/protocol/node_status_provider.hpp +++ b/libuavcan/include/uavcan/protocol/node_status_provider.hpp @@ -68,7 +68,7 @@ public: * Starts the provider and immediately broadcasts uavcan.protocol.NodeStatus. * Returns negative error code. */ - int startAndPublish(); + int startAndPublish(TransferPriority priority = TransferPriorityNormal); /** * Publish the message uavcan.protocol.NodeStatus right now, out of schedule. diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp index 5b3b4e571c..915c4b3b56 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp @@ -114,7 +114,8 @@ void DynamicNodeIDAllocationClient::handleAllocation( } int DynamicNodeIDAllocationClient::start(const protocol::HardwareVersion& hardware_version, - const NodeID preferred_node_id) + const NodeID preferred_node_id, + const TransferPriority transfer_priority) { terminate(); @@ -161,6 +162,11 @@ int DynamicNodeIDAllocationClient::start(const protocol::HardwareVersion& hardwa return res; } dnida_pub_.allowAnonymousTransfers(); + res = dnida_pub_.setPriority(transfer_priority); + if (res < 0) + { + return res; + } res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDAllocationClient::handleAllocation)); if (res < 0) diff --git a/libuavcan/src/protocol/uc_global_time_sync_master.cpp b/libuavcan/src/protocol/uc_global_time_sync_master.cpp index 8825c1ba85..6d90cf2eb6 100644 --- a/libuavcan/src/protocol/uc_global_time_sync_master.cpp +++ b/libuavcan/src/protocol/uc_global_time_sync_master.cpp @@ -24,6 +24,12 @@ int GlobalTimeSyncMaster::IfaceMaster::init() 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; } diff --git a/libuavcan/src/protocol/uc_logger.cpp b/libuavcan/src/protocol/uc_logger.cpp index 3c6661e306..241267ac50 100644 --- a/libuavcan/src/protocol/uc_logger.cpp +++ b/libuavcan/src/protocol/uc_logger.cpp @@ -17,7 +17,12 @@ Logger::LogLevel Logger::getExternalSinkLevel() const int Logger::init() { - return logmsg_pub_.init(); + const int res = logmsg_pub_.init(); + if (res < 0) + { + return res; + } + return logmsg_pub_.setPriority(TransferPriorityLow); // Fixed priority } int Logger::log(const protocol::debug::LogMessage& message) diff --git a/libuavcan/src/protocol/uc_node_status_provider.cpp b/libuavcan/src/protocol/uc_node_status_provider.cpp index d83b48f191..bd090adb10 100644 --- a/libuavcan/src/protocol/uc_node_status_provider.cpp +++ b/libuavcan/src/protocol/uc_node_status_provider.cpp @@ -61,7 +61,7 @@ void NodeStatusProvider::handleGetNodeInfoRequest(const protocol::GetNodeInfo::R rsp = node_info_; } -int NodeStatusProvider::startAndPublish() +int NodeStatusProvider::startAndPublish(TransferPriority priority) { if (!isNodeInfoInitialized()) { @@ -71,6 +71,12 @@ int NodeStatusProvider::startAndPublish() int res = -1; + res = node_status_pub_.setPriority(priority); + if (res < 0) + { + goto fail; + } + if (!getNode().isPassiveMode()) { res = publish();