From 4852ac332d017eb850dc622a1fe6043a2a16e3e4 Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Mon, 25 Jul 2022 09:00:50 +0530 Subject: [PATCH] AP_UAVCAN: add option bit for disabling Node Unhealthy check --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 2 +- libraries/AP_UAVCAN/AP_UAVCAN.h | 1 + libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp | 9 +++++++-- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 964468a9db..e162e56c9b 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -122,7 +122,7 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { // @Param: OPTION // @DisplayName: UAVCAN options // @Description: Option flags - // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd + // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts,2:EnableCanfd,3:IgnoreDNANodeUnhealthy // @User: Advanced AP_GROUPINFO("OPTION", 5, AP_UAVCAN, _options, 0), diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 069962eed0..2523bc54ed 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -202,6 +202,7 @@ public: DNA_CLEAR_DATABASE = (1U<<0), DNA_IGNORE_DUPLICATE_NODE = (1U<<1), CANFD_ENABLED = (1U<<2), + DNA_IGNORE_UNHEALTHY_NODE = (1U<<3), }; // check if a option is set diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp index ed783df599..0e1b50cd03 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp @@ -475,8 +475,9 @@ void AP_UAVCAN_DNA_Server::handleNodeStatus(uint8_t node_id, const NodeStatusCb if (node_id > MAX_NODE_ID) { return; } - if (cb.msg->health != uavcan::protocol::NodeStatus::HEALTH_OK || - cb.msg->mode != uavcan::protocol::NodeStatus::MODE_OPERATIONAL) { + if ((cb.msg->health != uavcan::protocol::NodeStatus::HEALTH_OK || + cb.msg->mode != uavcan::protocol::NodeStatus::MODE_OPERATIONAL) && + !_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) { //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; @@ -714,6 +715,10 @@ bool AP_UAVCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) co return false; } case NODE_STATUS_UNHEALTHY: { + if (_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) { + // ignore error + return true; + } snprintf(fail_msg, fail_msg_len, "Node %d unhealthy!", fault_node_id); return false; }