diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index e670339e7b756..334d4b6b95bd3 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 { - uint64_t ret = MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | - MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION; + /** + * Basic capabilities, changed December 2024 + * - Param float (obsolete, replaced by param encode c cast, therefore removed) + * - 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 (added, code is there) + */ + uint64_t ret = MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION | + MAV_PROTOCOL_CAPABILITY_MISSION_INT | + MAV_PROTOCOL_CAPABILITY_COMMAND_INT | + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT | + MAV_PROTOCOL_CAPABILITY_PARAM_ENCODE_C_CAST; const auto mavlink_protocol = uartstate->get_protocol(); if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2 || mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL) { diff --git a/libraries/GCS_MAVLink/MissionItemProtocol.cpp b/libraries/GCS_MAVLink/MissionItemProtocol.cpp index 5667578ad064e..f2260860e51a7 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,10 @@ 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 (HAVE_PAYLOAD_SPACE(link->get_chan(), + mission_request_int_fallback_to_mission_request ? MISSION_REQUEST : MISSION_REQUEST_INT + ) + ) { queued_request_send(); } else { link->send_message(next_item_ap_message_id()); @@ -368,13 +373,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_INT); + 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); + mavlink_msg_mission_request_send( + link->get_chan(), + dest_sysid, + dest_compid, + request_i, + mission_type()); +} + timelast_request_ms = AP_HAL::millis(); } @@ -408,8 +424,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 (!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) + && (!mission_request_int_warning_sent)) { + mission_request_int_warning_sent = true; + GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "GCS ignored MISSION_REQUEST_INT; fallback to MISSION_REQUEST"); } } 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,