From 5ef4c8dd0958fe705975ef14ce9e9d22498d2dba Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Fri, 24 Mar 2023 10:34:45 +1100 Subject: [PATCH] GCS_MAVLink: move AP_KDECAN to a first-class library --- libraries/GCS_MAVLink/GCS_Common.cpp | 24 +++--------------------- 1 file changed, 3 insertions(+), 21 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 0d4d4d13d7..3247e9c27d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -56,6 +56,7 @@ #include #include #include +#include #include "MissionItemProtocol_Waypoints.h" #include "MissionItemProtocol_Rally.h" @@ -77,10 +78,6 @@ #include #include - // To be replaced with macro saying if KDECAN library is included - #if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub) - #include - #endif #include #include #endif @@ -4355,29 +4352,14 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_can(const mavlink_command_long_ return MAV_RESULT_TEMPORARILY_REJECTED; } - bool start_stop = is_equal(packet.param1,1.0f); bool result = true; bool can_exists = false; + uint8_t num_drivers = AP::can().get_num_drivers(); for (uint8_t i = 0; i < num_drivers; i++) { switch (AP::can().get_driver_type(i)) { - case AP_CANManager::Driver_Type_KDECAN: { -// To be replaced with macro saying if KDECAN library is included -#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub) - AP_KDECAN *ap_kdecan = AP_KDECAN::get_kdecan(i); - - if (ap_kdecan != nullptr) { - can_exists = true; - result = ap_kdecan->run_enumeration(start_stop) && result; - } -#else - UNUSED_RESULT(start_stop); // prevent unused variable error -#endif - break; - } case AP_CANManager::Driver_Type_CANTester: { -// To be replaced with macro saying if KDECAN library is included #if (APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane) || APM_BUILD_TYPE(APM_BUILD_ArduSub)) && (HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_ENABLE_CANTESTER) CANTester *cantester = CANTester::get_cantester(i); @@ -4386,7 +4368,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_preflight_can(const mavlink_command_long_ result = cantester->run_kdecan_enumeration(start_stop) && result; } #else - UNUSED_RESULT(start_stop); // prevent unused variable error + (void)can_exists; #endif break; }