From f2e97b934215af728a52c0f3046d2905dbcc7a90 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 2 Nov 2021 16:26:10 +1100 Subject: [PATCH] AP_UAVCAN: added CAN_Dn_UC_OPTION parameter this allows for 2 ways of controlling conflicts in the UAVCAN DNA database. The first is to set CAN_Dn_UC_OPTION to 1, which resets the DNA database, thus clearing any node conflicts. The second is to set CAN_Dn_UC_OPTION to 2, which ignores node conflicts in the DNA database These options are useful for vehicles with UAVCAN smart batteries where the node ID is fixed but the hwid changes and you want to do battery swapping (possibly without rebooting) --- libraries/AP_UAVCAN/AP_UAVCAN.cpp | 18 ++++++++++++++++++ libraries/AP_UAVCAN/AP_UAVCAN.h | 16 ++++++++++++++++ libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp | 12 +++++++++++- libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h | 1 + 4 files changed, 46 insertions(+), 1 deletion(-) diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 8086a7ff70..98e557ba91 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -99,6 +99,13 @@ const AP_Param::GroupInfo AP_UAVCAN::var_info[] = { // @User: Advanced AP_GROUPINFO("SRV_RT", 4, AP_UAVCAN, _servo_rate_hz, 50), + // @Param: OPTION + // @DisplayName: UAVCAN options + // @Description: Option flags + // @Bitmask: 0:ClearDNADatabase,1:IgnoreDNANodeConflicts + // @User: Advanced + AP_GROUPINFO("OPTION", 5, AP_UAVCAN, _options, 0), + AP_GROUPEND }; @@ -835,4 +842,15 @@ void AP_UAVCAN::handle_debug(AP_UAVCAN* ap_uavcan, uint8_t node_id, const DebugC #endif } +// check if a option is set and if it is then reset it to 0. +// return true if it was set +bool AP_UAVCAN::check_and_reset_option(Options option) +{ + bool ret = option_is_set(option); + if (ret) { + _options.set_and_save(int16_t(_options.get() & ~uint16_t(option))); + } + return ret; +} + #endif // HAL_NUM_CAN_IFACES diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index 82272fa8b2..e0cd8fed9c 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN.h @@ -140,6 +140,21 @@ public: const uavcan::ReceivedDataStructure *msg; }; + // options bitmask + enum class Options : uint16_t { + DNA_CLEAR_DATABASE = (1U<<0), + DNA_IGNORE_DUPLICATE_NODE = (1U<<1), + }; + + // check if a option is set + bool option_is_set(Options option) const { + return (uint16_t(_options.get()) & uint16_t(option)) != 0; + } + + // check if a option is set and if it is then reset it to + // 0. return true if it was set + bool check_and_reset_option(Options option); + private: // This will be needed to implement if UAVCAN is used with multithreading // Such cases will be firmware update, etc. @@ -170,6 +185,7 @@ private: AP_Int32 _servo_bm; AP_Int32 _esc_bm; AP_Int16 _servo_rate_hz; + AP_Int16 _options; uavcan::Node<0> *_node; diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp index 45be963c3d..031c3e3de2 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.cpp @@ -261,6 +261,8 @@ bool AP_UAVCAN_DNA_Server::init(AP_UAVCAN *ap_uavcan) WITH_SEMAPHORE(sem); + _ap_uavcan = ap_uavcan; + //Read the details from ap_uavcan uavcan::Node<0>* _node = ap_uavcan->get_node(); uint8_t node_id = _node->getNodeID().get(); @@ -315,6 +317,10 @@ bool AP_UAVCAN_DNA_Server::init(AP_UAVCAN *ap_uavcan) //Its not there a reset should write it in the Storage reset(); } + if (ap_uavcan->check_and_reset_option(AP_UAVCAN::Options::DNA_CLEAR_DATABASE)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "UC DNA database reset"); + reset(); + } // Making sure that the server is started with the same node ID const uint8_t stored_own_node_id = getNodeIDForUniqueID(own_unique_id, own_unique_id_len); static bool reset_done; @@ -541,7 +547,7 @@ void AP_UAVCAN_DNA_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[], nodeInfo_resp_rcvd = true; } setVerificationMask(node_id); - } else { + } else if (!_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) { /* This is a device with node_id already registered for another device */ server_state = DUPLICATE_NODES; @@ -704,6 +710,10 @@ bool AP_UAVCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) co return false; } case DUPLICATE_NODES: { + if (_ap_uavcan->option_is_set(AP_UAVCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) { + // ignore error + return true; + } snprintf(fail_msg, fail_msg_len, "Duplicate Node %s../%d!", fault_node_name, fault_node_id); return false; } diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h index 9254aa9c05..1c1d9684ac 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h +++ b/libraries/AP_UAVCAN/AP_UAVCAN_DNA_Server.h @@ -88,6 +88,7 @@ class AP_UAVCAN_DNA_Server bool isValidNodeDataAvailable(uint8_t node_id); HAL_Semaphore sem; + AP_UAVCAN *_ap_uavcan; public: AP_UAVCAN_DNA_Server(StorageAccess _storage) : storage(_storage) {}