mission require valid landing after DO_LAND_START

2026-10-28 02:53:34 +08:00 · 2017-04-20 14:41:01 -04:00
parent 56b028148b
commit ed1b442065
14 changed files with 343 additions and 350 deletions
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -42,73 +42,79 @@
 #include "mission_feasibility_checker.h"

 #include "mission_block.h"
+#include "navigator.h"
+
+#include <drivers/drv_pwm_output.h>
+#include <fw_pos_control_l1/landingslope.h>
 #include <geo/geo.h>
-#include <math.h>
 #include <mathlib/mathlib.h>
 #include <systemlib/mavlink_log.h>
-#include <fw_pos_control_l1/landingslope.h>
-#include <systemlib/err.h>
-#include <stdio.h>
-#include <fcntl.h>
-#include <errno.h>
 #include <uORB/topics/fence.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/fw_pos_ctrl_status.h>
+#include <uORB/topics/vehicle_status.h>

-MissionFeasibilityChecker::MissionFeasibilityChecker() :
-	_mavlink_log_pub(nullptr),
-	_fw_pos_ctrl_status_sub(-1),
-	_initDone(false),
-	_dist_1wp_ok(false)
+bool
+MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission, float max_waypoint_distance,
+		bool land_start_req)
 {
-	_fw_pos_ctrl_status = {};
-}
+	dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
+	const size_t nMissionItems = mission.count;
+
+	const bool isRotarywing = (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol);
+
+	Geofence &geofence = _navigator->get_geofence();
+	fw_pos_ctrl_status_s *fw_pos_ctrl_status = _navigator->get_fw_pos_ctrl_status();
+	const float home_alt = _navigator->get_home_position()->alt;
+	const bool home_valid = _navigator->home_position_valid();


-bool MissionFeasibilityChecker::checkMissionFeasible(orb_advert_t *mavlink_log_pub, bool isRotarywing,
-		dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
-		float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued,
-		float default_acceptance_rad,
-		bool condition_landed)
-{
+
+	bool &warning_issued = _navigator->get_mission_result()->warning;
+	const float default_acceptance_rad  = _navigator->get_default_acceptance_radius();
+	const bool landed = _navigator->get_land_detected()->landed;
+
 	bool failed = false;
 	bool warned = false;
-	/* Init if not done yet */
-	init();
-
-	_mavlink_log_pub = mavlink_log_pub;

 	// first check if we have a valid position
-	if (!home_valid /* can later use global / local pos for finer granularity */) {
+	if (!home_valid) {
 		failed = true;
 		warned = true;
-		mavlink_log_info(_mavlink_log_pub, "Not yet ready for mission, no position lock.");
+		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.");

 	} else {
+		const double lat = _navigator->get_home_position()->lat;
+		const double lon = _navigator->get_home_position()->lon;
+
 		failed = failed
-			 || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
+			 || !check_dist_1wp(dm_current, nMissionItems, lat, lon, max_waypoint_distance, warning_issued);
 	}

 	// check if all mission item commands are supported
-	failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, condition_landed);
-	failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt);
+	failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, landed);
+	failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt, home_valid);
 	failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);

 	if (isRotarywing) {
 		failed = failed
-			 || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad);
+			 || !checkRotarywing(dm_current, nMissionItems, home_alt, home_valid, default_acceptance_rad);

 	} else {
-		failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
+		failed = failed
+			 || !checkFixedwing(dm_current, nMissionItems, fw_pos_ctrl_status, home_alt, home_valid,
+					    default_acceptance_rad, land_start_req);
 	}

 	return !failed;
 }

-bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems,
-		Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad)
+bool
+MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMissionItems,
+		float home_alt, bool home_valid, float default_acceptance_rad)
 {
-	/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
 	for (size_t i = 0; i < nMissionItems; i++) {
-		struct mission_item_s missionitem;
+		struct mission_item_s missionitem = {};
 		const ssize_t len = sizeof(struct mission_item_s);

 		if (dm_read(dm_current, i, &missionitem, len) != len) {
@@ -120,9 +126,8 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr
 		if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) {
 			// make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius
 			// this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air
-			float takeoff_alt = missionitem.altitude_is_relative
-					    ? missionitem.altitude
-					    : missionitem.altitude - home_alt;
+			float takeoff_alt = missionitem.altitude_is_relative ? missionitem.altitude : missionitem.altitude - home_alt;
+
 			// check if we should use default acceptance radius
 			float acceptance_radius = default_acceptance_rad;

@@ -131,7 +136,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr
 			}

 			if (takeoff_alt - 1.0f < acceptance_radius) {
-				mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff altitude too low!");
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!");
 				return false;
 			}
 		}
@@ -141,26 +146,33 @@ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_curr
 	return true;
 }

-bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems,
-		Geofence &geofence, float home_alt, bool home_valid)
+bool
+MissionFeasibilityChecker::checkFixedwing(dm_item_t dm_current, size_t nMissionItems,
+		fw_pos_ctrl_status_s *fw_pos_ctrl_status, float home_alt, bool home_valid,
+		float default_acceptance_rad, bool land_start_req)
 {
-	/* Update fixed wing navigation capabilites */
-	updateNavigationCapabilities();
-
 	/* Perform checks and issue feedback to the user for all checks */
-	bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
+	bool resTakeoff = checkFixedWingTakeoff(dm_current, nMissionItems, home_alt, home_valid, land_start_req);
+	bool resLanding = checkFixedWingLanding(dm_current, nMissionItems, fw_pos_ctrl_status, land_start_req);

 	/* Mission is only marked as feasible if all checks return true */
-	return resLanding;
+	return (resTakeoff && resLanding);
 }

-bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
-		float home_alt)
+bool
+MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt,
+		bool home_valid)
 {
+
+	if (geofence.isHomeRequired() && !home_valid) {
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position");
+		return false;
+	}
+
 	/* Check if all mission items are inside the geofence (if we have a valid geofence) */
 	if (geofence.valid()) {
 		for (size_t i = 0; i < nMissionItems; i++) {
-			struct mission_item_s missionitem;
+			struct mission_item_s missionitem = {};
 			const ssize_t len = sizeof(missionitem);

 			if (dm_read(dm_current, i, &missionitem, len) != len) {
@@ -168,15 +180,17 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 				return false;
 			}

+			if (missionitem.altitude_is_relative && !home_valid) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position");
+				return false;
+			}
+
 			// Geofence function checks against home altitude amsl
-			missionitem.altitude = missionitem.altitude_is_relative
-					       ? missionitem.altitude + home_alt
-					       : missionitem.altitude;
+			missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude;

-			if (MissionBlock::item_contains_position(&missionitem) &&
-			    !geofence.inside(missionitem)) {
+			if (MissionBlock::item_contains_position(&missionitem) && !geofence.inside(missionitem)) {

-				mavlink_log_critical(_mavlink_log_pub, "Geofence violation for waypoint %d", i + 1);
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %d", i + 1);
 				return false;
 			}
 		}
@@ -185,12 +199,13 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 	return true;
 }

-bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems,
+bool
+MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems,
 		float home_alt, bool home_valid, bool &warning_issued, bool throw_error)
 {
-	/* Check if all waypoints are above the home altitude, only return false if bool throw_error = true */
+	/* Check if all waypoints are above the home altitude */
 	for (size_t i = 0; i < nMissionItems; i++) {
-		struct mission_item_s missionitem;
+		struct mission_item_s missionitem = {};
 		const ssize_t len = sizeof(struct mission_item_s);

 		if (dm_read(dm_current, i, &missionitem, len) != len) {
@@ -200,16 +215,16 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 		}

 		/* reject relative alt without home set */
-		if (missionitem.altitude_is_relative && !home_valid && isPositionCommand(missionitem.nav_cmd)) {
+		if (missionitem.altitude_is_relative && !home_valid && MissionBlock::item_contains_position(&missionitem)) {

 			warning_issued = true;

 			if (throw_error) {
-				mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: No home pos, WP %d uses rel alt", i + 1);
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No home pos, WP %d uses rel alt", i + 1);
 				return false;

 			} else	{
-				mavlink_log_critical(_mavlink_log_pub, "Warning: No home pos, WP %d uses rel alt", i + 1);
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: No home pos, WP %d uses rel alt", i + 1);
 				return true;
 			}
 		}
@@ -217,16 +232,16 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 		/* calculate the global waypoint altitude */
 		float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;

-		if (home_alt > wp_alt && isPositionCommand(missionitem.nav_cmd)) {
+		if ((home_alt > wp_alt) && MissionBlock::item_contains_position(&missionitem)) {

 			warning_issued = true;

 			if (throw_error) {
-				mavlink_log_critical(_mavlink_log_pub, "Rejecting mission: Waypoint %d below home", i + 1);
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Waypoint %d below home", i + 1);
 				return false;

 			} else	{
-				mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home", i + 1);
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: Waypoint %d below home", i + 1);
 				return true;
 			}
 		}
@@ -235,8 +250,8 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 	return true;
 }

-bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems,
-		bool condition_landed)
+bool
+MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool landed)
 {
 	// do not allow mission if we find unsupported item
 	for (size_t i = 0; i < nMissionItems; i++) {
@@ -245,7 +260,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, 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_log_pub, "Rejecting Mission: Cannot access SD card");
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Cannot access SD card");
 			return false;
 		}

@@ -277,31 +292,93 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
 		    missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {

-			mavlink_log_critical(_mavlink_log_pub, "Rejecting mission item %i: unsupported cmd: %d", (int)(i + 1),
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d", (int)(i + 1),
 					     (int)missionitem.nav_cmd);
 			return false;
 		}

-		// check if the mission starts with a land command while the vehicle is landed
-		if (missionitem.nav_cmd == NAV_CMD_LAND &&
-		    i == 0 &&
-		    condition_landed) {
+		/* Check non navigation item */
+		if (missionitem.nav_cmd == NAV_CMD_DO_SET_SERVO) {

-			mavlink_log_critical(_mavlink_log_pub, "Rejecting mission that starts with LAND command while vehicle is landed.");
-			return false;
+			/* check actuator number */
+			if (missionitem.params[0] < 0 || missionitem.params[0] > 5) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Actuator number %d is out of bounds 0..5",
+						     (int)missionitem.params[0]);
+				return false;
+			}
+
+			/* check actuator value */
+			if (missionitem.params[1] < -PWM_DEFAULT_MAX || missionitem.params[1] > PWM_DEFAULT_MAX) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+						     "Actuator value %d is out of bounds -PWM_DEFAULT_MAX..PWM_DEFAULT_MAX", (int)missionitem.params[1]);
+				return false;
+			}
 		}

+		// check if the mission starts with a land command while the vehicle is landed
+		if (missionitem.nav_cmd == NAV_CMD_LAND && i == 0 && landed) {

+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with landing");
+			return false;
+		}
 	}

 	return true;
 }

-bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
+bool
+MissionFeasibilityChecker::checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems,
+		float home_alt, bool home_valid, float default_acceptance_rad)
+{
+	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. */
+			return false;
+		}
+
+		// look for a takeoff waypoint
+		if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) {
+			// make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius
+			// this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air
+
+			float takeoff_alt = missionitem.altitude_is_relative
+					    ? missionitem.altitude
+					    : missionitem.altitude - home_alt;
+
+			// check if we should use default acceptance radius
+			float acceptance_radius = default_acceptance_rad;
+
+			if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) {
+				acceptance_radius = missionitem.acceptance_radius;
+			}
+
+			if (takeoff_alt - 1.0f < acceptance_radius) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff altitude too low!");
+				return false;
+			}
+		}
+	}
+
+	// all checks have passed
+	return true;
+}
+
+bool
+MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems,
+		fw_pos_ctrl_status_s *fw_pos_ctrl_status, bool land_start_req)
 {
 	/* Go through all mission items and search for a landing waypoint
 	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */

+	bool landing_valid = false;
+
+	bool land_start_found = false;
+	size_t do_land_start_index = 0;
+	size_t landing_approach_index = 0;
+
 	for (size_t i = 0; i < nMissionItems; i++) {
 		struct mission_item_s missionitem;
 		const ssize_t len = sizeof(missionitem);
@@ -311,64 +388,92 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 			return false;
 		}

-		if (missionitem.nav_cmd == NAV_CMD_LAND) {
-			struct mission_item_s missionitem_previous;
+		// if DO_LAND_START found then require valid landing AFTER
+		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
+			if (land_start_found) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start");
+				return false;

-			if (i != 0) {
-				if (dm_read(dm_current, i - 1, &missionitem_previous, len) != len) {
+			} else {
+				land_start_found = true;
+				do_land_start_index = i;
+			}
+		}
+
+		if (missionitem.nav_cmd == NAV_CMD_LAND) {
+			mission_item_s missionitem_previous {};
+
+			if (i > 0) {
+				landing_approach_index = i - 1;
+
+				if (dm_read(dm_current, landing_approach_index, &missionitem_previous, len) != len) {
 					/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 					return false;
 				}

-				float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon, missionitem.lat,
-						    missionitem.lon);
-				float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
-						      _fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
-				float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude,
-							_fw_pos_ctrl_status.landing_horizontal_slope_displacement, _fw_pos_ctrl_status.landing_slope_angle_rad);
-				float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
-//				warnx("wp_distance %.2f, delta_altitude %.2f, missionitem_previous.altitude %.2f, missionitem.altitude %.2f, slope_alt_req %.2f, wp_distance_req %.2f",
-//						wp_distance, delta_altitude, missionitem_previous.altitude, missionitem.altitude, slope_alt_req, wp_distance_req);
-//				warnx("_nav_caps.landing_horizontal_slope_displacement %.4f, _nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_flare_length %.4f",
-//						_nav_caps.landing_horizontal_slope_displacement, _nav_caps.landing_slope_angle_rad, _nav_caps.landing_flare_length);
+				if (MissionBlock::item_contains_position(&missionitem_previous)) {
+					float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon, missionitem.lat,
+							    missionitem.lon);

-				if (wp_distance > _fw_pos_ctrl_status.landing_flare_length) {
-					/* Last wp is before flare region */
+					float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
+							      fw_pos_ctrl_status->landing_horizontal_slope_displacement, fw_pos_ctrl_status->landing_slope_angle_rad);

-					if (delta_altitude < 0) {
-						if (missionitem_previous.altitude <= slope_alt_req) {
-							/* Landing waypoint is at or below altitude of slope at the given waypoint distance: this is ok, aircraft will intersect the slope */
-							return true;
+					float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude,
+								fw_pos_ctrl_status->landing_horizontal_slope_displacement, fw_pos_ctrl_status->landing_slope_angle_rad);
+
+					if (wp_distance > fw_pos_ctrl_status->landing_flare_length) {
+						/* Last wp is before flare region */
+
+						const float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
+
+						if (delta_altitude < 0) {
+							if (missionitem_previous.altitude > slope_alt_req) {
+								/* Landing waypoint is above altitude of slope at the given waypoint distance */
+								mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach");
+								mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %.1fm or move further away by %.1fm",
+										     (double)(slope_alt_req - missionitem_previous.altitude),
+										     (double)(wp_distance_req - wp_distance));
+
+								return false;
+							}

 						} else {
-							/* Landing waypoint is above altitude of slope at the given waypoint distance */
-							mavlink_log_critical(_mavlink_log_pub, "Landing: last waypoint too high/too close");
-							mavlink_log_critical(_mavlink_log_pub, "Move down to %.1fm or move further away by %.1fm",
-									     (double)(slope_alt_req),
-									     (double)(wp_distance_req - wp_distance));
+							/* Landing waypoint is above last waypoint */
+							mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint");
 							return false;
 						}

 					} else {
-						/* Landing waypoint is above last waypoint */
-						mavlink_log_critical(_mavlink_log_pub, "Landing waypoint above last nav waypoint");
+						/* Last wp is in flare region */
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare");
 						return false;
 					}

+					landing_valid = true;
+
 				} else {
-					/* Last wp is in flare region */
-					//xxx give recommendations
-					mavlink_log_critical(_mavlink_log_pub, "Last waypoint too close to landing waypoint");
+					// mission item before land doesn't have a position
+					mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: need landing approach");
 					return false;
 				}

 			} else {
-				mavlink_log_critical(_mavlink_log_pub, "Invalid mission: starts with land waypoint");
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint");
 				return false;
 			}
 		}
 	}

+	if (land_start_req && !land_start_found) {
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: land start required");
+		return false;
+	}
+
+	if (land_start_found && (!landing_valid || (do_land_start_index > landing_approach_index))) {
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start");
+		return false;
+	}
+
 	/* No landing waypoints or no waypoints */
 	return true;
 }
@@ -377,46 +482,23 @@ bool
 MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon,
 		float dist_first_wp, bool &warning_issued)
 {
-
 	/* check if first waypoint is not too far from home */
 	if (dist_first_wp > 0.0f) {
-		struct mission_item_s mission_item;
+		struct mission_item_s mission_item = {};

 		/* find first waypoint (with lat/lon) item in datamanager */
-		for (unsigned i = 0; i < nMissionItems; i++) {
-			if (dm_read(dm_current, i,
-				    &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
-				/* Check non navigation item */
-				if (mission_item.nav_cmd == NAV_CMD_DO_SET_SERVO) {
-
-					/* check actuator number */
-					if (mission_item.params[0] < 0 || mission_item.params[0] > 5) {
-						mavlink_log_critical(_mavlink_log_pub, "Actuator number %d is out of bounds 0..5", (int)mission_item.params[0]);
-						warning_issued = true;
-						return false;
-					}
-
-					/* check actuator value */
-					if (mission_item.params[1] < -2000 || mission_item.params[1] > 2000) {
-						mavlink_log_critical(_mavlink_log_pub, "Actuator value %d is out of bounds -2000..2000", (int)mission_item.params[1]);
-						warning_issued = true;
-						return false;
-					}
-				}
-
-				/* check only items with valid lat/lon */
-				else if (isPositionCommand(mission_item.nav_cmd)) {
+		for (size_t i = 0; i < nMissionItems; i++) {
+			if (dm_read(dm_current, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
+				if (MissionBlock::item_contains_position(&mission_item)) {
+					/* check only items with valid lat/lon */

 					/* check distance from current position to item */
-					float dist_to_1wp = get_distance_to_next_waypoint(
-								    mission_item.lat, mission_item.lon, curr_lat, curr_lon);
+					float dist_to_1wp = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, curr_lat, curr_lon);

 					if (dist_to_1wp < dist_first_wp) {
-						_dist_1wp_ok = true;
-
 						if (dist_to_1wp > ((dist_first_wp * 3) / 2)) {
 							/* allow at 2/3 distance, but warn */
-							mavlink_log_critical(_mavlink_log_pub, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
+							mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
 							warning_issued = true;
 						}

@@ -424,7 +506,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI

 					} else {
 						/* item is too far from home */
-						mavlink_log_critical(_mavlink_log_pub, "First waypoint too far: %d m > %d, refusing mission",
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "First waypoint too far: %d m > %d, refusing mission",
 								     (int)dist_to_1wp, (int)dist_first_wp);
 						warning_issued = true;
 						return false;
@@ -433,51 +515,15 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI

 			} else {
 				/* error reading, mission is invalid */
-				mavlink_log_info(_mavlink_log_pub, "error reading offboard mission");
+				mavlink_log_info(_navigator->get_mavlink_log_pub(), "error reading offboard mission");
 				return false;
 			}
 		}

 		/* no waypoints found in mission, then we will not fly far away */
-		_dist_1wp_ok = true;
 		return true;

 	} else {
 		return true;
 	}
 }
-
-bool
-MissionFeasibilityChecker::isPositionCommand(unsigned cmd)
-{
-	if (cmd == NAV_CMD_WAYPOINT ||
-	    cmd == NAV_CMD_LOITER_UNLIMITED ||
-	    cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-	    cmd == NAV_CMD_LAND ||
-	    cmd == NAV_CMD_TAKEOFF ||
-	    cmd == NAV_CMD_LOITER_TO_ALT ||
-	    cmd == NAV_CMD_VTOL_TAKEOFF ||
-	    cmd == NAV_CMD_VTOL_LAND) {
-
-		return true;
-
-	} else {
-		return false;
-
-	}
-}
-
-void MissionFeasibilityChecker::updateNavigationCapabilities()
-{
-	(void)orb_copy(ORB_ID(fw_pos_ctrl_status), _fw_pos_ctrl_status_sub, &_fw_pos_ctrl_status);
-}
-
-void MissionFeasibilityChecker::init()
-{
-	if (!_initDone) {
-
-		_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
-
-		_initDone = true;
-	}
-}