From 7856294334039d0a277cc1e7045bb55348a4e5ff Mon Sep 17 00:00:00 2001 From: Patrick Menschel Date: Fri, 13 Dec 2024 12:19:19 +0100 Subject: [PATCH 1/2] GCS_MAVLINK: Use MISSION_REQUEST_INT instead of MISSION_REQUEST Use MISSION_REQUEST_INT to request mission items from ground control station. Fallback to the deprecated MISSION_REQUEST after the first timeout and if the ground control station then answers, send a text warning to be displayed in the ground station. --- libraries/GCS_MAVLink/MissionItemProtocol.cpp | 36 ++++++++++++++----- libraries/GCS_MAVLink/MissionItemProtocol.h | 6 ++++ 2 files changed, 34 insertions(+), 8 deletions(-) diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.cpp b/libraries/GCS_MAVLink/MissionItemProtocol.cpp index 5667578ad064e..a99788f56ad12 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol.cpp +++ b/libraries/GCS_MAVLink/MissionItemProtocol.cpp @@ -27,6 +27,8 @@ void MissionItemProtocol::init_send_requests(GCS_MAVLINK &_link, mission_item_warning_sent = false; mission_request_warning_sent = false; + mission_request_int_fallback_to_mission_request = false; + mission_request_int_warning_sent = false; } void MissionItemProtocol::handle_mission_clear_all(const GCS_MAVLINK &_link, @@ -315,7 +317,7 @@ void MissionItemProtocol::handle_mission_item(const mavlink_message_t &msg, cons return; } // if we have enough space, then send the next WP request immediately - if (HAVE_PAYLOAD_SPACE(link->get_chan(), MISSION_REQUEST)) { + if (mission_request_int_fallback_to_mission_request ? HAVE_PAYLOAD_SPACE(link->get_chan(), MISSION_REQUEST) : HAVE_PAYLOAD_SPACE(link->get_chan(), MISSION_REQUEST_INT)) { queued_request_send(); } else { link->send_message(next_item_ap_message_id()); @@ -368,13 +370,24 @@ void MissionItemProtocol::queued_request_send() INTERNAL_ERROR(AP_InternalError::error_t::gcs_bad_missionprotocol_link); return; } - CHECK_PAYLOAD_SIZE2_VOID(link->get_chan(), MISSION_REQUEST); - mavlink_msg_mission_request_send( - link->get_chan(), - dest_sysid, - dest_compid, - request_i, - mission_type()); + if (mission_request_int_fallback_to_mission_request) { + CHECK_PAYLOAD_SIZE2_VOID(link->get_chan(), MISSION_REQUEST); + mavlink_msg_mission_request_int_send( + link->get_chan(), + dest_sysid, + dest_compid, + request_i, + mission_type()); + } else { + CHECK_PAYLOAD_SIZE2_VOID(link->get_chan(), MISSION_REQUEST_INT); + mavlink_msg_mission_request_send( + link->get_chan(), + dest_sysid, + dest_compid, + request_i, + mission_type()); +} + timelast_request_ms = AP_HAL::millis(); } @@ -408,8 +421,15 @@ void MissionItemProtocol::update() // resend request if we haven't gotten one: const uint32_t wp_recv_timeout_ms = 1000U + link->get_stream_slowdown_ms(); if (tnow - timelast_request_ms > wp_recv_timeout_ms) { + if (not mission_request_int_fallback_to_mission_request) { + mission_request_int_fallback_to_mission_request = true; + } timelast_request_ms = tnow; link->send_message(next_item_ap_message_id()); + } else if ((mission_request_int_fallback_to_mission_request) + && (not mission_request_int_warning_sent)) { + mission_request_int_warning_sent = true; + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "This GCS ignores MISSION_REQUEST_INT; MISSION_REQUEST support will soon end!"); } } diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.h b/libraries/GCS_MAVLink/MissionItemProtocol.h index 37f37fd559716..e22b0819cb0da 100644 --- a/libraries/GCS_MAVLink/MissionItemProtocol.h +++ b/libraries/GCS_MAVLink/MissionItemProtocol.h @@ -101,6 +101,12 @@ class MissionItemProtocol // then we issue a warning - once per transfer. bool mission_item_warning_sent = false; + // if the autopilot sends a mission_request_int to a GCS and runs into a timeout, + // it does fallback to mission_request instead, ... + bool mission_request_int_fallback_to_mission_request = false; + // ... and we issue a warning - once per transfer. + bool mission_request_int_warning_sent = false; + // support for GCS getting waypoints etc from us: virtual MAV_MISSION_RESULT get_item(const GCS_MAVLINK &_link, const mavlink_message_t &msg, From eaee905875b32d7b34217448dcc64ca672c4c3e3 Mon Sep 17 00:00:00 2001 From: Patrick Menschel Date: Tue, 17 Dec 2024 20:43:48 +0100 Subject: [PATCH 2/2] GCS_Mavlink: Fix advertised autopilot capabilities Some capabilities are available but not advertised. FIXME: MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT is deprecated! --- libraries/GCS_MAVLink/GCS_Common.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index e670339e7b756..ab4feb9c26cdc 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -7098,8 +7098,20 @@ bool GCS_MAVLINK::mavlink_coordinate_frame_to_location_alt_frame(const MAV_FRAME uint64_t GCS_MAVLINK::capabilities() const { + /** + * Basic capabilities, changed December 2024 + * - Param float (FIXME: deprecated since 2022) + * - Compass calibration (already there, not checked) + * - Mission Int (added, code is there) + * - Command Int (added, code is there) + * - Set Position Target Global Int (added, code is there, used for guided mode) + * - param encode c cast (TODO: should be added, code is there) + */ uint64_t ret = MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | - MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION; + MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION | + MAV_PROTOCOL_CAPABILITY_MISSION_INT | + MAV_PROTOCOL_CAPABILITY_COMMAND_INT | + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT; const auto mavlink_protocol = uartstate->get_protocol(); if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2 || mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL) {