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) {}