aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_UAVCAN: add node healthy check

Browse Source
This commit is contained in:
bugobliterator 2022-04-29 17:17:51 +05:30 committed by Andrew Tridgell
parent 34583d750d
commit 48f1e18161
2 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp
View File

@ -349,6 +349,7 @@ bool AP_UAVCAN_DNA_Server::init()
if any duplicates are on the bus carrying our Node ID */ if any duplicates are on the bus carrying our Node ID */
addToSeenNodeMask(node_id); addToSeenNodeMask(node_id);
setVerificationMask(node_id); setVerificationMask(node_id);
node_healthy_mask.set(node_id);
self_node_id[driver_index] = node_id; self_node_id[driver_index] = node_id;
return true; return true;
} }
@ -474,6 +475,18 @@ void AP_UAVCAN_DNA_Server::handleNodeStatus(uint8_t node_id, const NodeStatusCb
if (node_id > MAX_NODE_ID) { if (node_id > MAX_NODE_ID) {
return; return;
} }
if (cb.msg->health != uavcan::protocol::NodeStatus::HEALTH_OK ||
cb.msg->mode != uavcan::protocol::NodeStatus::MODE_OPERATIONAL) {
//if node is not healthy or operational, clear resp health mask, and set fault_node_id
fault_node_id = node_id;
server_state = NODE_STATUS_UNHEALTHY;
node_healthy_mask.clear(node_id);
} else {
node_healthy_mask.set(node_id);
if (node_healthy_mask == verified_mask) {
server_state = HEALTHY;
}
}
if (!isNodeIDVerified(node_id)) { if (!isNodeIDVerified(node_id)) {
//immediately begin verification of the node_id //immediately begin verification of the node_id
if (getNodeInfo_client[driver_index] != nullptr) { if (getNodeInfo_client[driver_index] != nullptr) {
@ -700,6 +713,10 @@ bool AP_UAVCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) co
snprintf(fail_msg, fail_msg_len, "Failed to add Node %d!", fault_node_id); snprintf(fail_msg, fail_msg_len, "Failed to add Node %d!", fault_node_id);
return false; return false;
} }
case NODE_STATUS_UNHEALTHY: {
snprintf(fail_msg, fail_msg_len, "Node %d unhealthy!", fault_node_id);
return false;
}
} }
// should never get; compiler should enforce all server_states are covered // should never get; compiler should enforce all server_states are covered
return false; return false;

2
libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h
View File

@ -25,6 +25,7 @@ class AP_UAVCAN_DNA_Server
}; };
enum ServerState { enum ServerState {
NODE_STATUS_UNHEALTHY = -5,
STORAGE_FAILURE = -3, STORAGE_FAILURE = -3,
DUPLICATE_NODES = -2, DUPLICATE_NODES = -2,
FAILED_TO_ADD_NODE = -1, FAILED_TO_ADD_NODE = -1,
@ -40,6 +41,7 @@ class AP_UAVCAN_DNA_Server
Bitmask<128> verified_mask; Bitmask<128> verified_mask;
Bitmask<128> node_seen_mask; Bitmask<128> node_seen_mask;
Bitmask<128> logged; Bitmask<128> logged;
Bitmask<128> node_healthy_mask;
uint8_t last_logging_count; uint8_t last_logging_count;