From 11f0c5a3e680580831d3781b466cef21a0628bc8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Igor=20Mi=C5=A1i=C4=87?= <igy1000mb@gmail.com>
Date: Thu, 4 Mar 2021 14:06:06 +0100
Subject: [PATCH] mavlink_mission: sync transfer_dataman_id with mission.cpp

---
 src/modules/mavlink/mavlink_mission.cpp | 34 +++++++++++++++++++++++--
 src/modules/mavlink/mavlink_mission.h   |  9 +++++++
 src/modules/navigator/mission.cpp       | 28 +++++++++++++++++++-
 3 files changed, 68 insertions(+), 3 deletions(-)

diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index b4b2ab0b73..7c8a8f94ca 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -943,8 +943,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 			_transfer_partner_sysid = msg->sysid;
 			_transfer_partner_compid = msg->compid;
 			_transfer_count = wpc.count;
-			_transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
-						DM_KEY_WAYPOINTS_OFFBOARD_0);	// use inactive storage for transmission
+			_transfer_dataman_id = next_transfer_dataman_id();
 			_transfer_current_seq = -1;
 
 			if (_mission_type == MAV_MISSION_TYPE_FENCE) {
@@ -994,6 +993,37 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 	}
 }
 
+dm_item_t
+MavlinkMissionManager::next_transfer_dataman_id()
+{
+	dm_item_t transfer_dataman_id = _dataman_id;
+	/* lock MISSION_STATE item */
+	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+
+	if (dm_lock_ret != 0) {
+		PX4_ERR("DM_KEY_MISSION_STATE lock failed");
+	}
+
+	mission_s mission_state;
+	int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
+
+	/* unlock MISSION_STATE item */
+	if (dm_lock_ret == 0) {
+		dm_unlock(DM_KEY_MISSION_STATE);
+	}
+
+	if (ret == sizeof(mission_s)) {
+		_dataman_id = (dm_item_t)mission_state.dataman_id;
+		transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
+				       DM_KEY_WAYPOINTS_OFFBOARD_0);	// use inactive storage for transmission
+
+	} else {
+		PX4_ERR("Can't read DM_KEY_MISSION_STATE");
+	}
+
+	return transfer_dataman_id;
+}
+
 void
 MavlinkMissionManager::switch_to_idle_state()
 {
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
index 6740f0c697..93847b3ff4 100644
--- a/src/modules/mavlink/mavlink_mission.h
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -216,6 +216,15 @@ private:
 
 	void handle_mission_count(const mavlink_message_t *msg);
 
+	/**
+	 * Helper function to determine next transfer dataman id.
+	 * It reads DM_KEY_MISSION_STATE which stores previous valid dataman_id.
+	 * Since mavlink_mission module and mission module communicate over dataman,
+	 * mission module is restoring dataman_id for invalid mission to previous valid mission
+	 * if valid mission exist.
+	 */
+	dm_item_t next_transfer_dataman_id();
+
 	void handle_mission_item(const mavlink_message_t *msg);
 	void handle_mission_item_int(const mavlink_message_t *msg);
 	void handle_mission_item_both(const mavlink_message_t *msg);
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 18304a03a1..485dc63b43 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -565,6 +565,8 @@ Mission::update_mission()
 bool
 Mission::restore_old_mission()
 {
+	bool success = false;
+
 	_mission = _old_mission;
 
 	/* check if valid. It could be invalid if there is no previous mission. */
@@ -575,9 +577,33 @@ Mission::restore_old_mission()
 		_mission_changed = true;
 		_navigator->get_mission_result()->failure = false;
 		_current_mission_index = _old_mission.current_seq;
+
+		/* lock MISSION_STATE item */
+		int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+
+		if (dm_lock_ret != 0) {
+			PX4_ERR("DM_KEY_MISSION_STATE lock failed");
+		}
+
+		/* Since mission is invalid restore DM_KEY_MISSION_STATE with old dataman_id.
+		 * This helps mavlink_mission module to not overwrite the valid mission if two or more invalid missions are sent.
+		 */
+		int res =  dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &_mission, sizeof(mission_s));
+
+		/* unlock MISSION_STATE item */
+		if (dm_lock_ret == 0) {
+			dm_unlock(DM_KEY_MISSION_STATE);
+		}
+
+		if (res == sizeof(mission_s)) {
+			success = true;
+
+		} else {
+			PX4_ERR("Can't write DM_KEY_MISSION_STATE. The next invalid mission will not be restored!");
+		}
 	}
 
-	return valid;
+	return success;
 }
 
 void