From 9f7ed65bc0b57a85bbed87aef1d023e6e5c580ce Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 16 Nov 2024 21:21:13 -0600 Subject: [PATCH] AP_BattMonitor: optimize DroneCAN subscription process * remove unnecessary nullptr check, these are always called from an initialized AP_DroneCAN so if it's nullptr something has gone horrifically wrong * pass in driver index instead of repeatedly calling function to get it * simplify error handling; knowing exactly which allocation failed is not super helpful and one failing likely means subsequent ones will too, as it can only fail due to being out of memory --- .../AP_BattMonitor_DroneCAN.cpp | 21 ++++++------------- .../AP_BattMonitor/AP_BattMonitor_DroneCAN.h | 2 +- 2 files changed, 7 insertions(+), 16 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index 24bb0b69f5..d505dcc20d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp @@ -42,23 +42,14 @@ AP_BattMonitor_DroneCAN::AP_BattMonitor_DroneCAN(AP_BattMonitor &mon, AP_BattMon _state.healthy = false; } -void AP_BattMonitor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) +bool AP_BattMonitor_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) { - if (ap_dronecan == nullptr) { - return; - } + const auto driver_index = ap_dronecan->get_driver_index(); - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_trampoline, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("battinfo_sub"); - } - - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_aux_trampoline, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("battinfo_aux_sub"); - } - - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_mppt_stream_trampoline, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("mppt_stream_sub"); - } + return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_trampoline, driver_index) != nullptr) + && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_battery_info_aux_trampoline, driver_index) != nullptr) + && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_mppt_stream_trampoline, driver_index) != nullptr) + ; } /* diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h index a25ef96678..7d34b91acf 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.h @@ -47,7 +47,7 @@ public: // return mavlink fault bitmask (see MAV_BATTERY_FAULT enum) uint32_t get_mavlink_fault_bitmask() const override; - static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + static bool subscribe_msgs(AP_DroneCAN* ap_dronecan); static AP_BattMonitor_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t battery_id); static void handle_battery_info_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_power_BatteryInfo &msg); static void handle_battery_info_aux_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const ardupilot_equipment_power_BatteryInfoAux &msg);