From fbeab64be2138bfef99fcbb15334a151c5d8e49b Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 16 Nov 2024 21:21:13 -0600 Subject: [PATCH] AP_RangeFinder: 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 --- libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.cpp | 11 ++++------- libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.h | 2 +- 2 files changed, 5 insertions(+), 8 deletions(-) diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.cpp index 94c3aead5a..37c623b531 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.cpp @@ -13,14 +13,11 @@ extern const AP_HAL::HAL& hal; #define debug_range_finder_uavcan(level_debug, can_driver, fmt, args...) do { if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) { hal.console->printf(fmt, ##args); }} while (0) //links the rangefinder uavcan message to this backend -void AP_RangeFinder_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) +bool AP_RangeFinder_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) { - if (ap_dronecan == nullptr) { - return; - } - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("measurement_sub"); - } + const auto driver_index = ap_dronecan->get_driver_index(); + + return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, driver_index) != nullptr); } //Method to find the backend relating to the node id diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.h index 6d763b2b97..6f575db839 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_DroneCAN.h @@ -16,7 +16,7 @@ public: void update() override; - static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + static bool subscribe_msgs(AP_DroneCAN* ap_dronecan); static AP_RangeFinder_DroneCAN* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id, uint8_t address, bool create_new); static AP_RangeFinder_Backend* detect(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);