AP_DroneCAN: DNAServer: clean up and optimize allocation

libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp

@@ -444,65 +444,54 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co
     }
 }
 
-/* Handle the allocation message from the devices supporting
+// process node ID allocation messages for DNA
-dynamic node allocation. */
+void AP_DroneCAN_DNA_Server::handle_allocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg)
-void AP_DroneCAN_DNA_Server::handleAllocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg)
 {
     if (transfer.source_node_id != 0) {
-        //Ignore Allocation messages that are not DNA requests
+        return; // ignore allocation messages that are not DNA requests
-        return;
     }
     uint32_t now = AP_HAL::millis();
 
     if (rcvd_unique_id_offset == 0 ||
-        (now - last_alloc_msg_ms) > 500) {
+        (now - last_alloc_msg_ms) > UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_FOLLOWUP_TIMEOUT_MS) {
         if (msg.first_part_of_unique_id) {
             rcvd_unique_id_offset = 0;
         } else {
-            //we are only accepting first part
+            return; // only accepting the first part
-            return;
         }
     } else if (msg.first_part_of_unique_id) {
-        // we are only accepting follow up messages
+        return; // only accepting follow up messages
-        return;
     }
 
     if (rcvd_unique_id_offset) {
-        debug_dronecan(AP_CANManager::LOG_DEBUG, "TIME: %ld  -- Accepting Followup part! %u\n",
+        debug_dronecan(AP_CANManager::LOG_DEBUG, "TIME: %lu  -- Accepting Followup part! %u\n",
-                     (long int)AP_HAL::millis(),
+                     now,
                      unsigned((now - last_alloc_msg_ms)));
     } else {
-        debug_dronecan(AP_CANManager::LOG_DEBUG, "TIME: %ld  -- Accepting First part! %u\n",
+        debug_dronecan(AP_CANManager::LOG_DEBUG, "TIME: %lu  -- Accepting First part! %u\n",
-                     (long int)AP_HAL::millis(),
+                     now,
                      unsigned((now - last_alloc_msg_ms)));
     }
 
     last_alloc_msg_ms = now;
-    if ((rcvd_unique_id_offset + msg.unique_id.len) > 16) {
+    if ((rcvd_unique_id_offset + msg.unique_id.len) > sizeof(rcvd_unique_id)) {
-        //This request is malformed, Reset!
+        rcvd_unique_id_offset = 0; // reset state, request contains an over-long ID
-        rcvd_unique_id_offset = 0;
         return;
     }
 
-    //copy over the unique_id
+    // save the new portion of the unique ID
-    for (uint8_t i=rcvd_unique_id_offset; i<(rcvd_unique_id_offset + msg.unique_id.len); i++) {
+    memcpy(&rcvd_unique_id[rcvd_unique_id_offset], msg.unique_id.data, msg.unique_id.len);
-        rcvd_unique_id[i] = msg.unique_id.data[i - rcvd_unique_id_offset];
-    }
     rcvd_unique_id_offset += msg.unique_id.len;
 
-    //send follow up message
+    // respond with the message containing the received unique ID so far, or
+    // with the node ID if we successfully allocated one
     uavcan_protocol_dynamic_node_id_Allocation rsp {};
-
-    /* Respond with the message containing the received unique ID so far
-    or with node id if we successfully allocated one. */
     memcpy(rsp.unique_id.data, rcvd_unique_id, rcvd_unique_id_offset);
     rsp.unique_id.len = rcvd_unique_id_offset;
 
-    if (rcvd_unique_id_offset == 16) {
+    if (rcvd_unique_id_offset == sizeof(rcvd_unique_id)) { // full unique ID received, allocate it!
-        //We have received the full Unique ID, time to do allocation
+        rsp.node_id = db.handle_allocation(msg.node_id, rcvd_unique_id);
-        rsp.node_id = db.handle_allocation(msg.node_id, (const uint8_t*)rcvd_unique_id);
+        rcvd_unique_id_offset = 0; // reset state for next allocation
-        //reset states as well        
-        rcvd_unique_id_offset = 0;
     }
 
     allocation_pub.broadcast(rsp, false); // never publish allocation message with CAN FD

libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h

@@ -123,7 +123,7 @@ class AP_DroneCAN_DNA_Server
     char fault_node_name[15];
 
 
-    //Allocation params
+    // dynamic node ID allocation state variables
     uint8_t rcvd_unique_id[16];
     uint8_t rcvd_unique_id_offset;
     uint32_t last_alloc_msg_ms;
@@ -133,7 +133,7 @@ class AP_DroneCAN_DNA_Server
 
     Canard::Publisher<uavcan_protocol_dynamic_node_id_Allocation> allocation_pub{_canard_iface};
 
-    Canard::ObjCallback<AP_DroneCAN_DNA_Server, uavcan_protocol_dynamic_node_id_Allocation> allocation_cb{this, &AP_DroneCAN_DNA_Server::handleAllocation};
+    Canard::ObjCallback<AP_DroneCAN_DNA_Server, uavcan_protocol_dynamic_node_id_Allocation> allocation_cb{this, &AP_DroneCAN_DNA_Server::handle_allocation};
     Canard::Subscriber<uavcan_protocol_dynamic_node_id_Allocation> allocation_sub;
 
     Canard::ObjCallback<AP_DroneCAN_DNA_Server, uavcan_protocol_NodeStatus> node_status_cb{this, &AP_DroneCAN_DNA_Server::handleNodeStatus};
@@ -159,8 +159,8 @@ public:
     //report the server state, along with failure message if any
     bool prearm_check(char* fail_msg, uint8_t fail_msg_len) const;
 
-    //Callbacks
+    // canard message handler callbacks
-    void handleAllocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg);
+    void handle_allocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg);
     void handleNodeStatus(const CanardRxTransfer& transfer, const uavcan_protocol_NodeStatus& msg);
     void handleNodeInfo(const CanardRxTransfer& transfer, const uavcan_protocol_GetNodeInfoResponse& rsp);