From 532ab09c726ac5c6b4c18a4b8b2757fb1eda4162 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 25 Oct 2023 08:31:50 +1100 Subject: [PATCH] AP_BattMonitor: added option allowing InfoAux to be from different node this allows the CAN node providing cell voltages to be a different CAN node from the node providing the base current/voltage --- .../AP_BattMonitor_DroneCAN.cpp | 28 ++++++++++++++++++- .../AP_BattMonitor/AP_BattMonitor_Params.cpp | 2 +- .../AP_BattMonitor/AP_BattMonitor_Params.h | 1 + 3 files changed, 29 insertions(+), 2 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index 8036ac4e5a..ddd5976c5f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp @@ -217,7 +217,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; } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index 01ec3bbded..f1d6522a6d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/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 diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 9366965328..fff2719f8f 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -25,6 +25,7 @@ public: 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(); }