From 4ade22c94ba061d09fb7a577596bcacf5ac77f76 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 11 Feb 2015 19:50:51 +1100 Subject: [PATCH] Copter: use common send_autopilot_version() --- ArduCopter/GCS_Mavlink.pde | 50 +------------------------------------- 1 file changed, 1 insertion(+), 49 deletions(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 3f4b315d79..9825c65b5a 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -128,54 +128,6 @@ static NOINLINE void send_attitude(mavlink_channel_t chan) gyro.z); } -static NOINLINE void send_autopilot_version(mavlink_channel_t chan) -{ - uint16_t capabilities = 0; - uint32_t flight_sw_version = 0; - uint32_t middleware_sw_version = 0; - uint32_t os_sw_version = 0; - uint32_t board_version = 0; - uint8_t flight_custom_version[8]; - uint8_t middleware_custom_version[8]; - uint8_t os_custom_version[8]; - uint16_t vendor_id = 0; - uint16_t product_id = 0; - uint64_t uid = 0; - -#if defined(GIT_VERSION) - memcpy(flight_custom_version, GIT_VERSION, 8); -#else - memset(middleware_custom_version,0,8); -#endif - -#if defined(PX4_GIT_VERSION) - memcpy(middleware_custom_version, PX4_GIT_VERSION, 8); -#else - memset(middleware_custom_version,0,8); -#endif - -#if defined(NUTTX_GIT_VERSION) - memcpy(os_custom_version, NUTTX_GIT_VERSION, 8); -#else - memset(os_custom_version,0,8); -#endif - - mavlink_msg_autopilot_version_send( - chan, - capabilities, - flight_sw_version, - middleware_sw_version, - os_sw_version, - board_version, - flight_custom_version, - middleware_custom_version, - os_custom_version, - vendor_id, - product_id, - uid - ); -} - #if AC_FENCE == ENABLED static NOINLINE void send_limits_status(mavlink_channel_t chan) { @@ -1628,7 +1580,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #endif // AC_RALLY == ENABLED case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: - send_autopilot_version(chan); + gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); break;