From d9cabde13e01e404c5b1e0527c6964b41241befa Mon Sep 17 00:00:00 2001
From: Andreas Antener <antener_a@gmx.ch>
Date: Sun, 20 Mar 2016 13:40:19 +0100
Subject: [PATCH] reset mission after vehicle was in mission and then disarms

---
 src/modules/navigator/mission.cpp | 82 +++++++++++++++++++++++++++++++
 src/modules/navigator/mission.h   | 11 +++++
 2 files changed, 93 insertions(+)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 62300d0926..9e5aa0c69a 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -80,6 +80,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
 	_mission_type(MISSION_TYPE_NONE),
 	_inited(false),
 	_home_inited(false),
+	_need_mission_reset(false),
 	_missionFeasibilityChecker(),
 	_min_current_sp_distance_xy(FLT_MAX),
 	_mission_item_previous_alt(NAN),
@@ -111,6 +112,12 @@ Mission::on_inactive()
 			update_offboard_mission();
 		}
 
+		/* reset the current offboard mission if needed */
+		if (need_to_reset_mission(false)) {
+			reset_offboard_mission(_offboard_mission);
+			update_offboard_mission();
+		}
+
 	} else {
 
 		/* load missions from storage */
@@ -164,6 +171,13 @@ Mission::on_active()
 		update_offboard_mission();
 	}
 
+	/* reset the current offboard mission if needed */
+	if (need_to_reset_mission(true)) {
+		reset_offboard_mission(_offboard_mission);
+		update_offboard_mission();
+		offboard_updated = true;
+	}
+
 	/* reset mission items if needed */
 	if (onboard_updated || offboard_updated) {
 		set_mission_items();
@@ -1038,3 +1052,71 @@ Mission::check_mission_valid()
 
 	return _navigator->get_mission_result()->valid;
 }
+
+void
+Mission::reset_offboard_mission(struct mission_s &mission)
+{
+	dm_lock(DM_KEY_MISSION_STATE);
+
+	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
+		if (mission.dataman_id >= 0 && mission.dataman_id <= 1) {
+			/* set current item to 0 */
+			mission.current_seq = 0;
+
+			/* reset jump counters */
+			if (mission.count > 0) {
+				dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
+
+				for (int index = 0; index < mission.count; index++) {
+					struct mission_item_s item;
+					const ssize_t len = sizeof(struct mission_item_s);
+
+					if (dm_read(dm_current, index, &item, len) != len) {
+						PX4_WARN("could not read mission item during reset");
+						break;
+					}
+
+					if (item.nav_cmd == NAV_CMD_DO_JUMP) {
+						item.do_jump_current_count = 0;
+
+						if (dm_write(dm_current, index, DM_PERSIST_POWER_ON_RESET,
+							     &item, len) != len) {
+							PX4_WARN("could not save mission item during reset");
+							break;
+						}
+					}
+				}
+			}
+
+		} else {
+			mavlink_and_console_log_critical(_navigator->get_mavlink_fd(), "ERROR: could not read mission");
+
+			/* initialize mission state in dataman */
+			mission.dataman_id = 0;
+			mission.count = 0;
+			mission.current_seq = 0;
+		}
+
+		dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s));
+
+		mavlink_and_console_log_info(_navigator->get_mavlink_fd(), "mission reset");
+	}
+
+	dm_unlock(DM_KEY_MISSION_STATE);
+}
+
+bool
+Mission::need_to_reset_mission(bool active)
+{
+	/* reset mission state when disarmed */
+	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset) {
+		_need_mission_reset = false;
+		return true;
+
+	} else if (_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED && active) {
+		/* mission is running, need reset after disarm */
+		_need_mission_reset = true;
+	}
+
+	return false;
+}
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 5d491df1c7..83b917a22a 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -203,6 +203,16 @@ private:
 	 */
 	bool check_mission_valid();
 
+	/**
+	 * Reset offboard mission
+	 */
+	void reset_offboard_mission(struct mission_s &mission);
+
+	/**
+	 * Returns true if we need to reset the mission
+	 */
+	bool need_to_reset_mission(bool active);
+
 	control::BlockParamInt _param_onboard_enabled;
 	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;
@@ -224,6 +234,7 @@ private:
 
 	bool _inited;
 	bool _home_inited;
+	bool _need_mission_reset;
 
 	MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */