From cd1118acb47a306feb8e7c442da57496b2d7e1f6 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 16 Nov 2024 21:21:13 -0600 Subject: [PATCH] AP_Airspeed: 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_Airspeed/AP_Airspeed_DroneCAN.cpp | 16 +++++----------- libraries/AP_Airspeed/AP_Airspeed_DroneCAN.h | 2 +- 2 files changed, 6 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.cpp b/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.cpp index 4f0b3da42f..8d3dff897a 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.cpp @@ -13,21 +13,15 @@ extern const AP_HAL::HAL& hal; AP_Airspeed_DroneCAN::DetectedModules AP_Airspeed_DroneCAN::_detected_modules[]; HAL_Semaphore AP_Airspeed_DroneCAN::_sem_registry; -void AP_Airspeed_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) +bool AP_Airspeed_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) { - if (ap_dronecan == nullptr) { - return; - } - - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_airspeed, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("airspeed_sub"); - } + const auto driver_index = ap_dronecan->get_driver_index(); + return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_airspeed, driver_index) != nullptr) #if AP_AIRSPEED_HYGROMETER_ENABLE - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_hygrometer, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("hygrometer_sub"); - } + && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_hygrometer, driver_index) != nullptr) #endif + ; } AP_Airspeed_Backend* AP_Airspeed_DroneCAN::probe(AP_Airspeed &_frontend, uint8_t _instance, uint32_t previous_devid) diff --git a/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.h b/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.h index 2167f9f061..7d92c27f95 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.h +++ b/libraries/AP_Airspeed/AP_Airspeed_DroneCAN.h @@ -26,7 +26,7 @@ public: bool get_hygrometer(uint32_t &last_sample_ms, float &temperature, float &humidity) override; #endif - static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + static bool subscribe_msgs(AP_DroneCAN* ap_dronecan); static AP_Airspeed_Backend* probe(AP_Airspeed &_frontend, uint8_t _instance, uint32_t previous_devid);