Restore old mission if a new mission is not feasible

msg/mission.msg

@@ -3,3 +3,6 @@ uint8 dataman_id	# default 0, there are two offboard storage places in the datam
 
 uint16 count		# count of the missions stored in the dataman
 int32 current_seq	# default -1, start at the one changed latest
+
+bool valid			# true if mission passed feasibility checks
+bool clear_mission	# true if mission is cleared in dataman

src/modules/mavlink/mavlink_mission.cpp

@@ -148,14 +148,15 @@ MavlinkMissionManager::load_safepoint_stats()
  * Publish mission topic to notify navigator about changes.
  */
 int
-MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq)
+MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool clear_mission)
 {
 	// We want to make sure the whole struct is initialized including padding before getting written by dataman.
 	mission_s mission{};
 	mission.timestamp = hrt_absolute_time();
 	mission.dataman_id = dataman_id;
 	mission.count = count;
 	mission.current_seq = seq;
+	mission.clear_mission = clear_mission;
 
 	/* update mission state in dataman */
 
@@ -486,6 +487,12 @@ MavlinkMissionManager::send()
 
 	if (_mission_result_sub.update(&mission_result)) {
 
+		/* If an infeasible mission is uploaded and the old feasible mission exists item count could be wrong.
+		 * This is restoring item count from the feasible mission. */
+		if (_count[MAV_MISSION_TYPE_MISSION] != mission_result.seq_total) {
+			_count[MAV_MISSION_TYPE_MISSION] = mission_result.seq_total;
+		}
+
 		if (_current_seq != mission_result.seq_current) {
 			_current_seq = mission_result.seq_current;
 
@@ -943,8 +950,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 +1000,42 @@ 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 if (ret == 0) {
+		//dataman is empty (new or formatted SD card)
+		transfer_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
+
+	} else {
+		PX4_ERR("Dataman can't read DM_KEY_MISSION_STATE. The actual size of readout is %d, expected size is %d.", ret,
+			static_cast<int>(sizeof(mission_s)));
+	}
+
+	return transfer_dataman_id;
+}
+
 void
 MavlinkMissionManager::switch_to_idle_state()
 {
@@ -1273,7 +1315,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
 			switch (wpca.mission_type) {
 			case MAV_MISSION_TYPE_MISSION:
 				ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
-							    DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
+							    DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, true);
 				break;
 
 			case MAV_MISSION_TYPE_FENCE:
@@ -1286,7 +1328,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
 
 			case MAV_MISSION_TYPE_ALL:
 				ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
-							    DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
+							    DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, true);
 				ret = update_geofence_count(0) || ret;
 				ret = update_safepoint_count(0) || ret;
 				break;

src/modules/mavlink/mavlink_mission.h

@@ -159,7 +159,7 @@ class MavlinkMissionManager
 
 	void init_offboard_mission();
 
-	int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq);
+	int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool clear_mission = false);
 
 	/** store the geofence count to dataman */
 	int update_geofence_count(unsigned count);
@@ -216,6 +216,15 @@ class MavlinkMissionManager
 
 	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);

src/modules/navigator/mission.cpp

@@ -482,56 +482,70 @@ void
 Mission::update_mission()
 {
 
-	bool failed = true;
+	bool failed = false;
 
 	/* Reset vehicle_roi
 	 * Missions that do not explicitly configure ROI would not override
 	 * an existing ROI setting from previous missions */
 	_navigator->reset_vroi();
 
-	const mission_s old_mission = _mission;
+	_old_mission = _mission;
+	_old_mission.current_seq = _current_mission_index;
 
 	if (_mission_sub.copy(&_mission)) {
-		/* determine current index */
-		if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) {
-			_current_mission_index = _mission.current_seq;
+
+		/* If _mission.clear_mission flag is set to true, mission is deleted from dataman.
+		 * No mission validation needed. */
+		if (_mission.clear_mission) {
+			_mission.count = 0;
+			_mission.current_seq = 0;
+			_current_mission_index = 0;
+			_mission.valid = false;
 
 		} else {
-			/* if less items available, reset to first item */
-			if (_current_mission_index >= (int)_mission.count) {
-				_current_mission_index = 0;
 
-			} else if (_current_mission_index < 0) {
-				/* if not initialized, set it to 0 */
-				_current_mission_index = 0;
+			/* determine current index */
+			if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) {
+				_current_mission_index = _mission.current_seq;
+
+			} else {
+				/* if less items available, reset to first item */
+				if (_current_mission_index >= (int)_mission.count) {
+					_current_mission_index = 0;
+
+				} else if (_current_mission_index < 0) {
+					/* if not initialized, set it to 0 */
+					_current_mission_index = 0;
+				}
+
+				/* otherwise, just leave it */
 			}
 
-			/* otherwise, just leave it */
-		}
+			check_mission_valid(true);
 
-		check_mission_valid(true);
+			failed = !_navigator->get_mission_result()->valid;
 
-		failed = !_navigator->get_mission_result()->valid;
+			if (!failed) {
+				/* reset mission failure if we have an updated valid mission */
+				_navigator->get_mission_result()->failure = false;
 
-		if (!failed) {
-			/* reset mission failure if we have an updated valid mission */
-			_navigator->get_mission_result()->failure = false;
+				/* reset sequence info as well */
+				_navigator->get_mission_result()->seq_reached = -1;
+				_navigator->get_mission_result()->seq_total = _mission.count;
 
-			/* reset sequence info as well */
-			_navigator->get_mission_result()->seq_reached = -1;
-			_navigator->get_mission_result()->seq_total = _mission.count;
+				/* reset work item if new mission has been accepted */
+				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+				_mission_changed = true;
+			}
 
-			/* reset work item if new mission has been accepted */
-			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
-			_mission_changed = true;
-		}
+			/* check if the mission waypoints changed while the vehicle is in air
+			 * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */
+			if (((_mission.count != _old_mission.count) ||
+			     (_mission.dataman_id != _old_mission.dataman_id)) &&
+			    !_navigator->get_land_detected()->landed) {
+				_mission_waypoints_changed = true;
+			}
 
-		/* check if the mission waypoints changed while the vehicle is in air
-		 * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */
-		if (((_mission.count != old_mission.count) ||
-		     (_mission.dataman_id != old_mission.dataman_id)) &&
-		    !_navigator->get_land_detected()->landed) {
-			_mission_waypoints_changed = true;
 		}
 
 	} else {
@@ -544,10 +558,15 @@ Mission::update_mission()
 			PX4_WARN("mission check failed");
 		}
 
-		// reset the mission
-		_mission.count = 0;
-		_mission.current_seq = 0;
-		_current_mission_index = 0;
+		bool restored = restore_old_mission();
+
+		if (!restored) {
+
+			// Mission can't be restored, reset the mission
+			_mission.count = 0;
+			_mission.current_seq = 0;
+			_current_mission_index = 0;
+		}
 	}
 
 	// find and store landing start marker (if available)
@@ -556,6 +575,68 @@ Mission::update_mission()
 	set_current_mission_item();
 }
 
+bool
+Mission::restore_old_mission()
+{
+	bool success = false;
+
+	_mission = _old_mission;
+
+	if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) &&
+	    _mission.valid) {
+
+		/* if armed and valid don't check old mission validity again but update navigator */
+		_navigator->get_mission_result()->valid = true;
+		_navigator->get_mission_result()->seq_total = _mission.count;
+		_navigator->increment_mission_instance_count();
+		_navigator->set_mission_result_updated();
+		_home_inited = _navigator->home_position_valid();
+		find_mission_land_start();
+
+	} else {
+		/* if not armed check validity again in case we changed feasibility parameters */
+		check_mission_valid(true);
+	}
+
+	if (_mission.valid) {
+		_mission_changed = true;
+		_navigator->get_mission_result()->failure = false;
+
+		/* For completed old mission reset sequence to the first item. */
+		if (_old_mission.current_seq >= _old_mission.count) {
+			_current_mission_index = 0;
+
+		} else {
+			_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 success;
+}
 
 void
 Mission::advance_mission()
@@ -1717,6 +1798,7 @@ Mission::check_mission_valid(bool force)
 					_param_mis_dist_wps.get(),
 					_navigator->mission_landing_required());
 
+		_mission.valid = _navigator->get_mission_result()->valid;
 		_navigator->get_mission_result()->seq_total = _mission.count;
 		_navigator->increment_mission_instance_count();
 		_navigator->set_mission_result_updated();

src/modules/navigator/mission.h

@@ -110,6 +110,11 @@ class Mission : public MissionBlock, public ModuleParams
 	 */
 	void update_mission();
 
+	/**
+	 * Restore old mission if update mission was not successful
+	 */
+	bool restore_old_mission();
+
 	/**
 	 * Move on to next mission item or switch to loiter
 	 */
@@ -247,6 +252,7 @@ class Mission : public MissionBlock, public ModuleParams
 
 	uORB::Subscription	_mission_sub{ORB_ID(mission)};		/**< mission subscription */
 	mission_s		_mission {};
+	mission_s		_old_mission {};			/**< old mission is used as a backup and for comparison with a new mission */
 
 	int32_t _current_mission_index{-1};