diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp index 123b3408c3..9ba5a98dad 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.cpp @@ -26,15 +26,11 @@ AP_OpticalFlow_HereFlow::AP_OpticalFlow_HereFlow(AP_OpticalFlow &flow) : } //links the HereFlow messages to the backend -void AP_OpticalFlow_HereFlow::subscribe_msgs(AP_DroneCAN* ap_dronecan) +bool AP_OpticalFlow_HereFlow::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_measurement, ap_dronecan->get_driver_index()) == nullptr) { - AP_BoardConfig::allocation_error("measurement_sub"); - } + return (Canard::allocate_sub_arg_callback(ap_dronecan, &handle_measurement, driver_index) != nullptr); } //updates driver states based on received HereFlow messages diff --git a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h index afc7584163..7b457bf6c9 100644 --- a/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h +++ b/libraries/AP_OpticalFlow/AP_OpticalFlow_HereFlow.h @@ -15,7 +15,7 @@ public: void update() override; - static void subscribe_msgs(AP_DroneCAN* ap_dronecan); + static bool subscribe_msgs(AP_DroneCAN* ap_dronecan); static void handle_measurement(AP_DroneCAN *ap_dronecan, const CanardRxTransfer& transfer, const com_hex_equipment_flow_Measurement &msg);