diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp new file mode 100644 index 0000000000..48b5284732 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class implements client-side logic of dynamic node ID allocation procedure. + * + * Once started, the object will be publishing dynamic node ID allocation requests at the maximum frequency allowed + * by the specification, until a Node ID is granted by the allocator. Note that if there are multiple responses, + * Node ID from the first response will be taken, and all subsequent responses will be ignored. + * + * If the local node is equipped with redundant CAN interfaces, all of them will be used for publishing requests + * and listening for responses. + * + * Once dynamic allocation is complete (or not needed anymore), the object can be deleted. + */ +class DynamicNodeIDAllocationClient : private TimerBase +{ + typedef MethodBinder&)> + DynamicNodeIDAllocationCallback; + + Publisher dnida_pub_; + Subscriber dnida_sub_; + + uint64_t short_unique_id_; + NodeID preferred_node_id_; + NodeID allocated_node_id_; + NodeID allocator_node_id_; + + virtual void handleTimerEvent(const TimerEvent&); + + void handleDynamicNodeIDAllocation(const ReceivedDataStructure& msg); + + int startImpl(); + +public: + DynamicNodeIDAllocationClient(INode& node) + : TimerBase(node) + , dnida_pub_(node) + , dnida_sub_(node) + , short_unique_id_(0) + { } + + /** + * Starts the client with a pre-computed short unique Node ID. + * @param short_unique_node_id The unique ID, only lower 57 bits of it will be used. + * @param preferred_node_id Node ID that the application would like to take; default is any. + * @return Zero on success + * Negative error code on failure + * -ErrLogic if the node is not in passive mode (i.e. allocation is meaningless) + * -ErrInvalidParam if the supplied short unique ID is invalid + */ + int start(uint64_t short_unique_node_id, NodeID preferred_node_id = NodeID::Broadcast); + + /** + * This overload computes a short unique Node ID using data from uavcan.protocol.HardwareVersion structure. + * @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; default is any. + * @return Refer to the overload for details + * -ErrInvalidParam if short unique ID could not be computed + */ + int start(const protocol::HardwareVersion& hardware_version, NodeID preferred_node_id = NodeID::Broadcast); + + /** + * This method allows to retrieve the value that was allocated to the local node. + * If no value was allocated yet, the returned Node ID will be invalid (non-unicast). + * Tip: use getAllocatedNodeID().isUnicast() to check if allocation is complete. + * @return If allocation is complete, a valid unicast Node ID will be returned. + * If allocation is not complete yet, an invalid Node ID will be returned. + */ + NodeID getAllocatedNodeID() const { return allocated_node_id_; } + + /** + * This method allows to retrieve node ID of the allocator that granted our Node ID. + * If no Node ID was allocated yet, the returned Node ID will be invalid (non-unicast). + * @return If allocation is complete, a valid unicast Node ID will be returned. + * If allocation is not complete yet, an invalid Node ID will be returned. + */ + NodeID getAllocatorNodeID() const { return allocator_node_id_; } + + /** + * This utility method simply returns the short node ID used in the allocation request messages. + * Returned value will be zero if the short unique node ID has not been initialized yet. + */ + uint64_t getShortUniqueNodeID() const { return short_unique_id_; } +}; + +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp new file mode 100644 index 0000000000..7a989c13a8 --- /dev/null +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp @@ -0,0 +1,138 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include // For CRC64 + +namespace uavcan +{ + +void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&) +{ + UAVCAN_ASSERT(!allocated_node_id_.isUnicast()); + UAVCAN_ASSERT(preferred_node_id_.isUnicast()); + + protocol::DynamicNodeIDAllocation msg; + msg.short_unique_id = short_unique_id_; + msg.node_id = preferred_node_id_.get(); + + UAVCAN_TRACE("DynamicNodeIDAllocation", "Broadcasting a request: short unique ID 0x%016llx, preferred ID %d", + static_cast(short_unique_id_), + static_cast(preferred_node_id_.get())); + + const int res = dnida_pub_.broadcast(msg); + if (res < 0) + { + dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocation pub failed"); + } +} + +void DynamicNodeIDAllocationClient::handleDynamicNodeIDAllocation( + const ReceivedDataStructure& msg) +{ + if ((msg.short_unique_id != short_unique_id_) || msg.isRogueTransfer()) + { + return; // Not ours + } + + if (allocated_node_id_.isUnicast()) + { + UAVCAN_TRACE("DynamicNodeIDAllocation", "Redundant response from %d", + static_cast(msg.getSrcNodeID().get())); + return; // Allocation is already done + } + + const NodeID allocation(msg.node_id); + if (!allocation.isUnicast()) + { + dnida_sub_.getNode().registerInternalFailure("DynamicNodeIDAllocation bad node ID allocated"); + return; + } + + allocated_node_id_ = allocation; + allocator_node_id_ = msg.getSrcNodeID(); + + UAVCAN_TRACE("DynamicNodeIDAllocation", "Allocation done: requested %d, received %d from %d", + static_cast(preferred_node_id_.get()), + static_cast(allocated_node_id_.get()), + static_cast(msg.getSrcNodeID().get())); + + TimerBase::stop(); +} + +int DynamicNodeIDAllocationClient::startImpl() +{ + short_unique_id_ &= protocol::DynamicNodeIDAllocation::FieldTypes::short_unique_id::mask(); + + if ((short_unique_id_ == 0) || !preferred_node_id_.isUnicast()) + { + // It's not like a zero unique ID is not unique enough, but it's surely suspicious + return -ErrInvalidParam; + } + + UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Short unique node ID: 0x%016llx, preferred node ID: %d", + static_cast(short_unique_id_), static_cast(preferred_node_id_.get())); + + allocated_node_id_ = NodeID(); + UAVCAN_ASSERT(!allocated_node_id_.isUnicast()); + UAVCAN_ASSERT(!allocated_node_id_.isValid()); + + int res = dnida_pub_.init(); + if (res < 0) + { + return res; + } + dnida_pub_.allowRogueTransfers(); + + res = dnida_sub_.start( + DynamicNodeIDAllocationCallback(this, &DynamicNodeIDAllocationClient::handleDynamicNodeIDAllocation)); + if (res < 0) + { + return res; + } + dnida_sub_.allowRogueTransfers(); + + startPeriodic( + MonotonicDuration::fromMSec(protocol::DynamicNodeIDAllocation::ALLOCATEE_MIN_BROADCAST_INTERVAL_SEC * 1000)); + + return 0; +} + +int DynamicNodeIDAllocationClient::start(const uint64_t short_unique_node_id, const NodeID preferred_node_id) +{ + short_unique_id_ = short_unique_node_id; + preferred_node_id_ = preferred_node_id; + return startImpl(); +} + +int DynamicNodeIDAllocationClient::start(const protocol::HardwareVersion& hardware_version, + const NodeID preferred_node_id) +{ + // Checking if unique ID is set + bool unique_id_is_zero = true; + for (uint8_t i = 0; i < hardware_version.unique_id.size(); i++) + { + if (hardware_version.unique_id[i] != 0) + { + unique_id_is_zero = false; + break; + } + } + + if (unique_id_is_zero) + { + return -ErrInvalidParam; + } + + // Reducing the ID to 64 bits according to the specification + DataTypeSignatureCRC crc; + crc.add(hardware_version.unique_id.begin(), hardware_version.unique_id.size()); + short_unique_id_ = crc.get(); + + preferred_node_id_ = preferred_node_id; + + return startImpl(); +} + +} diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp new file mode 100644 index 0000000000..0e86d84e54 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp @@ -0,0 +1,143 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +TEST(DynamicNodeIDAllocationClient, Basic) +{ + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + uavcan::protocol::HardwareVersion hwver; + + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver)); // Empty hardware version is not allowed + + /* + * Initializing. + * Reduced signature was calculated as follows: + * >>> crc = pyuavcan.dsdl.signature.Signature() + * >>> crc.add(range(16)) + * >>> crc.get_value() + * 4539764000456687298L + * >>> hex(crc.get_value()) + * '0x3f007b4e4353bec2L' + */ + const uavcan::uint64_t UniqueID64Bit = 0x3f007b4e4353bec2ULL; + const uavcan::uint64_t UniqueID57Bit = UniqueID64Bit & ((1ULL << 57) - 1U); + for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++) + { + hwver.unique_id[i] = i; + } + + // More incorrect inputs + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(0)); + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver, uavcan::NodeID::Broadcast)); // Bad node ID + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver, uavcan::NodeID())); // Ditto + + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, dnidac.start(hwver, PreferredNodeID)); + + // Making sure the signature reduction was performed correctly + ASSERT_EQ(UniqueID57Bit, dnidac.getShortUniqueNodeID()); + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + + /* + * Initializing subscriber + * Rogue transfers must be enabled + */ + SubscriberWithCollector dynid_sub(nodes.a); + ASSERT_LE(0, dynid_sub.start()); + dynid_sub.subscriber.allowRogueTransfers(); + + /* + * Monitoring requests at 1Hz + * First request will be sent immediately + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + ASSERT_TRUE(dynid_sub.collector.msg.get()); + ASSERT_EQ(UniqueID57Bit, dynid_sub.collector.msg->short_unique_id); + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + dynid_sub.collector.msg.reset(); + + // Rate validation + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(dynid_sub.collector.msg.get()); + + // Second - rate is 1 Hz + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(900)); + ASSERT_TRUE(dynid_sub.collector.msg.get()); + ASSERT_EQ(UniqueID57Bit, dynid_sub.collector.msg->short_unique_id); + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + dynid_sub.collector.msg.reset(); + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + + /* + * Sending a bunch of responses + * Note that response transfers are NOT rogue + */ + uavcan::Publisher dynid_pub(nodes.a); + ASSERT_LE(0, dynid_pub.init()); + + uavcan::protocol::DynamicNodeIDAllocation msg; + + msg.short_unique_id = 123; // garbage + msg.node_id = 100; // oh whatever + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); + + msg.short_unique_id = 0; // garbage + msg.node_id = 101; + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); + + msg.short_unique_id = UniqueID57Bit; // correct ID + msg.node_id = 102; // THIS NODE ID WILL BE USED + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); + + msg.short_unique_id = UniqueID57Bit; // repeating, will be ignored + msg.node_id = 103; + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); + + /* + * Validating the results + */ + ASSERT_FALSE(dynid_sub.collector.msg.get()); + + ASSERT_TRUE(dnidac.getAllocatedNodeID().isUnicast()); + ASSERT_TRUE(dnidac.getAllocatorNodeID().isUnicast()); + + ASSERT_EQ(102, dnidac.getAllocatedNodeID().get()); + ASSERT_EQ(10, dnidac.getAllocatorNodeID().get()); + + // Making sure requests have stopped + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + ASSERT_FALSE(dynid_sub.collector.msg.get()); +} + + +TEST(DynamicNodeIDAllocationClient, NonPassiveMode) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + ASSERT_LE(-uavcan::ErrLogic, dnidac.start(123456789)); +}