New dynamic node ID allocation client

dsdl/uavcan/protocol/559.DynamicNodeIDAllocation.uavcan

@@ -1,147 +0,0 @@
-#
-# This message is used for dynamic Node ID allocation. This algorithm is an
-# optional extension to the UAVCAN specification, and it is not mandatory to
-# support it.
-#
-# The dynamic Node ID allocation algorithm works as follows:
-#
-# - As a prerequisite, the network contains at least one node that has some
-#   Node ID assigned, that will handle allocation requests from other nodes.
-#   This node will be later referred to as Allocator. It is recognized that
-#   since UAVCAN is a decentralized network, there might be more than one
-#   Allocator, but only one of them should handle allocation requests at any
-#   given time. This issue is covered below in detail.
-#
-# - The network may also contain any number of nodes that seek to have
-#   Node IDs assigned to them. In order to get a Node ID assigned, these nodes
-#   make a request to the Allocator. Requesting nodes will be referred to as
-#   Allocatees.
-#
-# - In order to get a Node ID allocated, the Allocatee fills this message
-#   structure with its short unique ID (see below to learn how to compute one)
-#   and its preferred Node ID (which is basically the Node ID it would like to
-#   take, if possible). If the Allocatee doesn't have any preference, the
-#   preferred Node ID can be set to zero.
-#
-# - The Allocatee broadcasts the message to the bus. Since it doesn't have a
-#   Node ID at the moment, the field Source Node ID in the broadcasted message
-#   must be zero. Note that it may require special logic in the transport
-#   layer to receive messages where Source Node ID is zeroed.
-#
-# - The Allocator receives the message and refers to its Node ID Table, where
-#   it looks for any unallocated Node ID value that equal to or greater than
-#   the value that was requested by the Allocatee. If the desired Node ID is
-#   set to zero, or if all Node IDs that are larger than the preferred one are
-#   already taken, the Allocator traverses the Node ID Table from top to
-#   bottom (i.e. from high to low Node ID) until it finds first unallocated
-#   entry. In a case if all available entires are taken, the Allocator ignores
-#   the request. Please note that it is recommended to restrict the range of
-#   dynamically assignable Node IDs, as described below.
-#
-# - The Allocator fills the response message. The short unique ID is set the
-#   same as in the request, the Node ID field is assigned the value obtained
-#   on the previous step. When filled, the message is broadcasted as usual,
-#   i.e. the field Source Node ID must be set to the actual Node ID of the
-#   Allocator. This allows other Allocators, if any, to understand that this
-#   message is not an allocation request, otherwise Source Node ID would be
-#   zero.
-#
-# - The Allocatee receives the response. Sice there can be other nodes trying
-#   to get a Node ID at the same time, the Allocatee filters other messages
-#   by means of comparing its own short unique ID with short unique ID in the
-#   messages passing by. When a message is received such that its short unique
-#   ID equals the short unique ID of the Allocatee, the Allocatee gets the
-#   allocated Node ID from the message, accepts it and continues its normal
-#   operation as a full-fledged UAVCAN node.
-#
-# - It is recommended that once Allocatee was granted a Node ID, it stops
-#   listening to other messages of this type and reconfigures its CAN
-#   acceptance filters accordingly.
-#
-# It has been mentioned above that a network may accomodate more than one
-# Allocator at the time. In this case, the Allocators should agree between
-# each other that only one of them is handling allocation requests at the
-# moment. A possible way to reach such an agreement is to let one Allocator
-# monitor the presence of the second one (e.g. by means of listening to its
-# NodeStatus messages), so that the second Allocator will ignore allocation
-# requests as long as the first one is alive.
-#
-# Every Allocator must continuously maintain a table that indicates which
-# Node IDs are taken, and which are free. The logic is like that: immediately
-# after initialization, only one Node ID is considered to be taken - the one
-# that belongs to the Allocator itself. The allocator subscribes to the
-# messages uavcan.protocol.NodeStatus and this one, that are then used to
-# update the table in real time - once a NodeStatus or this message are
-# received, the table is updated to indicate that the Node ID that sent the
-# NodeStatus message is taken. The Allocator must not process allocation
-# requests sooner than a few seconds after its initialization, in order to
-# ensure that all nodes that exist in the network were able to transmit their
-# NodeStatus message at least once. The exact duration of initialization delay
-# is defined in a constant below.
-#
-# In order to minimize chances of Node ID collision between nodes that rely on
-# the dynamic allocation and the nodes that use statically allocated Node IDs,
-# it is recommended to restrict the range of dynamically allocatable Node IDs.
-# Two constants below define a range for dynamically allocated Node ID.
-#
-# There is a limit that restricts the maximum frequency at which the Allocatee
-# is allowed to broadcast allocation requests. It is defined in a constant
-# below.
-#
-# Fields of this message are documented below.
-#
-
-#
-# This is the delay that Allocator must take to initialize its Node ID Table.
-#
-uint8 ALLOCATOR_INITIALIZATION_DELAY_SEC = 10
-
-#
-# This is the minimum interval (maximum frequency) at which the Allocatee can
-# broadcast allocation requests.
-#
-uint8 ALLOCATEE_MIN_BROADCAST_INTERVAL_SEC = 1
-
-#
-# It is recommended to pick Node IDs from this range only, even if the
-# Allocatee requests a Node ID that is outside of this range.
-#
-uint7 RECOMMENDED_DYNAMIC_NODE_ID_RANGE_MIN = 64
-uint7 RECOMMENDED_DYNAMIC_NODE_ID_RANGE_MAX = 125
-
-#
-# This field contains 57 bits that uniquely identify the node that requests
-# Node ID allocation. The specification does not define exact algorithm for
-# computing short unique ID, but it is recommended to stick to the following
-# recommendations:
-#
-# 1. If the node's unique ID is 64 bit large, lower 57 bits should be used.
-#
-# 2. If the node's unique ID is larger than 64 bits, it should be reduced
-#    to 64 bit using the CRC-64-WE hash function (the same function that is
-#    used in DSDL signature computation, see specification for details), and
-#    the result should be processed as desribed in the first item above.
-#
-# Many microcontrollers provide vendor-assigned unique product IDs that are
-# typically 12 to 16 bytes long (e.g. STM32, LPC11), which should be employed
-# as described in the second item above.
-#
-# A node is not allowed to use this algorithm if it doesn't have a unique ID
-# that is 57 or more bits long.
-#
-# Regarding the collision probability: the birthday paradox suggests that a
-# network with n=30 nodes will experience a short unique ID collision with
-# approximate probability of (n^2)/(2*(2^57-1)) = 3.1e-15, assuming that
-# the ID reduction function is perfect.
-#
-truncated uint57 short_unique_id
-
-#
-# In the request, this field should contain Node ID that the requesting node
-# would like to take. The allocator then may choose to provide the requested
-# Node ID, or to assign a higher one, or to disregard this preference
-# completely. It is recommended that the allocator always tries to provide
-# Node ID that is equal or higher than requested one, as it would allow nodes
-# to get Node ID according to their desired priority on the bus.
-#
-truncated uint7 node_id

dsdl/uavcan/protocol/dynamic_node_id/559.Allocation.uavcan

@@ -0,0 +1,64 @@
+#
+# This message is used for dynamic Node ID allocation. This algorithm is an optional extension to the UAVCAN
+# specification, and it is not mandatory to support it.
+#
+# On the client's side the protocol is defined through the following set of rules:
+#
+# Rule A. On initialization:
+# 1. The client subscribes to this message.
+# 2. The client starts the Request Timer with interval of DEFAULT_REQUEST_INTERVAL_MS.
+#
+# Rule B. On expiration of Request Timer:
+# 1. Request Timer restarts.
+# 2. The client broadcasts a first-stage Allocation request message, where the fields are assigned following values:
+#    node_id                 - preferred node ID, or zero if the client doesn't have any preference
+#    first_part_of_unique_id - true
+#    unique_id               - first MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST bytes of unique ID
+#
+# Rule C. On any Allocation message, even if other rules also match:
+# 1. Request Timer restarts.
+#
+# Rule D. On an Allocation message WHERE (source node ID is non-anonymous) AND (client's unique ID starts with the
+# bytes available in the field unique_id) AND (unique_id is less than 16 bytes long):
+# 1. The client broadcasts a second-stage Allocation request message, where the fields are assigned following values:
+#    node_id                 - same value as in the first-stage
+#    first_part_of_unique_id - false
+#    unique_id               - at most MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST bytes of local unique ID with an offset
+#                              equal to number of bytes in the received unique ID
+#
+# Rule E. On an Allocation message WHERE (source node ID is non-anonymous) AND (unique_id fully matches client's
+# unique ID) AND (node_id in the received message is not zero):
+# 1. Request Timer stops.
+# 2. The client initializes its node_id with the received value.
+# 3. The client terminates subscription to Allocation messages.
+# 4. Exit.
+#
+
+#
+# Recommended request transmission period.
+#
+uint16 DEFAULT_REQUEST_PERIOD_MS = 1000
+
+#
+# Any request message can accommodate no more than this number of bytes of unique ID.
+# This limitation is needed to ensure that all request transfers are single-frame.
+#
+uint8 MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST = 7
+
+#
+# If transfer is anonymous, this is the preferred ID.
+# If transfer is non-anonymous, this is allocated ID.
+#
+uint7 node_id
+
+#
+# If transfer is anonymous, this field indicates first-stage request.
+# If transfer is non-anonymous, this field should be ignored.
+#
+bool first_part_of_unique_id
+
+#
+# If transfer is anonymous, this array must not contain more than MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST items.
+# Note that array is tail-optimized, i.e. it will not be prepended with length field.
+#
+uint8[<=16] unique_id

libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp

@@ -10,7 +10,7 @@
 #include <uavcan/node/timer.hpp>
 #include <uavcan/util/method_binder.hpp>
 #include <uavcan/build_config.hpp>
-#include <uavcan/protocol/DynamicNodeIDAllocation.hpp>
+#include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
 #include <uavcan/protocol/HardwareVersion.hpp>
 
 namespace uavcan
@@ -18,9 +18,8 @@ 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
+ * Once started, the object will be publishing dynamic node ID allocation requests at the default frequency defined
- * by the specification, until a Node ID is granted by the allocator. Note that if there are multiple responses,
+ * by the specification, until a Node ID is granted by the allocator.
- * 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.
@@ -31,73 +30,60 @@ class DynamicNodeIDAllocationClient : private TimerBase
 {
     typedef MethodBinder<DynamicNodeIDAllocationClient*,
                          void (DynamicNodeIDAllocationClient::*)
-                             (const ReceivedDataStructure<protocol::DynamicNodeIDAllocation>&)>
+                             (const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>&)>
-        DynamicNodeIDAllocationCallback;
+        AllocationCallback;
 
-    Publisher<protocol::DynamicNodeIDAllocation> dnida_pub_;
+    Publisher<protocol::dynamic_node_id::Allocation> dnida_pub_;
-    Subscriber<protocol::DynamicNodeIDAllocation, DynamicNodeIDAllocationCallback> dnida_sub_;
+    Subscriber<protocol::dynamic_node_id::Allocation, AllocationCallback> dnida_sub_;
 
-    uint64_t short_unique_id_;
+    uint8_t unique_id_[protocol::HardwareVersion::FieldTypes::unique_id::MaxSize];
     NodeID preferred_node_id_;
     NodeID allocated_node_id_;
     NodeID allocator_node_id_;
 
+    void terminate();
+
     virtual void handleTimerEvent(const TimerEvent&);
 
-    void handleDynamicNodeIDAllocation(const ReceivedDataStructure<protocol::DynamicNodeIDAllocation>& msg);
+    void handleAllocation(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& 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 hardware_version  Hardware version information, where unique_id must be set correctly.
-     * @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; set to broadcast (zero) if
-     * @param preferred_node_id         Node ID that the application would like to take; default is any.
+     *                          the application doesn't have any preference (this is default).
-     * @return                          Zero on success
+     * @return                  Zero on success
-     *                                  Negative error code on failure
+     *                          Negative error code on failure
-     *                                  -ErrLogic if the node is not in passive mode (i.e. allocation is meaningless)
+     *                          -ErrLogic if 1. the node is not in passive mode or 2. the client is already started
-     *                                  -ErrInvalidParam if the supplied short unique ID is invalid
      */
-    int start(uint64_t short_unique_node_id, NodeID preferred_node_id = NodeID::Broadcast);
+    int start(const protocol::HardwareVersion& hardware_version, const NodeID preferred_node_id = NodeID::Broadcast);
 
     /**
-     * This overload computes a short unique Node ID using data from uavcan.protocol.HardwareVersion structure.
+     * Use this method to determine when allocation is complete.
-     * @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);
+    bool isAllocationComplete() const { return getAllocatedNodeID().isUnicast(); }
 
     /**
-     * This method allows to retrieve the value that was allocated to the local node.
+     * This method allows to retrieve the node ID that was allocated to the local node.
-     * If no value was allocated yet, the returned Node ID will be invalid (non-unicast).
+     * If no node ID 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.
-     * @return          If allocation is complete, a valid unicast Node ID will be returned.
+     *                  If allocation is not complete yet, a non-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).
+     * 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.
+     * @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.
+     *                  If allocation is not complete yet, an non-unicast 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_; }
 };
 
 }

libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp

@@ -3,117 +3,133 @@
  */
 
 #include <uavcan/protocol/dynamic_node_id_allocation_client.hpp>
-#include <uavcan/data_type.hpp>  // For CRC64
 
 namespace uavcan
 {
 
-void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&)
+void DynamicNodeIDAllocationClient::terminate()
 {
-    UAVCAN_ASSERT(!allocated_node_id_.isUnicast());
+    UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Client terminated");
-    UAVCAN_ASSERT(preferred_node_id_.isUnicast());
+    stop();
-
+    dnida_sub_.stop();
-    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<unsigned long long>(short_unique_id_),
-                 static_cast<int>(preferred_node_id_.get()));
-
-    const int res = dnida_pub_.broadcast(msg);
-    if (res < 0)
-    {
-        dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocation pub failed");
-    }
 }
 
-void DynamicNodeIDAllocationClient::handleDynamicNodeIDAllocation(
+void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&)
-    const ReceivedDataStructure<protocol::DynamicNodeIDAllocation>& msg)
 {
-    if ((msg.short_unique_id != short_unique_id_) || msg.isAnonymousTransfer())
+    // This method implements Rule B
+    UAVCAN_ASSERT(preferred_node_id_.isValid());
+    if (isAllocationComplete())
     {
-        return;  // Not ours
+        UAVCAN_ASSERT(0);
-    }
+        terminate();
-
-    if (allocated_node_id_.isUnicast())
-    {
-        UAVCAN_TRACE("DynamicNodeIDAllocation", "Redundant response from %d",
-                     static_cast<int>(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;
+    // Filling the message
-    allocator_node_id_ = msg.getSrcNodeID();
+    protocol::dynamic_node_id::Allocation msg;
+    msg.node_id = preferred_node_id_.get();
+    msg.first_part_of_unique_id = true;
 
-    UAVCAN_TRACE("DynamicNodeIDAllocation", "Allocation done: requested %d, received %d from %d",
+    msg.unique_id.resize(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST);
-                 static_cast<int>(preferred_node_id_.get()),
+    copy(unique_id_, unique_id_ + msg.unique_id.size(), msg.unique_id.begin());
-                 static_cast<int>(allocated_node_id_.get()),
+    UAVCAN_ASSERT(equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_));
+
+    // Broadcasting
+    UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Broadcasting 1st stage: preferred ID: %d",
+                 static_cast<int>(preferred_node_id_.get()));
+    const int res = dnida_pub_.broadcast(msg);
+    if (res < 0)
+    {
+        dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocationClient request failed");
+    }
+}
+
+void DynamicNodeIDAllocationClient::handleAllocation(
+    const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& msg)
+{
+    /*
+     * TODO This method can blow the stack easily
+     */
+    UAVCAN_ASSERT(preferred_node_id_.isValid());
+    if (isAllocationComplete())
+    {
+        UAVCAN_ASSERT(0);
+        terminate();
+        return;
+    }
+
+    startPeriodic(getPeriod()); // Restarting the timer - Rule C
+    UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Request timer reset because of Allocation message from %d",
                  static_cast<int>(msg.getSrcNodeID().get()));
 
-    TimerBase::stop();
+    // Rule D
-}
+    if (!msg.isAnonymousTransfer() &&
-
+        msg.unique_id.size() > 0 &&
-int DynamicNodeIDAllocationClient::startImpl()
+        msg.unique_id.size() < msg.unique_id.capacity() &&
-{
+        equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_))
-    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
+        // Filling the response message
-        return -ErrInvalidParam;
+        const uint8_t size_of_unique_id_in_response =
+            min(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST,
+                static_cast<uint8_t>(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("DynamicNodeIDAllocationClient",
+                     "Broadcasting 2nd stage: preferred ID: %d, size of unique ID: %d",
+                     static_cast<int>(preferred_node_id_.get()), static_cast<int>(second_stage.unique_id.size()));
+        const int res = dnida_pub_.broadcast(second_stage);
+        if (res < 0)
+        {
+            dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocationClient request failed");
+        }
     }
 
-    UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Short unique node ID: 0x%016llx, preferred node ID: %d",
+    // Rule E
-                 static_cast<unsigned long long>(short_unique_id_), static_cast<int>(preferred_node_id_.get()));
+    if (!msg.isAnonymousTransfer() &&
-
+        msg.unique_id.size() == msg.unique_id.capacity() &&
-    allocated_node_id_ = NodeID();
+        equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_) &&
-    UAVCAN_ASSERT(!allocated_node_id_.isUnicast());
+        msg.node_id > 0)
-    UAVCAN_ASSERT(!allocated_node_id_.isValid());
-
-    int res = dnida_pub_.init();
-    if (res < 0)
     {
-        return res;
+        allocated_node_id_ = msg.node_id;
+        allocator_node_id_ = msg.getSrcNodeID();
+        UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Allocation complete, node ID %d provided by %d",
+                     static_cast<int>(allocated_node_id_.get()), static_cast<int>(allocator_node_id_.get()));
+        terminate();
+        UAVCAN_ASSERT(isAllocationComplete());
     }
-    dnida_pub_.allowAnonymousTransfers();
-
-    res = dnida_sub_.start(
-        DynamicNodeIDAllocationCallback(this, &DynamicNodeIDAllocationClient::handleDynamicNodeIDAllocation));
-    if (res < 0)
-    {
-        return res;
-    }
-    dnida_sub_.allowAnonymousTransfers();
-
-    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
+    terminate();
-    bool unique_id_is_zero = true;
+
-    for (uint8_t i = 0; i < hardware_version.unique_id.size(); i++)
+    // Allocation is not possible if node ID is already set
+    if (dnida_pub_.getNode().getNodeID().isUnicast())
     {
-        if (hardware_version.unique_id[i] != 0)
+        return -ErrLogic;
+    }
+
+    // Unique ID initialization & validation
+    copy(hardware_version.unique_id.begin(), hardware_version.unique_id.end(), unique_id_);
+    bool unique_id_is_zero = true;
+    for (uint8_t i = 0; i < sizeof(unique_id_); i++)
+    {
+        if (unique_id_[i] != 0)
         {
             unique_id_is_zero = false;
             break;
@@ -125,14 +141,37 @@ int DynamicNodeIDAllocationClient::start(const protocol::HardwareVersion& hardwa
         return -ErrInvalidParam;
     }
 
-    // Reducing the ID to 64 bits according to the specification
+    if (!preferred_node_id.isValid())  // Only broadcast and unicast are allowed
-    DataTypeSignatureCRC crc;
+    {
-    crc.add(hardware_version.unique_id.begin(), hardware_version.unique_id.size());
+        return -ErrInvalidParam;
-    short_unique_id_ = crc.get();
+    }
 
+    // Initializing the fields
     preferred_node_id_ = preferred_node_id;
+    allocated_node_id_ = NodeID();
+    allocator_node_id_ = NodeID();
+    UAVCAN_ASSERT(preferred_node_id_.isValid());
+    UAVCAN_ASSERT(!allocated_node_id_.isValid());
+    UAVCAN_ASSERT(!allocator_node_id_.isValid());
 
-    return startImpl();
+    // Initializing node objects - Rule A
+    int res = dnida_pub_.init();
+    if (res < 0)
+    {
+        return res;
+    }
+    dnida_pub_.allowAnonymousTransfers();
+
+    res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDAllocationClient::handleAllocation));
+    if (res < 0)
+    {
+        return res;
+    }
+    dnida_sub_.allowAnonymousTransfers();
+
+    startPeriodic(MonotonicDuration::fromMSec(protocol::dynamic_node_id::Allocation::DEFAULT_REQUEST_PERIOD_MS));
+
+    return 0;
 }
 
 }

libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp

@@ -15,118 +15,150 @@ TEST(DynamicNodeIDAllocationClient, Basic)
     uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b);
 
     uavcan::GlobalDataTypeRegistry::instance().reset();
-    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::DynamicNodeIDAllocation> _reg1;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::Allocation> _reg1;
+    (void)_reg1;
 
+    /*
+     * Client initialization
+     */
     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(hwver, uavcan::NodeID()));
-    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());
+    ASSERT_FALSE(dnidac.isAllocationComplete());
 
     /*
-     * Initializing subscriber
+     * Subscriber (server emulation)
-     * Anonymous transfers must be enabled
      */
-    SubscriberWithCollector<uavcan::protocol::DynamicNodeIDAllocation> dynid_sub(nodes.a);
+    SubscriberWithCollector<uavcan::protocol::dynamic_node_id::Allocation> dynid_sub(nodes.a);
     ASSERT_LE(0, dynid_sub.start());
     dynid_sub.subscriber.allowAnonymousTransfers();
 
     /*
      * 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);
+    std::cout << "First-stage request:\n" << *dynid_sub.collector.msg << std::endl;
     ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id);
+    ASSERT_TRUE(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()));
     dynid_sub.collector.msg.reset();
 
     // Rate validation
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500));
     ASSERT_FALSE(dynid_sub.collector.msg.get());
 
     // Second - rate is 1 Hz
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(900));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500));
     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());
+    ASSERT_FALSE(dnidac.isAllocationComplete());
 
     /*
-     * Sending a bunch of responses
+     * Publisher (server emulation)
-     * Note that response transfers are NOT anonymous
      */
-    uavcan::Publisher<uavcan::protocol::DynamicNodeIDAllocation> dynid_pub(nodes.a);
+    uavcan::Publisher<uavcan::protocol::dynamic_node_id::Allocation> dynid_pub(nodes.a);
     ASSERT_LE(0, dynid_pub.init());
 
-    uavcan::protocol::DynamicNodeIDAllocation msg;
+    /*
-
+     * Sending some some Allocation messages - the timer will keep restarting
-    msg.short_unique_id = 123;          // garbage
+     */
-    msg.node_id = 100;                  // oh whatever
+    for (int i = 0; i < 10; i++)
-    ASSERT_LE(0, dynid_pub.broadcast(msg));
+    {
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50));
+        uavcan::protocol::dynamic_node_id::Allocation msg;  // Contents of the message doesn't matter
-
+        ASSERT_LE(0, dynid_pub.broadcast(msg));
-    msg.short_unique_id = 0;            // garbage
+        nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(210));
-    msg.node_id = 101;
+        ASSERT_FALSE(dynid_sub.collector.msg.get());
-    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
+     * Responding with partially matching unique ID - the client will respond with second-stage request immediately
      */
-    ASSERT_FALSE(dynid_sub.collector.msg.get());
+    {
+        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());
 
-    ASSERT_TRUE(dnidac.getAllocatedNodeID().isUnicast());
+        std::cout << "First-stage offer:\n" << msg << std::endl;
-    ASSERT_TRUE(dnidac.getAllocatorNodeID().isUnicast());
 
-    ASSERT_EQ(102, dnidac.getAllocatedNodeID().get());
+        ASSERT_FALSE(dynid_sub.collector.msg.get());
-    ASSERT_EQ(10,  dnidac.getAllocatorNodeID().get());
+        ASSERT_LE(0, dynid_pub.broadcast(msg));
+        nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
 
-    // Making sure requests have stopped
+        ASSERT_TRUE(dynid_sub.collector.msg.get());
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
+        std::cout << "Second-stage request:\n" << *dynid_sub.collector.msg << std::endl;
-    ASSERT_FALSE(dynid_sub.collector.msg.get());
+        ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id);
+        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));
+        dynid_sub.collector.msg.reset();
+    }
+
+    /*
+     * Responding with second-stage offer, expecting the last request back
+     */
+    {
+        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());
+
+        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));
+
+        ASSERT_TRUE(dynid_sub.collector.msg.get());
+        std::cout << "Last request:\n" << *dynid_sub.collector.msg << std::endl;
+        ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id);
+        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));
+        dynid_sub.collector.msg.reset();
+    }
+
+    ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid());
+    ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid());
+    ASSERT_FALSE(dnidac.isAllocationComplete());
+
+    /*
+     * Now we have full unique ID for this client received, and it is possible to grant allocation
+     */
+    {
+        uavcan::protocol::dynamic_node_id::Allocation msg;
+        msg.unique_id.resize(16);
+        msg.node_id = 72;
+        uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.end(), msg.unique_id.begin());
+
+        ASSERT_FALSE(dynid_sub.collector.msg.get());
+        ASSERT_LE(0, dynid_pub.broadcast(msg));
+        nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
+        ASSERT_FALSE(dynid_sub.collector.msg.get());
+    }
+
+    ASSERT_EQ(uavcan::NodeID(72), dnidac.getAllocatedNodeID());
+    ASSERT_EQ(uavcan::NodeID(10), dnidac.getAllocatorNodeID());
+    ASSERT_TRUE(dnidac.isAllocationComplete());
 }
 
 
@@ -137,7 +169,14 @@ TEST(DynamicNodeIDAllocationClient, NonPassiveMode)
     uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b);
 
     uavcan::GlobalDataTypeRegistry::instance().reset();
-    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::DynamicNodeIDAllocation> _reg1;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::Allocation> _reg1;
+    (void)_reg1;
 
-    ASSERT_LE(-uavcan::ErrLogic, dnidac.start(123456789));
+    uavcan::protocol::HardwareVersion hwver;
+    for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++)
+    {
+        hwver.unique_id[i] = i;
+    }
+
+    ASSERT_LE(-uavcan::ErrLogic, dnidac.start(hwver));
 }