From 7695b65b7f595b69c3ae4a4a13c777368d59b288 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Mar 2017 22:48:38 +0100 Subject: [PATCH] MAVLink app: Default to MISSION, not MISSION_INT In order to ensure correct transmission the mission system needs to default to the legacy protocol and switch to the new implementation when these conditions are met: * If the GCS sends a MISSION_REQUEST_INT - it will do this based on the AUTOPILOT_VERSION flag indicating int mission support * If the autopilot sends a MISSION_REQUEST and has the AUTOPILOT_VERSION flag for 2.0 set, the GCS should NACK it, which will make the autopilot retry a MISSION_REQUEST_INT * If the autopilot sends a MISSION_REQUEST_INT and the GCS does not support it, the GCS will ignore and time out. The autopilot could retry now opportunistically with the old protocol, but this is not great for lossy links. * If the GCS sends a MISSION_ITEM_INT - this is a fallback --- src/modules/mavlink/mavlink_mission.cpp | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index d61e7e98ac..e7f1400dfe 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -73,7 +73,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m _time_last_reached(0), _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), - _int_mode(true), + _int_mode(false), _max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX), _filesystem_errcount(0), _my_dataman_id(0), @@ -86,7 +86,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m _offboard_mission_sub(-1), _mission_result_sub(-1), _offboard_mission_pub(nullptr), - _slow_rate_limiter(_interval / 10.0f), + _slow_rate_limiter(_interval / 5.0f), _verbose(false) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); @@ -297,6 +297,7 @@ void MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq) { if (seq < _max_count) { + _time_last_sent = hrt_absolute_time(); if (_int_mode) { @@ -490,6 +491,16 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) } _state = MAVLINK_WPM_STATE_IDLE; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + + // INT mode is not supported + if (_int_mode && wpa.type != MAV_MISSION_ACCEPTED) { + _int_mode = false; + + } else if (wpa.type != MAV_MISSION_ACCEPTED) { + _int_mode = true; + } } } else { @@ -721,11 +732,6 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) /* looks like our MISSION_REQUEST was lost, try again */ if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); } - // If the counterpart tries this, its an indication int mode might not be supported - if (_int_mode) { - _int_mode = false; - } - } else { if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); }