Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Thomas Watson 2024-07-13 14:59:20 -05:00 committed by Andrew Tridgell
parent 56593574a9
commit 60a9f1722c
2 changed files with 31 additions and 110 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

115
libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
View File

@ -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;
}

26
libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
View File

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