diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp index a4ed933109..a54bf4632f 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp @@ -25,6 +25,8 @@ namespace uavcan * and listening for responses. * * Once dynamic allocation is complete (or not needed anymore), the object can be deleted. + * + * Note that this class uses std::rand(), which must be correctly seeded before use. */ class UAVCAN_EXPORT DynamicNodeIDClient : private TimerBase { @@ -33,16 +35,29 @@ class UAVCAN_EXPORT DynamicNodeIDClient : private TimerBase (const ReceivedDataStructure&)> AllocationCallback; + enum Mode + { + ModeWaitingForTimeSlot, + ModeDelayBeforeFollowup, + NumModes + }; + Publisher dnida_pub_; Subscriber dnida_sub_; uint8_t unique_id_[protocol::HardwareVersion::FieldTypes::unique_id::MaxSize]; + uint8_t size_of_received_unique_id_; + NodeID preferred_node_id_; NodeID allocated_node_id_; NodeID allocator_node_id_; void terminate(); + static MonotonicDuration getRandomDuration(uint32_t lower_bound_msec, uint32_t upper_bound_msec); + + void restartTimer(const Mode mode); + virtual void handleTimerEvent(const TimerEvent&); void handleAllocation(const ReceivedDataStructure& msg); @@ -52,6 +67,7 @@ public: : TimerBase(node) , dnida_pub_(node) , dnida_sub_(node) + , size_of_received_unique_id_(0) { } /** diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp index 29a5751e42..b07dfb491f 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp @@ -2,6 +2,7 @@ * Copyright (C) 2015 Pavel Kirienko */ +#include #include namespace uavcan @@ -14,10 +15,36 @@ void DynamicNodeIDClient::terminate() dnida_sub_.stop(); } +MonotonicDuration DynamicNodeIDClient::getRandomDuration(uint32_t lower_bound_msec, uint32_t upper_bound_msec) +{ + UAVCAN_ASSERT(upper_bound_msec > lower_bound_msec); + // coverity[dont_call] + return MonotonicDuration::fromMSec(lower_bound_msec + + static_cast(std::rand()) % (upper_bound_msec - lower_bound_msec)); +} + +void DynamicNodeIDClient::restartTimer(const Mode mode) +{ + UAVCAN_ASSERT(mode < NumModes); + UAVCAN_ASSERT((mode == ModeWaitingForTimeSlot) == (size_of_received_unique_id_ == 0)); + + const MonotonicDuration delay = (mode == ModeWaitingForTimeSlot) ? + getRandomDuration(protocol::dynamic_node_id::Allocation::MIN_REQUEST_PERIOD_MS, + protocol::dynamic_node_id::Allocation::MAX_REQUEST_PERIOD_MS) : + getRandomDuration(protocol::dynamic_node_id::Allocation::MIN_FOLLOWUP_DELAY_MS, + protocol::dynamic_node_id::Allocation::MAX_FOLLOWUP_DELAY_MS); + + startOneShotWithDelay(delay); + + UAVCAN_TRACE("DynamicNodeIDClient", "Restart mode %d, delay %d ms", + static_cast(mode), static_cast(delay.toMSec())); +} + void DynamicNodeIDClient::handleTimerEvent(const TimerEvent&) { - // This method implements Rule B UAVCAN_ASSERT(preferred_node_id_.isValid()); + UAVCAN_ASSERT(size_of_received_unique_id_ < protocol::dynamic_node_id::Allocation::FieldTypes::unique_id::MaxSize); + if (isAllocationComplete()) { UAVCAN_ASSERT(0); @@ -25,19 +52,37 @@ void DynamicNodeIDClient::handleTimerEvent(const TimerEvent&) return; } - // Filling the message - protocol::dynamic_node_id::Allocation msg; - msg.node_id = preferred_node_id_.get(); - msg.first_part_of_unique_id = true; + /* + * Filling the message. + */ + protocol::dynamic_node_id::Allocation tx; + tx.node_id = preferred_node_id_.get(); + tx.first_part_of_unique_id = (size_of_received_unique_id_ == 0); - msg.unique_id.resize(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST); - copy(unique_id_, unique_id_ + msg.unique_id.size(), msg.unique_id.begin()); - UAVCAN_ASSERT(equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_)); + const uint8_t size_of_unique_id_in_request = + min(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST, + static_cast(tx.unique_id.capacity() - size_of_received_unique_id_)); - // Broadcasting - UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting 1st stage: preferred ID: %d", - static_cast(preferred_node_id_.get())); - const int res = dnida_pub_.broadcast(msg); + tx.unique_id.resize(size_of_unique_id_in_request); + copy(unique_id_ + size_of_received_unique_id_, + unique_id_ + size_of_received_unique_id_ + size_of_unique_id_in_request, + tx.unique_id.begin()); + + UAVCAN_ASSERT(equal(tx.unique_id.begin(), tx.unique_id.end(), unique_id_ + size_of_received_unique_id_)); + + /* + * Resetting the state - this way we can continue with a first stage request on the next attempt. + */ + size_of_received_unique_id_ = 0; + restartTimer(ModeWaitingForTimeSlot); + + /* + * Broadcasting the message. + */ + UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting; preferred ID %d, size of UID %d", + static_cast(preferred_node_id_.get()), + static_cast(tx.unique_id.size())); + const int res = dnida_pub_.broadcast(tx); if (res < 0) { dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed"); @@ -46,9 +91,6 @@ void DynamicNodeIDClient::handleTimerEvent(const TimerEvent&) void DynamicNodeIDClient::handleAllocation(const ReceivedDataStructure& msg) { - /* - * TODO This method can blow the stack easily - */ UAVCAN_ASSERT(preferred_node_id_.isValid()); if (isAllocationComplete()) { @@ -57,57 +99,48 @@ void DynamicNodeIDClient::handleAllocation(const ReceivedDataStructure(msg.getSrcNodeID().get())); + UAVCAN_TRACE("DynamicNodeIDClient", "Allocation message from %d, %d bytes of unique ID, node ID %d", + static_cast(msg.getSrcNodeID().get()), static_cast(msg.unique_id.size()), + static_cast(msg.node_id)); - // Rule D - if (!msg.isAnonymousTransfer() && - msg.unique_id.size() > 0 && - msg.unique_id.size() < msg.unique_id.capacity() && - equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_)) + /* + * Switching to passive state by default; will switch to active state if response matches. + */ + size_of_received_unique_id_ = 0; + restartTimer(ModeWaitingForTimeSlot); + + /* + * Filtering out anonymous and invalid messages. + */ + const bool full_response = (msg.unique_id.size() == msg.unique_id.capacity()); + + if (msg.isAnonymousTransfer() || + msg.unique_id.empty() || + (full_response && (msg.node_id == 0))) { - // Filling the response message - const uint8_t size_of_unique_id_in_response = - min(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST, - static_cast(msg.unique_id.capacity() - msg.unique_id.size())); - - protocol::dynamic_node_id::Allocation second_stage; - second_stage.node_id = preferred_node_id_.get(); - second_stage.first_part_of_unique_id = false; - - second_stage.unique_id.resize(size_of_unique_id_in_response); - - copy(unique_id_ + msg.unique_id.size(), - unique_id_ + msg.unique_id.size() + size_of_unique_id_in_response, - second_stage.unique_id.begin()); - - UAVCAN_ASSERT(equal(second_stage.unique_id.begin(), - second_stage.unique_id.end(), - unique_id_ + msg.unique_id.size())); - - // Broadcasting the response - UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting 2nd stage: preferred ID: %d, size of unique ID: %d", - static_cast(preferred_node_id_.get()), static_cast(second_stage.unique_id.size())); - const int res = dnida_pub_.broadcast(second_stage); - if (res < 0) - { - dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed"); - } + UAVCAN_TRACE("DynamicNodeIDClient", "Message from %d ignored", static_cast(msg.getSrcNodeID().get())); + return; } - // Rule E - if (!msg.isAnonymousTransfer() && - msg.unique_id.size() == msg.unique_id.capacity() && - equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_) && - msg.node_id > 0) + /* + * If matches, either switch to active mode or complete the allocation. + */ + if (equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_)) { - allocated_node_id_ = msg.node_id; - allocator_node_id_ = msg.getSrcNodeID(); - UAVCAN_TRACE("DynamicNodeIDClient", "Allocation complete, node ID %d provided by %d", - static_cast(allocated_node_id_.get()), static_cast(allocator_node_id_.get())); - terminate(); - UAVCAN_ASSERT(isAllocationComplete()); + if (full_response) + { + allocated_node_id_ = msg.node_id; + allocator_node_id_ = msg.getSrcNodeID(); + terminate(); + UAVCAN_ASSERT(isAllocationComplete()); + UAVCAN_TRACE("DynamicNodeIDClient", "Allocation complete, node ID %d provided by %d", + static_cast(allocated_node_id_.get()), static_cast(allocator_node_id_.get())); + } + else + { + size_of_received_unique_id_ = msg.unique_id.size(); + restartTimer(ModeDelayBeforeFollowup); + } } } @@ -169,7 +202,7 @@ int DynamicNodeIDClient::start(const protocol::HardwareVersion& hardware_version } dnida_sub_.allowAnonymousTransfers(); - startPeriodic(MonotonicDuration::fromMSec(1000 /* TODO FIXME */)); + restartTimer(ModeWaitingForTimeSlot); return 0; } diff --git a/libuavcan/test/protocol/dynamic_node_id_client.cpp b/libuavcan/test/protocol/dynamic_node_id_client.cpp index c9c327ece7..8a2b672737 100644 --- a/libuavcan/test/protocol/dynamic_node_id_client.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_client.cpp @@ -92,16 +92,17 @@ TEST(DynamicNodeIDClient, Basic) /* * Responding with partially matching unique ID - the client will respond with second-stage request immediately */ + const uint8_t BytesPerRequest = uavcan::protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST; { uavcan::protocol::dynamic_node_id::Allocation msg; - msg.unique_id.resize(7); - uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.begin() + 7, msg.unique_id.begin()); + msg.unique_id.resize(BytesPerRequest); + uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.begin() + BytesPerRequest, msg.unique_id.begin()); std::cout << "First-stage offer:\n" << msg << std::endl; ASSERT_FALSE(dynid_sub.collector.msg.get()); ASSERT_LE(0, dynid_pub.broadcast(msg)); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500)); ASSERT_TRUE(dynid_sub.collector.msg.get()); std::cout << "Second-stage request:\n" << *dynid_sub.collector.msg << std::endl; @@ -109,7 +110,7 @@ TEST(DynamicNodeIDClient, Basic) ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id); ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), dynid_sub.collector.msg->unique_id.end(), - hwver.unique_id.begin() + 7)); + hwver.unique_id.begin() + BytesPerRequest)); dynid_sub.collector.msg.reset(); } @@ -118,14 +119,14 @@ TEST(DynamicNodeIDClient, Basic) */ { uavcan::protocol::dynamic_node_id::Allocation msg; - msg.unique_id.resize(14); - uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.begin() + 14, msg.unique_id.begin()); + msg.unique_id.resize(BytesPerRequest * 2); + uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.begin() + BytesPerRequest * 2, msg.unique_id.begin()); std::cout << "Second-stage offer:\n" << msg << std::endl; ASSERT_FALSE(dynid_sub.collector.msg.get()); ASSERT_LE(0, dynid_pub.broadcast(msg)); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500)); ASSERT_TRUE(dynid_sub.collector.msg.get()); std::cout << "Last request:\n" << *dynid_sub.collector.msg << std::endl; @@ -133,7 +134,7 @@ TEST(DynamicNodeIDClient, Basic) ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id); ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), dynid_sub.collector.msg->unique_id.end(), - hwver.unique_id.begin() + 14)); + hwver.unique_id.begin() + BytesPerRequest * 2)); dynid_sub.collector.msg.reset(); }