From 01c6765b6f97026f8aab419b06ba0bc2fbd07c13 Mon Sep 17 00:00:00 2001 From: Thomas Watson Date: Sat, 16 Nov 2024 21:21:13 -0600 Subject: [PATCH] AP_Mount: 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_Mount/AP_Mount_Xacti.cpp | 18 +++++------------- libraries/AP_Mount/AP_Mount_Xacti.h | 2 +- 2 files changed, 6 insertions(+), 14 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Xacti.cpp b/libraries/AP_Mount/AP_Mount_Xacti.cpp index eed9638375..410de2660c 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.cpp +++ b/libraries/AP_Mount/AP_Mount_Xacti.cpp @@ -416,21 +416,13 @@ void AP_Mount_Xacti::send_target_angles(float pitch_rad, float yaw_rad, bool yaw } // subscribe to Xacti DroneCAN messages -void AP_Mount_Xacti::subscribe_msgs(AP_DroneCAN* ap_dronecan) +bool AP_Mount_Xacti::subscribe_msgs(AP_DroneCAN* ap_dronecan) { - // return immediately if DroneCAN is unavailable - if (ap_dronecan == nullptr) { - GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "%s DroneCAN subscribe failed", send_text_prefix); - return; - } + const auto driver_index = ap_dronecan->get_driver_index(); - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_gimbal_attitude_status, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("gimbal_attitude_status_sub"); - } - - if (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_gnss_status_req, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("gnss_status_req_sub"); - } + return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_gimbal_attitude_status, driver_index) != nullptr) + && (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_gnss_status_req, driver_index) != nullptr) + ; } // register backend in detected modules array used to map DroneCAN port and node id to backend diff --git a/libraries/AP_Mount/AP_Mount_Xacti.h b/libraries/AP_Mount/AP_Mount_Xacti.h index 71ece3aed8..14643a7acc 100644 --- a/libraries/AP_Mount/AP_Mount_Xacti.h +++ b/libraries/AP_Mount/AP_Mount_Xacti.h @@ -75,7 +75,7 @@ public: void send_camera_settings(mavlink_channel_t chan) const override; // subscribe to Xacti DroneCAN messages - static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + static bool subscribe_msgs(AP_DroneCAN* ap_dronecan); // xacti specific message handlers static void handle_gimbal_attitude_status(AP_DroneCAN* ap_dronecan, const CanardRxTransfer& transfer, const com_xacti_GimbalAttitudeStatus &msg);