From 60a9f1722c6e91f3e4bb9e20b91cf57677442816 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 13 Jul 2024 14:59:20 -0500 Subject: [PATCH] AP_DroneCAN: DNA_Server: unify bitmask handling Put documentation with each bitmask and use the object directly. Node ID range checks can be removed as the bitmask itself checks and we don't expect to trip them. Substantially cleans up the code. --- .../AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp | 115 ++++-------------- .../AP_DroneCAN/AP_DroneCAN_DNA_Server.h | 26 +--- 2 files changed, 31 insertions(+), 110 deletions(-) diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp index 7de2081eda..d61aae7995 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp +++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp @@ -91,16 +91,6 @@ void AP_DroneCAN_DNA_Server::writeNodeData(const NodeData &data, uint8_t node_id storage.write_block(NODEDATA_LOC(node_id), &data, sizeof(struct NodeData)); } -/* Set Occupation Mask, handy for keeping track of all node ids that -are allocated and all that are available. */ -void AP_DroneCAN_DNA_Server::setOccupationMask(uint8_t node_id) -{ - if (node_id > MAX_NODE_ID) { - return; - } - occupation_mask.set(node_id); -} - /* Remove Node Data from Server Record in Storage, and also clear Occupation Mask */ void AP_DroneCAN_DNA_Server::freeNodeID(uint8_t node_id) @@ -116,38 +106,7 @@ void AP_DroneCAN_DNA_Server::freeNodeID(uint8_t node_id) writeNodeData(node_data, node_id); //Clear Occupation Mask - occupation_mask.clear(node_id); -} - -/* Sets the verification mask. This is to be called, once -The Seen Node has been both registered and verified against the -Server Records. */ -void AP_DroneCAN_DNA_Server::setVerificationMask(uint8_t node_id) -{ - if (node_id > MAX_NODE_ID) { - return; - } - verified_mask.set(node_id); -} - -/* Checks if the NodeID is occupied, i.e. its recorded -in the Server Records against a unique ID */ -bool AP_DroneCAN_DNA_Server::isNodeIDOccupied(uint8_t node_id) const -{ - if (node_id > MAX_NODE_ID) { - return false; - } - return occupation_mask.get(node_id); -} - -/* Checks if NodeID is verified, i.e. the unique id in -Storage Records matches the one provided by Device with this node id. */ -bool AP_DroneCAN_DNA_Server::isNodeIDVerified(uint8_t node_id) const -{ - if (node_id > MAX_NODE_ID) { - return false; - } - return verified_mask.get(node_id); + node_storage_occupied.clear(node_id); } /* Go through Server Records, and fetch node id that matches the provided @@ -159,7 +118,7 @@ uint8_t AP_DroneCAN_DNA_Server::getNodeIDForUniqueID(const uint8_t unique_id[], getHash(cmp_node_data, unique_id, size); for (int i = MAX_NODE_ID; i > 0; i--) { - if (isNodeIDOccupied(i)) { + if (node_storage_occupied.get(i)) { readNodeData(node_data, i); if (memcmp(node_data.hwid_hash, cmp_node_data.hwid_hash, sizeof(NodeData::hwid_hash)) == 0) { return i; // node ID found @@ -181,7 +140,7 @@ void AP_DroneCAN_DNA_Server::addNodeIDForUniqueID(uint8_t node_id, const uint8_t //Write Data to the records writeNodeData(node_data, node_id); - setOccupationMask(node_id); + node_storage_occupied.set(node_id); } //Checks if a valid Server Record is present for specified Node ID @@ -223,10 +182,10 @@ bool AP_DroneCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id } /* Go through our records and look for valid NodeData, to initialise - occupation mask */ + occupied status */ for (uint8_t i = 1; i <= MAX_NODE_ID; i++) { if (isValidNodeDataAvailable(i)) { - occupation_mask.set(i); + node_storage_occupied.set(i); } } @@ -246,9 +205,9 @@ bool AP_DroneCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id } /* Also add to seen node id this is to verify if any duplicates are on the bus carrying our Node ID */ - addToSeenNodeMask(node_id); - setVerificationMask(node_id); - node_healthy_mask.set(node_id); + node_seen.set(node_id); + node_verified.set(node_id); + node_healthy.set(node_id); self_node_id = node_id; return true; } @@ -259,7 +218,7 @@ void AP_DroneCAN_DNA_Server::reset() { NodeData node_data; memset(&node_data, 0, sizeof(node_data)); - occupation_mask.clearall(); + node_storage_occupied.clearall(); //Just write empty Node Data to the Records // ensure node ID 0 is cleared even if we can't use it so we know the state @@ -282,7 +241,7 @@ uint8_t AP_DroneCAN_DNA_Server::findFreeNodeID(uint8_t preferred) // Search up uint8_t candidate = preferred; while (candidate <= 125) { - if (!isNodeIDOccupied(candidate)) { + if (!node_storage_occupied.get(candidate)) { return candidate; } candidate++; @@ -290,7 +249,7 @@ uint8_t AP_DroneCAN_DNA_Server::findFreeNodeID(uint8_t preferred) //Search down candidate = preferred; while (candidate > 0) { - if (!isNodeIDOccupied(candidate)) { + if (!node_storage_occupied.get(candidate)) { return candidate; } candidate--; @@ -299,25 +258,6 @@ uint8_t AP_DroneCAN_DNA_Server::findFreeNodeID(uint8_t preferred) return 0; } -//Check if we have received Node Status from this node_id -bool AP_DroneCAN_DNA_Server::isNodeSeen(uint8_t node_id) -{ - if (node_id > MAX_NODE_ID) { - return false; - } - return node_seen_mask.get(node_id); -} - -/* Set the Seen Node Mask, to be called when received -Node Status from the node id */ -void AP_DroneCAN_DNA_Server::addToSeenNodeMask(uint8_t node_id) -{ - if (node_id > MAX_NODE_ID) { - return; - } - node_seen_mask.set(node_id); -} - /* Run through the list of seen node ids for verification no more than once per 5 second. We continually verify the nodes in our seen list, So that we can raise issue if there are duplicates @@ -333,7 +273,7 @@ void AP_DroneCAN_DNA_Server::verify_nodes() uint8_t log_count = AP::logger().get_log_start_count(); if (log_count != last_logging_count) { last_logging_count = log_count; - logged.clearall(); + node_logged.clearall(); } #endif @@ -348,12 +288,7 @@ void AP_DroneCAN_DNA_Server::verify_nodes() Reason for this could be either the node was disconnected Or a node with conflicting ID appeared and is sending response at the same time. */ - /* Only report if the node was verified, otherwise ignore - as this could be just Bootloader to Application transition. */ - if (isNodeIDVerified(curr_verifying_node)) { - // remove verification flag for this node - verified_mask.clear(curr_verifying_node); - } + node_verified.clear(curr_verifying_node); } last_verification_request = now; @@ -363,11 +298,11 @@ void AP_DroneCAN_DNA_Server::verify_nodes() if ((curr_verifying_node == self_node_id) || (curr_verifying_node == 0)) { continue; } - if (isNodeSeen(curr_verifying_node)) { + if (node_seen.get(curr_verifying_node)) { break; } } - if (isNodeIDOccupied(curr_verifying_node)) { + if (node_storage_occupied.get(curr_verifying_node)) { uavcan_protocol_GetNodeInfoRequest request; node_info_client.request(curr_verifying_node, request); nodeInfo_resp_rcvd = false; @@ -388,20 +323,20 @@ void AP_DroneCAN_DNA_Server::handleNodeStatus(const CanardRxTransfer& transfer, //if node is not healthy or operational, clear resp health mask, and set fault_node_id fault_node_id = transfer.source_node_id; server_state = NODE_STATUS_UNHEALTHY; - node_healthy_mask.clear(transfer.source_node_id); + node_healthy.clear(transfer.source_node_id); } else { - node_healthy_mask.set(transfer.source_node_id); - if (node_healthy_mask == verified_mask) { + node_healthy.set(transfer.source_node_id); + if (node_healthy == node_verified) { server_state = HEALTHY; } } - if (!isNodeIDVerified(transfer.source_node_id)) { + if (!node_verified.get(transfer.source_node_id)) { //immediately begin verification of the node_id uavcan_protocol_GetNodeInfoRequest request; node_info_client.request(transfer.source_node_id, request); } //Add node to seen list if not seen before - addToSeenNodeMask(transfer.source_node_id); + node_seen.set(transfer.source_node_id); } /* Node Info message handler @@ -418,8 +353,8 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co if we haven't logged this node then log it now */ #if HAL_LOGGING_ENABLED - if (!logged.get(transfer.source_node_id) && AP::logger().logging_started()) { - logged.set(transfer.source_node_id); + if (!node_logged.get(transfer.source_node_id) && AP::logger().logging_started()) { + node_logged.set(transfer.source_node_id); uint64_t uid[2]; memcpy(uid, rsp.hardware_version.unique_id, sizeof(rsp.hardware_version.unique_id)); // @LoggerMessage: CAND @@ -446,13 +381,13 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co } #endif - if (isNodeIDOccupied(transfer.source_node_id)) { + if (node_storage_occupied.get(transfer.source_node_id)) { //if node_id already registered, just verify if Unique ID matches as well if (transfer.source_node_id == getNodeIDForUniqueID(rsp.hardware_version.unique_id, 16)) { if (transfer.source_node_id == curr_verifying_node) { nodeInfo_resp_rcvd = true; } - setVerificationMask(transfer.source_node_id); + node_verified.set(transfer.source_node_id); } else if (!_ap_dronecan.option_is_set(AP_DroneCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) { /* This is a device with node_id already registered for another device */ @@ -471,7 +406,7 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co //add a new server record addNodeIDForUniqueID(transfer.source_node_id, rsp.hardware_version.unique_id, 16); //Verify as well - setVerificationMask(transfer.source_node_id); + node_verified.set(transfer.source_node_id); if (transfer.source_node_id == curr_verifying_node) { nodeInfo_resp_rcvd = true; } diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h index db5ed70a31..48b2c3cc31 100644 --- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h +++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h @@ -34,11 +34,12 @@ class AP_DroneCAN_DNA_Server uint8_t self_node_id; bool nodeInfo_resp_rcvd; - Bitmask<128> occupation_mask; - Bitmask<128> verified_mask; - Bitmask<128> node_seen_mask; - Bitmask<128> logged; - Bitmask<128> node_healthy_mask; + // bitmasks containing a status for each possible node ID (except 0 and > MAX_NODE_ID) + Bitmask<128> node_storage_occupied; // storage has a valid entry + Bitmask<128> node_verified; // node seen and unique ID matches stored + Bitmask<128> node_seen; // received NodeStatus + Bitmask<128> node_logged; // written to log fle + Bitmask<128> node_healthy; // reports healthy uint8_t last_logging_count; @@ -53,10 +54,6 @@ class AP_DroneCAN_DNA_Server uint8_t rcvd_unique_id_offset; uint32_t last_alloc_msg_ms; - //Methods to handle and report Node IDs seen on the bus - void addToSeenNodeMask(uint8_t node_id); - bool isNodeSeen(uint8_t node_id); - //Generates 6Byte long hash from the specified unique_id void getHash(NodeData &node_data, const uint8_t unique_id[], uint8_t size) const; @@ -70,13 +67,8 @@ class AP_DroneCAN_DNA_Server void writeNodeData(const NodeData &data, uint8_t node_id); //Methods to set, clear and report NodeIDs allocated/registered so far - void setOccupationMask(uint8_t node_id); - bool isNodeIDOccupied(uint8_t node_id) const; void freeNodeID(uint8_t node_id); - //Set the mask to report that the unique id matches the record - void setVerificationMask(uint8_t node_id); - //Go through List to find node id for specified unique id uint8_t getNodeIDForUniqueID(const uint8_t unique_id[], uint8_t size); @@ -114,12 +106,6 @@ public: //Initialises publisher and Server Record for specified uavcan driver bool init(uint8_t own_unique_id[], uint8_t own_unique_id_len, uint8_t node_id); - /* Checks if the node id has been verified against the record - Specific CAN drivers are expected to check use this method to - verify if the node is healthy and has static node_id against - hwid in the records */ - bool isNodeIDVerified(uint8_t node_id) const; - /* Subscribe to the messages to be handled for maintaining and allocating Node ID list */ static void subscribe_msgs(AP_DroneCAN* ap_dronecan);