AP_BattMonitor: added option allowing split InfoAux

libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp

@@ -47,6 +47,9 @@ bool AP_BattMonitor_Backend::capacity_remaining_pct(uint8_t &percentage) const
     if (!has_current() || !_state.healthy) {
         return false;
     }
+    if (isnan(_state.consumed_mah) || _params._pack_capacity <= 0) {
+        return false;
+    }
 
     const float mah_remaining = _params._pack_capacity - _state.consumed_mah;
     percentage = constrain_float(100 * mah_remaining / _params._pack_capacity, 0, UINT8_MAX);

libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp

@@ -61,28 +61,39 @@ void AP_BattMonitor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan)
     }
 }
 
+/*
+  match a battery ID to driver serial number
+  when serial number is negative, all batteries are accepted, otherwise it must match
+*/
+bool AP_BattMonitor_DroneCAN::match_battery_id(uint8_t instance, uint8_t battery_id)
+{
+    const auto serial_num = AP::battery().get_serial_number(instance);
+    return serial_num < 0 || serial_num == (int32_t)battery_id;
+}
+
 AP_BattMonitor_DroneCAN* AP_BattMonitor_DroneCAN::get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t battery_id)
 {
     if (ap_dronecan == nullptr) {
         return nullptr;
     }
-    for (uint8_t i = 0; i < AP::battery()._num_instances; i++) {
-        if (AP::battery().drivers[i] == nullptr ||
-            AP::battery().get_type(i) != AP_BattMonitor::Type::UAVCAN_BatteryInfo) {
+    const auto &batt = AP::battery();
+    for (uint8_t i = 0; i < batt._num_instances; i++) {
+        if (batt.drivers[i] == nullptr ||
+            batt.get_type(i) != AP_BattMonitor::Type::UAVCAN_BatteryInfo) {
             continue;
         }
-        AP_BattMonitor_DroneCAN* driver = (AP_BattMonitor_DroneCAN*)AP::battery().drivers[i];
+        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;
         }
     }
     // find empty uavcan driver
-    for (uint8_t i = 0; i < AP::battery()._num_instances; i++) {
-        if (AP::battery().drivers[i] != nullptr &&
-            AP::battery().get_type(i) == AP_BattMonitor::Type::UAVCAN_BatteryInfo &&
+    for (uint8_t i = 0; i < batt._num_instances; i++) {
+        if (batt.drivers[i] != nullptr &&
+            batt.get_type(i) == AP_BattMonitor::Type::UAVCAN_BatteryInfo &&
             match_battery_id(i, battery_id)) {
 
-            AP_BattMonitor_DroneCAN* batmon = (AP_BattMonitor_DroneCAN*)AP::battery().drivers[i];
+            AP_BattMonitor_DroneCAN* batmon = (AP_BattMonitor_DroneCAN*)batt.drivers[i];
             if(batmon->_ap_dronecan != nullptr || batmon->_node_id != 0) {
                 continue;
             }
@@ -142,21 +153,22 @@ void AP_BattMonitor_DroneCAN::handle_battery_info_aux(const ardupilot_equipment_
 {
     WITH_SEMAPHORE(_sem_battmon);
     uint8_t cell_count = MIN(ARRAY_SIZE(_interim_state.cell_voltages.cells), msg.voltage_cell.len);
-    float remaining_capacity_ah = _remaining_capacity_wh / msg.nominal_voltage;
-    float full_charge_capacity_ah = _full_charge_capacity_wh / msg.nominal_voltage;
 
     _cycle_count = msg.cycle_count;
     for (uint8_t i = 0; i < cell_count; i++) {
         _interim_state.cell_voltages.cells[i] = msg.voltage_cell.data[i] * 1000;
     }
     _interim_state.is_powering_off = msg.is_powering_off;
-    _interim_state.consumed_mah = (full_charge_capacity_ah - remaining_capacity_ah) * 1000;
-    _interim_state.consumed_wh = _full_charge_capacity_wh - _remaining_capacity_wh;
-    _interim_state.time_remaining =  is_zero(_interim_state.current_amps) ? 0 : (remaining_capacity_ah / _interim_state.current_amps * 3600);
-    _interim_state.has_time_remaining = true;
+    if (!isnan(msg.nominal_voltage) && msg.nominal_voltage > 0) {
+        float remaining_capacity_ah = _remaining_capacity_wh / msg.nominal_voltage;
+        float full_charge_capacity_ah = _full_charge_capacity_wh / msg.nominal_voltage;
+        _interim_state.consumed_mah = (full_charge_capacity_ah - remaining_capacity_ah) * 1000;
+        _interim_state.consumed_wh = _full_charge_capacity_wh - _remaining_capacity_wh;
+        _interim_state.time_remaining =  is_zero(_interim_state.current_amps) ? 0 : (remaining_capacity_ah / _interim_state.current_amps * 3600);
+        _interim_state.has_time_remaining = true;
+    }
 
     _has_cell_voltages = true;
-    _has_time_remaining = true;
     _has_battery_info_aux = true;
 }
 
@@ -206,7 +218,33 @@ void AP_BattMonitor_DroneCAN::handle_battery_info_trampoline(AP_DroneCAN *ap_dro
 
 void AP_BattMonitor_DroneCAN::handle_battery_info_aux_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryInfoAux &msg)
 {
-    AP_BattMonitor_DroneCAN* driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, msg.battery_id);
+    const auto &batt = AP::battery();
+    AP_BattMonitor_DroneCAN *driver = nullptr;
+
+    /*
+      check for a backend with AllowSplitAuxInfo set, allowing InfoAux
+      from a different CAN node than the base battery information
+     */
+    for (uint8_t i = 0; i < batt._num_instances; i++) {
+        const auto *drv = batt.drivers[i];
+        if (drv != nullptr &&
+            batt.get_type(i) == AP_BattMonitor::Type::UAVCAN_BatteryInfo &&
+            drv->option_is_set(AP_BattMonitor_Params::Options::AllowSplitAuxInfo) &&
+            batt.get_serial_number(i) == int32_t(msg.battery_id)) {
+            driver = (AP_BattMonitor_DroneCAN *)batt.drivers[i];
+            if (driver->_ap_dronecan == nullptr) {
+                /* we have not received the main battery information
+                   yet. Discard InfoAux until we do so we can init the
+                   backend with the right node ID
+                 */
+                return;
+            }
+            break;
+        }
+    }
+    if (driver == nullptr) {
+        driver = get_dronecan_backend(ap_dronecan, transfer.source_node_id, msg.battery_id);
+    }
     if (driver == nullptr) {
         return;
     }

libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h

@@ -63,10 +63,7 @@ class AP_BattMonitor_DroneCAN : public AP_BattMonitor_Backend
     void handle_battery_info_aux(const ardupilot_equipment_power_BatteryInfoAux &msg);
     void update_interim_state(const float voltage, const float current, const float temperature_K, const uint8_t soc);
 
-    static bool match_battery_id(uint8_t instance, uint8_t battery_id) {
-        // when serial number is negative, all batteries are accepted. Else, it must match
-        return (AP::battery().get_serial_number(instance) < 0) || (AP::battery().get_serial_number(instance) == (int32_t)battery_id);
-    }
+    static bool match_battery_id(uint8_t instance, uint8_t battery_id);
 
     // MPPT related enums and methods
     enum class MPPT_FaultFlags : uint8_t {

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -150,7 +150,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
+    // @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
     // @User: Advanced
     AP_GROUPINFO("OPTIONS", 21, AP_BattMonitor_Params, _options, 0),
 #endif // HAL_BUILD_AP_PERIPH

libraries/AP_BattMonitor/AP_BattMonitor_Params.h

@@ -25,6 +25,7 @@ class AP_BattMonitor_Params {
         MPPT_Power_Off_At_Boot              = (1U<<4),  // MPPT Disabled at startup (aka boot), if HW supports it
         MPPT_Power_On_At_Boot               = (1U<<5),  // MPPT Enabled at startup (aka boot), if HW supports it. If Power_Off_at_Boot is also set, the behavior is Power_Off_at_Boot
         GCS_Resting_Voltage                 = (1U<<6),  // send resistance resting voltage to GCS
+        AllowSplitAuxInfo                   = (1U<<7),  // allow different node to provide aux info for DroneCAN
     };
 
     BattMonitor_LowVoltage_Source failsafe_voltage_source(void) const { return (enum BattMonitor_LowVoltage_Source)_failsafe_voltage_source.get(); }