New dynamic node ID allocation client

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
- * 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.
+ * 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.
  *
  * 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>&)>
-        DynamicNodeIDAllocationCallback;
+                             (const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>&)>
+        AllocationCallback;
 
-    Publisher<protocol::DynamicNodeIDAllocation> dnida_pub_;
-    Subscriber<protocol::DynamicNodeIDAllocation, DynamicNodeIDAllocationCallback> dnida_sub_;
+    Publisher<protocol::dynamic_node_id::Allocation> dnida_pub_;
+    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);
-
-    int startImpl();
+    void handleAllocation(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& msg);
 
 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
+     * @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).
+     * @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(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.
-     * @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
+     * Use this method to determine when allocation is complete.
      */
-    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.
-     * 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.
+     * This method allows to retrieve the node ID that was allocated to the local node.
+     * 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, a non-unicast 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.
+     * 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 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_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<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");
-    }
+    UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Client terminated");
+    stop();
+    dnida_sub_.stop();
 }
 
-void DynamicNodeIDAllocationClient::handleDynamicNodeIDAllocation(
-    const ReceivedDataStructure<protocol::DynamicNodeIDAllocation>& msg)
+void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&)
 {
-    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
-    }
-
-    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");
+        UAVCAN_ASSERT(0);
+        terminate();
         return;
     }
 
-    allocated_node_id_ = allocation;
-    allocator_node_id_ = msg.getSrcNodeID();
+    // Filling the message
+    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",
-                 static_cast<int>(preferred_node_id_.get()),
-                 static_cast<int>(allocated_node_id_.get()),
+    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_));
+
+    // 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();
-}
-
-int DynamicNodeIDAllocationClient::startImpl()
-{
-    short_unique_id_ &= protocol::DynamicNodeIDAllocation::FieldTypes::short_unique_id::mask();
-
-    if ((short_unique_id_ == 0) || !preferred_node_id_.isUnicast())
+    // 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_))
     {
-        // It's not like a zero unique ID is not unique enough, but it's surely suspicious
-        return -ErrInvalidParam;
+        // 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<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",
-                 static_cast<unsigned long long>(short_unique_id_), static_cast<int>(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)
+    // 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)
     {
-        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
-    bool unique_id_is_zero = true;
-    for (uint8_t i = 0; i < hardware_version.unique_id.size(); i++)
+    terminate();
+
+    // 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
-    DataTypeSignatureCRC crc;
-    crc.add(hardware_version.unique_id.begin(), hardware_version.unique_id.size());
-    short_unique_id_ = crc.get();
+    if (!preferred_node_id.isValid())  // Only broadcast and unicast are allowed
+    {
+        return -ErrInvalidParam;
+    }
 
+    // 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(0));
-    ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver, uavcan::NodeID::Broadcast)); // Bad node ID
-    ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver, uavcan::NodeID()));          // Ditto
+    ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver, uavcan::NodeID()));
 
     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
-     * Anonymous transfers must be enabled
+     * Subscriber (server emulation)
      */
-    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
-     * Note that response transfers are NOT anonymous
+     * Publisher (server emulation)
      */
-    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;
-
-    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));
+    /*
+     * Sending some some Allocation messages - the timer will keep restarting
+     */
+    for (int i = 0; i < 10; i++)
+    {
+        uavcan::protocol::dynamic_node_id::Allocation msg;  // Contents of the message doesn't matter
+        ASSERT_LE(0, dynid_pub.broadcast(msg));
+        nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(210));
+        ASSERT_FALSE(dynid_sub.collector.msg.get());
+    }
 
     /*
-     * 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());
-    ASSERT_TRUE(dnidac.getAllocatorNodeID().isUnicast());
+        std::cout << "First-stage offer:\n" << msg << std::endl;
 
-    ASSERT_EQ(102, dnidac.getAllocatedNodeID().get());
-    ASSERT_EQ(10,  dnidac.getAllocatorNodeID().get());
+        ASSERT_FALSE(dynid_sub.collector.msg.get());
+        ASSERT_LE(0, dynid_pub.broadcast(msg));
+        nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
 
-    // Making sure requests have stopped
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
-    ASSERT_FALSE(dynid_sub.collector.msg.get());
+        ASSERT_TRUE(dynid_sub.collector.msg.get());
+        std::cout << "Second-stage 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() + 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));
 }