Move MISSION_STATE read/write from mavlink to navigator and commander

3 files changed, 38 insertions, 7 deletions

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 9244063b1..d9d8353f6 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -431,6 +431,37 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 }
 
 void
+Mission::save_offboard_mission_state()
+{
+	mission_s mission_state;
+
+	/* lock MISSION_STATE item */
+	dm_lock(DM_KEY_MISSION_STATE);
+
+	/* read current state */
+	int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
+
+	/* check if state actually changed to save flash write cycles */
+	if (read_res != sizeof(mission_s) || mission_state.dataman_id != _offboard_mission.dataman_id ||
+			mission_state.count != _offboard_mission.count ||
+	    		mission_state.current_index != _current_offboard_mission_index) {
+
+		mission_state.dataman_id = _offboard_mission.dataman_id;
+		mission_state.count = _offboard_mission.count;
+		mission_state.current_index = _current_offboard_mission_index;
+
+		/* write modifyed state only if changed */
+		if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "error saving mission state");
+
+		}
+	}
+
+	/* unlock MISSION_STATE item */
+	dm_unlock(DM_KEY_MISSION_STATE);
+}
+
+void
 Mission::report_mission_item_reached()
 {
 	if (_mission_type == MISSION_TYPE_OFFBOARD) {
@@ -445,6 +476,8 @@ Mission::report_current_offboard_mission_item()
 {
 	_mission_result.index_current_mission = _current_offboard_mission_index;
 	publish_mission_result();
+
+	save_offboard_mission_state();
 }
 
 void
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 322aaf96a..98b11bade 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -142,6 +142,11 @@ private:
 			       struct mission_item_s *new_mission_item);
 
 	/**
+	 * Save current offboard mission state to dataman
+	 */
+	void save_offboard_mission_state();
+
+	/**
 	 * Report that a mission item has been reached
 	 */
 	void report_mission_item_reached();
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 888c8e7f4..a3c190c7f 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -227,13 +227,6 @@ Navigator::task_main()
 			warnx("Could not clear geofence");
 	}
 
-	/* Get the last offboard mission id */
-	persistent_system_state_t sys_state;
-	if (dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) {
-		if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1))
-			_mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id);
-	}
-
 	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));