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

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

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -140,6 +140,21 @@ public:
         const uavcan::ReceivedDataStructure<DataType_> *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;
 

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

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