diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index cfa7ffdb07..b22c382012 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -1586,6 +1586,9 @@ void AllocationRequestManager::broadcastIntermediateAllocationResponse() msg.unique_id = current_unique_id_; UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity()); + UAVCAN_TRACE("AllocationRequestManager", "Intermediate response with %u bytes of unique ID", + unsigned(msg.unique_id.size())); + const int res = allocation_pub_.broadcast(msg); if (res < 0) { @@ -1610,7 +1613,7 @@ void AllocationRequestManager::handleAllocation(const ReceivedDataStructure (last_message_timestamp_ + stage_timeout_)) { - UAVCAN_TRACE("AllocationRequestReceiver", "Stage timeout, reset"); + UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset"); current_unique_id_.clear(); } last_message_timestamp_ = msg.getMonotonicTimestamp(); @@ -1656,7 +1659,7 @@ void AllocationRequestManager::handleAllocation(const ReceivedDataStructure #include #include +#include #include "helpers.hpp" class StorageBackend : public uavcan::IDynamicNodeIDStorageBackend @@ -89,6 +90,47 @@ public: }; +class AllocationRequestHandler : public uavcan::dynamic_node_id_server_impl::IAllocationRequestHandler +{ + std::vector > requests_; + +public: + virtual void handleAllocationRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) + { + requests_.push_back(std::pair(unique_id, preferred_node_id)); + } + + bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) + { + if (requests_.empty()) + { + std::cout << "No pending requests" << std::endl; + return false; + } + + const std::pair pair = requests_.at(requests_.size() - 1U); + requests_.pop_back(); + + if (pair.first != unique_id) + { + std::cout << "Unique ID mismatch" << std::endl; + return false; + } + + if (pair.second != preferred_node_id) + { + std::cout << "Node ID mismatch (" << pair.second.get() << ", " << preferred_node_id.get() << ")" + << std::endl; + return false; + } + + return true; + } + + void reset() { requests_.clear(); } +}; + + static const unsigned NumEntriesInStorageWithEmptyLog = 4; // last index + 3 items per log entry @@ -942,7 +984,59 @@ TEST(DynamicNodeIDAllocationServer, EventCodeToString) TEST(DynamicNodeIDAllocationServer, AllocationRequestManager) { + using namespace uavcan::protocol::dynamic_node_id; + using namespace uavcan::protocol::dynamic_node_id::server; + using namespace uavcan::dynamic_node_id_server_impl; + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + uavcan::DynamicNodeIDAllocationClient client(nodes.b); + + /* + * Client initialization + */ + uavcan::protocol::HardwareVersion hwver; + for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++) + { + hwver.unique_id[i] = i; + } + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, client.start(hwver, PreferredNodeID)); + + /* + * Request manager initialization + */ + AllocationRequestHandler handler; + + AllocationRequestManager manager(nodes.a, handler); + + ASSERT_LE(0, manager.init()); + + ASSERT_FALSE(manager.isActive()); + manager.setActive(true); + ASSERT_TRUE(manager.isActive()); + + /* + * Allocation + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + ASSERT_TRUE(handler.matchAndPopLastRequest(hwver.unique_id, PreferredNodeID)); + + ASSERT_LE(0, manager.broadcastAllocationResponse(hwver.unique_id, PreferredNodeID)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + + /* + * Checking the client + */ + ASSERT_TRUE(client.isAllocationComplete()); + + ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); }