Skip to content

Commit

Permalink
AP_DroneCAN: DNA_Server: unify bitmask handling
Browse files Browse the repository at this point in the history 
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.
  • Loading branch information
@tpwrules
tpwrules committed Jul 16, 2024
1 parent 2a902bb commit fc3342b
Show file tree
Hide file tree
Showing 2 changed files with 31 additions and 110 deletions.
115 changes: 25 additions & 90 deletions libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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);
}
}

Expand All @@ -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;
}
Expand All @@ -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
Expand All @@ -282,15 +241,15 @@ 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++;
}
//Search down
candidate = preferred;
while (candidate > 0) {
if (!isNodeIDOccupied(candidate)) {
if (!node_storage_occupied.get(candidate)) {
return candidate;
}
candidate--;
Expand All @@ -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
Expand All @@ -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

Expand All @@ -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;
Expand All @@ -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;
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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 */
Expand All @@ -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;
}
Expand Down
26 changes: 6 additions & 20 deletions libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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;

Expand All @@ -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);

Expand Down Expand Up @@ -114,12 +106,6 @@ class AP_DroneCAN_DNA_Server
//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);
Expand Down

0 comments on commit fc3342b

Please sign in to comment.