From 360e54f871dcab489515748d3647e14a37e47f74 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 16 Nov 2024 21:21:13 -0600 Subject: [PATCH] AP_GPS: 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_GPS/AP_GPS_DroneCAN.cpp | 35 +++++++--------------------- libraries/AP_GPS/AP_GPS_DroneCAN.h | 2 +- 2 files changed, 10 insertions(+), 27 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp index 8bd8cb7e37..aeec044228 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.cpp @@ -89,36 +89,19 @@ AP_GPS_DroneCAN::~AP_GPS_DroneCAN() #endif } -void AP_GPS_DroneCAN::subscribe_msgs(AP_DroneCAN* ap_dronecan) +bool AP_GPS_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_fix2_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("status_sub"); - } - - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_aux_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("status_sub"); - } - - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_heading_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("status_sub"); - } - - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_status_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("status_sub"); - } + return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_fix2_msg_trampoline, driver_index) != nullptr) + && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_aux_msg_trampoline, driver_index) != nullptr) + && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_heading_msg_trampoline, driver_index) != nullptr) + && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_status_msg_trampoline, driver_index) != nullptr) #if GPS_MOVING_BASELINE - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_moving_baseline_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("moving_baseline_sub"); - } - - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_relposheading_msg_trampoline, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("relposheading_sub"); - } + && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_moving_baseline_msg_trampoline, driver_index) != nullptr) + && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_relposheading_msg_trampoline, driver_index) != nullptr) #endif + ; } AP_GPS_Backend* AP_GPS_DroneCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state) diff --git a/libraries/AP_GPS/AP_GPS_DroneCAN.h b/libraries/AP_GPS/AP_GPS_DroneCAN.h index 1de0b6b8f4..a0b62c8e5b 100644 --- a/libraries/AP_GPS/AP_GPS_DroneCAN.h +++ b/libraries/AP_GPS/AP_GPS_DroneCAN.h @@ -44,7 +44,7 @@ public: const char *name() const override { return _name; } - static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + static bool subscribe_msgs(AP_DroneCAN* ap_dronecan); static AP_GPS_Backend* probe(AP_GPS &_gps, AP_GPS::GPS_State &_state); static void handle_fix2_msg_trampoline(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const uavcan_equipment_gnss_Fix2& msg);