From d86bab9c58b287a91e30039b0a80190202e1511e Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 16 Nov 2024 21:21:13 -0600 Subject: [PATCH] AP_Compass: 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_Compass/AP_Compass_DroneCAN.cpp | 20 ++++++-------------- libraries/AP_Compass/AP_Compass_DroneCAN.h | 2 +- 2 files changed, 7 insertions(+), 15 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_DroneCAN.cpp b/libraries/AP_Compass/AP_Compass_DroneCAN.cpp index 920a47a429..50008d8424 100644 --- a/libraries/AP_Compass/AP_Compass_DroneCAN.cpp +++ b/libraries/AP_Compass/AP_Compass_DroneCAN.cpp @@ -37,24 +37,16 @@ AP_Compass_DroneCAN::AP_Compass_DroneCAN(AP_DroneCAN* ap_dronecan, uint32_t devi { } -void AP_Compass_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) +bool AP_Compass_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) { - if (ap_dronecan == nullptr) { - return; - } - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("mag_sub"); - } - - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_2, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("mag2_sub"); - } + const auto driver_index = ap_dronecan->get_driver_index(); + return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field, driver_index) != nullptr) + && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_2, driver_index) != nullptr) #if AP_COMPASS_DRONECAN_HIRES_ENABLED - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_hires, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("mag3_sub"); - } + && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_magnetic_field_hires, driver_index) != nullptr) #endif + ; } AP_Compass_Backend* AP_Compass_DroneCAN::probe(uint8_t index) diff --git a/libraries/AP_Compass/AP_Compass_DroneCAN.h b/libraries/AP_Compass/AP_Compass_DroneCAN.h index 7a50f41406..927821e5ef 100644 --- a/libraries/AP_Compass/AP_Compass_DroneCAN.h +++ b/libraries/AP_Compass/AP_Compass_DroneCAN.h @@ -14,7 +14,7 @@ public: void read(void) override; - static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + static bool subscribe_msgs(AP_DroneCAN* ap_dronecan); static AP_Compass_Backend* probe(uint8_t index); static uint32_t get_detected_devid(uint8_t index) { return _detected_modules[index].devid; } static void handle_magnetic_field(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_ahrs_MagneticFieldStrength& msg);