do not update mission if it has unsupported mission items

src/modules/navigator/mission.cpp

@@ -207,6 +207,8 @@ Mission::update_onboard_mission()
 void
 Mission::update_offboard_mission()
 {
+	bool failed = true;
+
 	if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
 		warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq);
 		/* determine current index */
@@ -228,12 +230,15 @@ Mission::update_offboard_mission()
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
 		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
+		failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
 				_navigator->get_home_position()->alt);
 
 	} else {
 		warnx("offboard mission update failed");
+	}
+
+	if (failed) {
 		_offboard_mission.count = 0;
 		_offboard_mission.current_seq = 0;
 		_current_offboard_mission_index = 0;

src/modules/navigator/mission_feasibility_checker.cpp

@@ -65,6 +65,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
 
 bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
 {
+	bool failed = false;
 	/* Init if not done yet */
 	init();
 
@@ -74,11 +75,16 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
 		_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 	}
 
+	// check if all mission item commands are supported
+	failed |= !checkMissionItemValidity(dm_current, nMissionItems);
+
 
 	if (isRotarywing)
-		return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
+		failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
 	else
-		return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
+		failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
+
+	return !failed;
 }
 
 bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
@@ -163,6 +169,38 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 	return true;
 }
 
+bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems) {
+	// do not allow mission if we find unsupported item
+	for (size_t i = 0; i < nMissionItems; i++) {
+		struct mission_item_s missionitem;
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		if (dm_read(dm_current, i, &missionitem, len) != len) {
+			// not supposed to happen unless the datamanager can't access the SD card, etc.
+			mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access mission from SD card");
+			return false;
+		}
+
+		// check if we find unsupported item and reject mission if so
+		if (missionitem.nav_cmd != NAV_CMD_IDLE &&
+			missionitem.nav_cmd != NAV_CMD_WAYPOINT &&
+			missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
+			missionitem.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
+			missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
+			missionitem.nav_cmd != NAV_CMD_LAND &&
+			missionitem.nav_cmd != NAV_CMD_TAKEOFF &&
+			missionitem.nav_cmd != NAV_CMD_ROI &&
+			missionitem.nav_cmd != NAV_CMD_PATHPLANNING &&
+			missionitem.nav_cmd != NAV_CMD_DO_JUMP) {
+
+			mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1));
+			return false;
+		}
+	}
+	mavlink_log_critical(_mavlink_fd, "Mission is valid!");
+	return true;
+}
+
 bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
 {
 	/* Go through all mission items and search for a landing waypoint

src/modules/navigator/mission_feasibility_checker.h

@@ -62,6 +62,7 @@ private:
 	/* Checks for all airframes */
 	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
 	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false);
+	bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
 
 	/* Checks specific to fixedwing airframes */
 	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);