AP_BattMonitor: dynamically update node ID on hot-swap when battery ID matches to resume telemetry

Author: tatsuy

libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp

@@ -74,8 +74,16 @@ AP_BattMonitor_DroneCAN* AP_BattMonitor_DroneCAN::get_dronecan_backend(AP_DroneC
             continue;
         }
         AP_BattMonitor_DroneCAN* driver = (AP_BattMonitor_DroneCAN*)batt.drivers[i];
-        if (driver->_ap_dronecan == ap_dronecan && driver->_node_id == node_id && match_battery_id(i, battery_id)) {
-            return driver;
+        if (driver->_ap_dronecan == ap_dronecan && match_battery_id(i, battery_id)) {
+            if (driver->_node_id == node_id) {
+                return driver;
+            } else if (!driver->_interim_state.healthy && driver->option_is_set(AP_BattMonitor_Params::Options::AllowDynamicNodeUpdate)) {
+                GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Battery %u: Node change from %d to %d for Id %d",
+                    (unsigned)i+1, driver->_node_id, node_id, battery_id);
+                driver->_node_id = node_id;
+                driver->init();
+                return driver;
+            }
         }
     }
     // find empty uavcan driver

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -156,7 +156,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: Battery monitor options
     // @Description: This sets options to change the behaviour of the battery monitor
-    // @Bitmask: 0:Ignore DroneCAN SoC, 1:MPPT reports input voltage and current, 2:MPPT Powered off when disarmed, 3:MPPT Powered on when armed, 4:MPPT Powered off at boot, 5:MPPT Powered on at boot, 6:Send resistance compensated voltage to GCS, 7:Allow DroneCAN InfoAux to be from a different CAN node, 8:Battery is for internal autopilot use only, 9:Sum monitor measures minimum voltage instead of average
+    // @Bitmask: 0:Ignore DroneCAN SoC, 1:MPPT reports input voltage and current, 2:MPPT Powered off when disarmed, 3:MPPT Powered on when armed, 4:MPPT Powered off at boot, 5:MPPT Powered on at boot, 6:Send resistance compensated voltage to GCS, 7:Allow DroneCAN InfoAux to be from a different CAN node, 8:Battery is for internal autopilot use only, 9:Sum monitor measures minimum voltage instead of average, 10:Allow DroneCAN dynamic node update on hot-swap
     // @User: Advanced
     AP_GROUPINFO("OPTIONS", 21, AP_BattMonitor_Params, _options, 0),
 #endif // AP_BATTERY_OPTIONS_PARAM_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_Params.h

@@ -61,6 +61,7 @@ class AP_BattMonitor_Params {
         AllowSplitAuxInfo                   = (1U<<7),  // allow different node to provide aux info for DroneCAN
         InternalUseOnly                     = (1U<<8),  // for use internally to ArduPilot, not to be (eg.) sent via MAVLink BATTERY_STATUS
         Minimum_Voltage                     = (1U<<9),  // sum monitor measures minimum voltage rather than average
+        AllowDynamicNodeUpdate              = (1U<<10), // allow dynamic update of DroneCAN node ID during hot-swap when telemetry is lost
     };
 
     BattMonitor_LowVoltage_Source failsafe_voltage_source(void) const { return (enum BattMonitor_LowVoltage_Source)_failsafe_voltage_source.get(); }