From 9c86cdab2ef637c0559d1e33315f7492d9c0dcf3 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 1 Feb 2018 22:59:22 +0100
Subject: [PATCH] Robustify mission upload/download (take 2) (#8794)

* Revert "mavlink_mission: don't retransmit automatically"

This reverts commit 4e008fe89120e24e321e93fb551753242797f022.

* mavlink_mission: don't retry to send mission item

The mission items need to be requested one by one by a ground station.
This is a "pull" protocol and we should not retry to "push" the mission
items down.

If we do this we can trigger the situation where the autopilot keeps
retrying and the ground station does not time out because it keeps
receiving items (even though the items are the wrong ones).

* mavlink_mission: reduce retry timeout

When actively re-requesting lost mission items, we can be more agressive
and therefore lose less time when a mission item is lost on the way.
---
 src/modules/mavlink/mavlink_mission.cpp | 18 ++++++++++++++++--
 src/modules/mavlink/mavlink_mission.h   |  5 +++--
 2 files changed, 19 insertions(+), 4 deletions(-)

diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 906a806810..4380ff9f5a 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -296,6 +296,8 @@ MavlinkMissionManager::send_mission_current(uint16_t seq)
 void
 MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count, MAV_MISSION_TYPE mission_type)
 {
+	_time_last_sent = hrt_absolute_time();
+
 	mavlink_mission_count_t wpc;
 
 	wpc.target_system = sysid;
@@ -362,6 +364,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 	}
 
 	if (read_success) {
+		_time_last_sent = hrt_absolute_time();
+
 		if (_int_mode) {
 			mavlink_mission_item_int_t wp;
 			format_mavlink_mission_item(&mission_item, reinterpret_cast<mavlink_mission_item_t *>(&wp));
@@ -426,6 +430,8 @@ void
 MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq)
 {
 	if (seq < current_max_item_count()) {
+		_time_last_sent = hrt_absolute_time();
+
 		if (_int_mode) {
 			mavlink_mission_request_int_t wpr;
 			wpr.target_system = sysid;
@@ -518,8 +524,15 @@ MavlinkMissionManager::send(const hrt_abstime now)
 	}
 
 	/* check for timed-out operations */
-	if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0)
-	    && hrt_elapsed_time(&_time_last_recv) > _operation_timeout_us) {
+	if (_state == MAVLINK_WPM_STATE_GETLIST && (_time_last_sent > 0)
+	    && hrt_elapsed_time(&_time_last_sent) > MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT) {
+
+		// try to request item again after timeout
+		send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq);
+
+	} else if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0)
+		   && hrt_elapsed_time(&_time_last_recv) > MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT) {
+
 		_mavlink->send_statustext_critical("Operation timeout");
 
 		PX4_DEBUG("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state);
@@ -531,6 +544,7 @@ MavlinkMissionManager::send(const hrt_abstime now)
 
 	} else if (_state == MAVLINK_WPM_STATE_IDLE) {
 		// reset flags
+		_time_last_sent = 0;
 		_time_last_recv = 0;
 	}
 }
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
index fd85e8c1ec..8a829a4d1f 100644
--- a/src/modules/mavlink/mavlink_mission.h
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -67,6 +67,8 @@ enum MAVLINK_WPM_CODES {
 	MAVLINK_WPM_CODE_ENUM_END
 };
 
+static constexpr uint64_t MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT = 5000000; ///< Protocol action timeout in us
+static constexpr uint64_t MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT = 250000; ///< Protocol retry timeout in us
 
 class Mavlink;
 
@@ -92,13 +94,12 @@ private:
 	enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION};	///< mission type of current transmission (only one at a time possible)
 
 	uint64_t		_time_last_recv{0};
+	uint64_t		_time_last_sent{0};
 
 	uint8_t			_reached_sent_count{0};			///< last time when the vehicle reached a waypoint
 
 	bool			_int_mode{false};			///< Use accurate int32 instead of float
 
-	static constexpr uint32_t _operation_timeout_us = 5000000;
-
 	unsigned		_filesystem_errcount{0};		///< File system error count
 
 	static dm_item_t		_dataman_id;				///< Global Dataman storage ID for active mission