diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index d4b7aed2a9..8036ac4e5a 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp +++ b/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; } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h index 6b6151d477..819ade7316 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h @@ -63,10 +63,7 @@ private: 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 {