Revert "started with feasibility checks"

This reverts commit ceb8f6e1d5.

src/lib/matrix/matrix/Matrix.hpp

@@ -583,7 +583,7 @@ public:
 
 	bool isAllNan() const
 	{
-		const Matrix<Type, M, N> &self = *this;
+		const Matrix<float, M, N> &self = *this;
 		bool result = true;
 
 		for (size_t i = 0; i < M; i++) {
@@ -597,7 +597,7 @@ public:
 
 	bool isAllFinite() const
 	{
-		const Matrix<Type, M, N> &self = *this;
+		const Matrix<float, M, N> &self = *this;
 
 		for (size_t i = 0; i < M; i++) {
 			for (size_t j = 0; j < N; j++) {

src/modules/navigator/CMakeLists.txt

@@ -32,7 +32,6 @@
 ############################################################################
 
 add_subdirectory(GeofenceBreachAvoidance)
-add_subdirectory(MissionFeasibility)
 
 px4_add_module(
 	MODULE modules__navigator
@@ -54,5 +53,4 @@ px4_add_module(
 		geo
 		geofence_breach_avoidance
 		motion_planning
-		mission_feasibility_checker
 	)

src/modules/navigator/MissionFeasibility/CMakeLists.txt

@@ -1,7 +0,0 @@
-px4_add_library(mission_feasibility_checker
-	FeasibilityChecker.cpp
-)
-
-target_link_libraries(mission_feasibility_checker PUBLIC modules__navigator modules__dataman)
-
-px4_add_functional_gtest(SRC FeasibilityCheckerTest.cpp LINKLIBS mission_feasibility_checker)

src/modules/navigator/MissionFeasibility/FeasibilityChecker.cpp

@@ -1,741 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022 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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include "FeasibilityChecker.hpp"
-#include <systemlib/mavlink_log.h>
-#include <px4_platform_common/events.h>
-#include <drivers/drv_pwm_output.h>
-#include "../mission_block.h"
-#include <lib/mathlib/mathlib.h>
-#include <lib/geo/geo.h>
-
-FeasibilityChecker::FeasibilityChecker() :
-	ModuleParams(nullptr)
-{
-}
-
-void FeasibilityChecker::reset()
-{
-
-	_is_landed = false;
-	_home_alt_msl = NAN;
-	_home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
-	_vehicle_type = VehicleType::RotaryWing;
-
-	_mission_validity_failed = false;
-	_takeoff_failed = false;
-	_land_pattern_validity_failed = false;
-	_distance_first_waypoint_failed = false;
-	_distance_between_waypoints_failed = false;
-	_below_home_alt_failed = false;
-	_fixed_wing_land_approach_failed = false;
-	_takeoff_land_available_failed = false;
-
-	_found_item_with_position = false;
-	_has_vtol_takeoff = false;
-	_has_takeoff = false;
-
-	_landing_valid = false;
-	_do_land_start_index = -1;
-	_landing_approach_index = -1;
-	_mission_item_previous = {};
-
-	_first_waypoint_found = false;
-
-	_last_lat = (double)NAN;
-	_last_lon = (double)NAN;
-	_last_cmd = -1;
-}
-
-void FeasibilityChecker::updateData()
-{
-	home_position_s home = {};
-
-	if (_home_pos_sub.updated()) {
-		_home_pos_sub.update(&home);
-
-		if (home.valid_hpos) {
-			_home_lat_lon = matrix::Vector2d(home.lat, home.lon);
-		}
-
-		if (home.valid_alt) {
-			_home_alt_msl = home.alt;
-		}
-	}
-
-	vehicle_status_s status = {};
-
-	if (_status_sub.updated()) {
-		_status_sub.copy(&status);
-
-		if (status.is_vtol) {
-			_vehicle_type = VehicleType::Vtol;
-
-		} else if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-			_vehicle_type = VehicleType::RotaryWing;
-
-		} else if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-			_vehicle_type = VehicleType::FixedWing;
-
-		} else {
-			_vehicle_type = VehicleType::Other;
-		}
-	}
-
-	vehicle_land_detected_s land_detected = {};
-
-	if (_land_detector_sub.updated()) {
-		_land_detector_sub.copy(&land_detected);
-		_is_landed = land_detected.landed;
-	}
-
-	param_t handle = param_find("FW_LND_ANG");
-
-	if (handle != PARAM_INVALID) {
-		param_get(handle, &_param_fw_lnd_ang);
-	}
-
-	handle = param_find("MIS_DIST_1WP");
-
-	if (handle != PARAM_INVALID) {
-		param_get(handle, &_param_mis_dist_1wp);
-	}
-
-	handle = param_find("MIS_DIST_WPS");
-
-	if (handle != PARAM_INVALID) {
-		param_get(handle, &_param_mis_dist_wps);
-	}
-
-	handle = param_find("NAV_ACC_RAD");
-
-	if (handle != PARAM_INVALID) {
-		param_get(handle, &_param_nav_acc_rad);
-	}
-
-	handle = param_find("MIS_TKO_LAND_REQ");
-
-	if (handle != PARAM_INVALID) {
-		param_get(handle, &_param_mis_takeoff_land_req);
-	}
-}
-
-bool FeasibilityChecker::processNextItem(mission_item_s &mission_item, const int current_index, const int total_count)
-{
-	if (current_index == 0) {
-		reset();
-		updateData();
-	}
-
-	if (!_mission_validity_failed) {
-		_mission_validity_failed = !checkMissionItemValidity(mission_item, current_index);
-	}
-
-	if (_mission_validity_failed) {
-		// if a mission item is not valid then abort the other checks
-		return false;
-	}
-
-	doCommonChecks(mission_item, current_index);
-
-	if (_vehicle_type == VehicleType::Vtol) {
-		doVtolChecks(mission_item, current_index, total_count - 1);
-
-	} else if (_vehicle_type == VehicleType::FixedWing) {
-		doFixedWingChecks(mission_item, current_index, total_count - 1);
-
-	} else if (_vehicle_type == VehicleType::RotaryWing) {
-		doMulticopterChecks(mission_item, current_index);
-	}
-
-	if (current_index == total_count - 1) {
-		_takeoff_land_available_failed = !checkTakeoffLandAvailable();
-	}
-
-	_mission_item_previous = mission_item;
-
-	return true;
-}
-
-void FeasibilityChecker::doCommonChecks(mission_item_s &mission_item, const int current_index)
-{
-
-	if (!_distance_between_waypoints_failed) {
-		_distance_between_waypoints_failed = !checkDistancesBetweenWaypoints(mission_item);
-	}
-
-	if (!_distance_first_waypoint_failed) {
-		_distance_first_waypoint_failed = !checkDistanceToFirstWaypoint(mission_item);
-	}
-
-	if (!_below_home_alt_failed) {
-		_below_home_alt_failed = !checkIfBelowHomeAltitude(mission_item, current_index);
-	}
-
-	if (!_takeoff_failed) {
-		_takeoff_failed = !checkTakeoff(mission_item);
-	}
-}
-
-void FeasibilityChecker::doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index)
-{
-	if (!_land_pattern_validity_failed) {
-		_land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index);
-	}
-
-}
-
-void FeasibilityChecker::doFixedWingChecks(mission_item_s &mission_item, const int current_index, const int last_index)
-{
-	if (!_land_pattern_validity_failed) {
-		_land_pattern_validity_failed = !checkLandPatternValidity(mission_item, current_index, last_index);
-	}
-
-	if (!_fixed_wing_land_approach_failed) {
-		_fixed_wing_land_approach_failed = !checkFixedWindLandApproach(mission_item, current_index);
-	}
-
-}
-
-void FeasibilityChecker::doMulticopterChecks(mission_item_s &mission_item, const int current_index)
-{
-	// this flag is used for the checkTakeoffLandAvailable check at the very end
-	_landing_valid |= mission_item.nav_cmd == NAV_CMD_LAND;
-}
-
-bool FeasibilityChecker::checkMissionItemValidity(mission_item_s &mission_item, const int current_index)
-{
-	/* reject relative alt without home set */
-	if (mission_item.altitude_is_relative && !PX4_ISFINITE(_home_alt_msl)
-	    && MissionBlock::item_contains_position(mission_item)) {
-
-
-
-		mavlink_log_critical(_mavlink_log_pub, "Mission rejected: No home pos, WP %d uses rel alt\t", current_index + 1);
-		events::send<int16_t>(events::ID("navigator_mis_no_home_rel_alt"), {events::Log::Error, events::LogInternal::Info},
-				      "Mission rejected: No home position, waypoint {1} uses relative altitude",
-				      current_index + 1);
-		return false;
-
-	}
-
-	// check if we find unsupported items and reject mission if so
-	if (mission_item.nav_cmd != NAV_CMD_IDLE &&
-	    mission_item.nav_cmd != NAV_CMD_WAYPOINT &&
-	    mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED &&
-	    mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
-	    mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
-	    mission_item.nav_cmd != NAV_CMD_LAND &&
-	    mission_item.nav_cmd != NAV_CMD_TAKEOFF &&
-	    mission_item.nav_cmd != NAV_CMD_LOITER_TO_ALT &&
-	    mission_item.nav_cmd != NAV_CMD_VTOL_TAKEOFF &&
-	    mission_item.nav_cmd != NAV_CMD_VTOL_LAND &&
-	    mission_item.nav_cmd != NAV_CMD_DELAY &&
-	    mission_item.nav_cmd != NAV_CMD_CONDITION_GATE &&
-	    mission_item.nav_cmd != NAV_CMD_DO_WINCH &&
-	    mission_item.nav_cmd != NAV_CMD_DO_GRIPPER &&
-	    mission_item.nav_cmd != NAV_CMD_DO_JUMP &&
-	    mission_item.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
-	    mission_item.nav_cmd != NAV_CMD_DO_SET_HOME &&
-	    mission_item.nav_cmd != NAV_CMD_DO_SET_SERVO &&
-	    mission_item.nav_cmd != NAV_CMD_DO_LAND_START &&
-	    mission_item.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL &&
-	    mission_item.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
-	    mission_item.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE &&
-	    mission_item.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
-	    mission_item.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE &&
-	    mission_item.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE &&
-	    mission_item.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO &&
-	    mission_item.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
-	    mission_item.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
-	    mission_item.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW &&
-	    mission_item.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE &&
-	    mission_item.nav_cmd != NAV_CMD_DO_SET_ROI &&
-	    mission_item.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION &&
-	    mission_item.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET &&
-	    mission_item.nav_cmd != NAV_CMD_DO_SET_ROI_NONE &&
-	    mission_item.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
-	    mission_item.nav_cmd != NAV_CMD_OBLIQUE_SURVEY &&
-	    mission_item.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
-	    mission_item.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
-	    mission_item.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
-	    mission_item.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
-	    mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
-
-		mavlink_log_critical(_mavlink_log_pub, "Mission rejected: item %i: unsupported cmd: %d\t",
-				     (int)(current_index + 1),
-				     (int)mission_item.nav_cmd);
-		events::send<uint16_t, uint16_t>(events::ID("navigator_mis_unsup_cmd"), {events::Log::Error, events::LogInternal::Warning},
-						 "Mission rejected: item {1}: unsupported command: {2}", current_index + 1, mission_item.nav_cmd);
-		return false;
-	}
-
-	/* 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\t",
-					     (int)mission_item.params[0]);
-			events::send<uint32_t>(events::ID("navigator_mis_act_index"), {events::Log::Error, events::LogInternal::Warning},
-					       "Actuator number {1} is out of bounds 0..5", (int)mission_item.params[0]);
-			return false;
-		}
-
-		/* check actuator value */
-		if (mission_item.params[1] < -PWM_DEFAULT_MAX || mission_item.params[1] > PWM_DEFAULT_MAX) {
-			mavlink_log_critical(_mavlink_log_pub,
-					     "Actuator value %d is out of bounds -PWM_DEFAULT_MAX..PWM_DEFAULT_MAX\t", (int)mission_item.params[1]);
-			events::send<uint32_t, uint32_t>(events::ID("navigator_mis_act_range"), {events::Log::Error, events::LogInternal::Warning},
-							 "Actuator value {1} is out of bounds -{2}..{2}", (int)mission_item.params[1], PWM_DEFAULT_MAX);
-			return false;
-		}
-	}
-
-	// check if the mission starts with a land command while the vehicle is landed
-	if ((current_index == 0) && mission_item.nav_cmd == NAV_CMD_LAND && _is_landed) {
-
-		mavlink_log_critical(_mavlink_log_pub, "Mission rejected: starts with landing\t");
-		events::send(events::ID("navigator_mis_starts_w_landing"), {events::Log::Error, events::LogInternal::Info},
-			     "Mission rejected: starts with landing");
-		return false;
-	}
-
-	return true;
-}
-
-bool FeasibilityChecker::checkTakeoff(mission_item_s &mission_item)
-{
-
-	// look for a takeoff waypoint
-	if (mission_item.nav_cmd == NAV_CMD_TAKEOFF || mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
-		// make sure that the altitude of the waypoint is above the home altitude
-		const float takeoff_alt = mission_item.altitude_is_relative
-					  ? mission_item.altitude
-					  : mission_item.altitude - _home_alt_msl;
-
-		if (takeoff_alt < FLT_EPSILON) {
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff altitude below home altitude!\t");
-			events::send<float>(events::ID("navigator_mis_takeoff_too_low"), {events::Log::Error, events::LogInternal::Info},
-					    "Mission rejected: takeoff altitude too low! Minimum: {1:.1m_v}",
-					    mission_item.altitude_is_relative ? 0.0f : _home_alt_msl);
-			return false;
-		}
-
-		if (!_has_vtol_takeoff) {
-			_has_vtol_takeoff = mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF;
-		}
-
-		if (!_has_takeoff) {
-			_has_takeoff = true;
-		}
-
-
-		if (_found_item_with_position) {
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: takeoff not first waypoint item\t");
-			events::send(events::ID("navigator_mis_takeoff_not_first"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: takeoff is not the first waypoint item");
-			return false;
-		}
-	}
-
-	if (!_found_item_with_position) {
-		_found_item_with_position = (mission_item.nav_cmd != NAV_CMD_IDLE &&
-					     mission_item.nav_cmd != NAV_CMD_DELAY &&
-					     mission_item.nav_cmd != NAV_CMD_DO_JUMP &&
-					     mission_item.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
-					     mission_item.nav_cmd != NAV_CMD_DO_SET_HOME &&
-					     mission_item.nav_cmd != NAV_CMD_DO_SET_SERVO &&
-					     mission_item.nav_cmd != NAV_CMD_DO_LAND_START &&
-					     mission_item.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL &&
-					     mission_item.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
-					     mission_item.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE &&
-					     mission_item.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
-					     mission_item.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE &&
-					     mission_item.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE &&
-					     mission_item.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO &&
-					     mission_item.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
-					     mission_item.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
-					     mission_item.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW &&
-					     mission_item.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE &&
-					     mission_item.nav_cmd != NAV_CMD_DO_SET_ROI &&
-					     mission_item.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION &&
-					     mission_item.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET &&
-					     mission_item.nav_cmd != NAV_CMD_DO_SET_ROI_NONE &&
-					     mission_item.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
-					     mission_item.nav_cmd != NAV_CMD_OBLIQUE_SURVEY &&
-					     mission_item.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
-					     mission_item.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
-					     mission_item.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
-					     mission_item.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
-					     mission_item.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
-					     mission_item.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION);
-	}
-
-	return true;
-}
-
-bool FeasibilityChecker::checkFixedWindLandApproach(mission_item_s &mission_item, const int current_index)
-{
-	if (mission_item.nav_cmd == NAV_CMD_LAND && current_index > 0) {
-
-		if (MissionBlock::item_contains_position(_mission_item_previous)) {
-
-			const float land_alt_amsl = mission_item.altitude_is_relative ? mission_item.altitude +
-						    _home_alt_msl : mission_item.altitude;
-			const float entrance_alt_amsl = _mission_item_previous.altitude_is_relative ? _mission_item_previous.altitude +
-							_home_alt_msl : _mission_item_previous.altitude;
-			const float relative_approach_altitude = entrance_alt_amsl - land_alt_amsl;
-
-			if (relative_approach_altitude < FLT_EPSILON) {
-				mavlink_log_critical(_mavlink_log_pub,
-						     "Mission rejected: the approach waypoint must be above the landing point.\t");
-				events::send(events::ID("navigator_mis_approach_wp_below_land"), {events::Log::Error, events::LogInternal::Info},
-					     "Mission rejected: the approach waypoint must be above the landing point");
-				return false;
-			}
-
-			float landing_approach_distance;
-
-			if (_mission_item_previous.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
-				// assume this is a fixed-wing landing pattern with orbit to alt followed
-				// by tangent exit to landing approach and touchdown at landing waypoint
-
-				const float distance_orbit_center_to_land = get_distance_to_next_waypoint(_mission_item_previous.lat,
-						_mission_item_previous.lon, mission_item.lat, mission_item.lon);
-				const float orbit_radius = fabsf(_mission_item_previous.loiter_radius);
-
-				if (distance_orbit_center_to_land <= orbit_radius) {
-					mavlink_log_critical(_mavlink_log_pub,
-							     "Mission rejected: the landing point must be outside the orbit radius.\t");
-					events::send(events::ID("navigator_mis_land_wp_inside_orbit_radius"), {events::Log::Error, events::LogInternal::Info},
-						     "Mission rejected: the landing point must be outside the orbit radius");
-					return false;
-				}
-
-				landing_approach_distance = sqrtf(distance_orbit_center_to_land * distance_orbit_center_to_land - orbit_radius *
-								  orbit_radius);
-
-			} else if (_mission_item_previous.nav_cmd == NAV_CMD_WAYPOINT) {
-				// approaching directly from waypoint position
-
-				const float waypoint_distance = get_distance_to_next_waypoint(_mission_item_previous.lat, _mission_item_previous.lon,
-								mission_item.lat, mission_item.lon);
-				landing_approach_distance = waypoint_distance;
-
-			} else {
-				mavlink_log_critical(_mavlink_log_pub,
-						     "Mission rejected: unsupported landing approach entrance waypoint type. Only ORBIT_TO_ALT or WAYPOINT allowed.\t");
-				events::send(events::ID("navigator_mis_unsupported_landing_approach_wp"), {events::Log::Error, events::LogInternal::Info},
-					     "Mission rejected: unsupported landing approach entrance waypoint type. Only ORBIT_TO_ALT or WAYPOINT allowed");
-				return false;
-			}
-
-			const float glide_slope = relative_approach_altitude / landing_approach_distance;
-
-			// respect user setting as max glide slope, but account for floating point
-			// rounding on next check with small (arbitrary) 0.1 deg buffer, as the
-			// landing angle parameter is what is typically used for steepest glide
-			// in landing config
-			const float max_glide_slope = tanf(math::radians(_param_fw_lnd_ang + 0.1f));
-
-			if (glide_slope > max_glide_slope) {
-
-				const uint8_t land_angle_left_of_decimal = (uint8_t)_param_fw_lnd_ang;
-				const uint8_t land_angle_first_after_decimal = (uint8_t)((_param_fw_lnd_ang - floorf(
-							_param_fw_lnd_ang)) * 10.0f);
-
-				mavlink_log_critical(_mavlink_log_pub,
-						     "Mission rejected: the landing glide slope is steeper than the vehicle setting of %d.%d degrees.\t",
-						     (int)land_angle_left_of_decimal, (int)land_angle_first_after_decimal);
-				events::send<uint8_t, uint8_t>(events::ID("navigator_mis_glide_slope_too_steep"), {events::Log::Error, events::LogInternal::Info},
-							       "Mission rejected: the landing glide slope is steeper than the vehicle setting of {1}.{2} degrees",
-							       land_angle_left_of_decimal, land_angle_first_after_decimal);
-
-				const uint32_t acceptable_entrance_alt = (uint32_t)(max_glide_slope * landing_approach_distance);
-				const uint32_t acceptable_landing_dist = (uint32_t)ceilf(relative_approach_altitude / max_glide_slope);
-
-				mavlink_log_critical(_mavlink_log_pub,
-						     "Reduce the glide slope, lower the entrance altitude %d meters, or increase the landing approach distance %d meters.\t",
-						     (int)acceptable_entrance_alt, (int)acceptable_landing_dist);
-				events::send<uint32_t, uint32_t>(events::ID("navigator_mis_correct_glide_slope"), {events::Log::Error, events::LogInternal::Info},
-								 "Reduce the glide slope, lower the entrance altitude {1} meters, or increase the landing approach distance {2} meters",
-								 acceptable_entrance_alt, acceptable_landing_dist);
-
-				return false;
-			}
-
-			_landing_valid = true;
-
-		}
-	}
-
-	return true;
-}
-
-bool FeasibilityChecker::checkLandPatternValidity(mission_item_s &mission_item, const int current_index,
-		const int last_index)
-{
-
-	// if DO_LAND_START found then require valid landing AFTER
-	if (mission_item.nav_cmd == NAV_CMD_DO_LAND_START) {
-		if (_do_land_start_index >= 0) {
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: more than one land start.\t");
-			events::send(events::ID("navigator_mis_multiple_land"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: more than one land start commands");
-			return false;
-
-		}
-
-		_do_land_start_index = current_index;
-	}
-
-	const bool land_start_found = _do_land_start_index >= 0;
-
-	if (mission_item.nav_cmd == NAV_CMD_LAND || mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
-
-		if (current_index > 0) {
-			_landing_approach_index = current_index - 1;
-
-		} else {
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: starts with land waypoint.\t");
-			events::send(events::ID("navigator_mis_starts_w_landing2"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: starts with landing");
-			return false;
-		}
-
-	} else if (mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-		if (land_start_found && _do_land_start_index < current_index) {
-			mavlink_log_critical(_mavlink_log_pub,
-					     "Mission rejected: land start item before RTL item not possible.\t");
-			events::send(events::ID("navigator_mis_land_before_rtl"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: land start item before RTL item is not possible");
-			return false;
-		}
-	}
-
-	if (current_index == last_index && land_start_found && (_do_land_start_index > _landing_approach_index)) {
-		mavlink_log_critical(_mavlink_log_pub, "Mission rejected: invalid land start.\t");
-		events::send(events::ID("navigator_mis_invalid_land"), {events::Log::Error, events::LogInternal::Info},
-			     "Mission rejected: invalid land start");
-		return false;
-	}
-
-	_landing_valid = _landing_approach_index >= 0;
-
-	/* No landing waypoints or no waypoints */
-	return true;
-}
-
-bool FeasibilityChecker::checkTakeoffLandAvailable()
-{
-	bool result = true;
-
-	switch (_param_mis_takeoff_land_req) {
-	case 0:
-		result = true;
-		break;
-
-	case 1:
-		result = _has_takeoff;
-
-		if (!result) {
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff waypoint required.\t");
-			events::send(events::ID("navigator_mis_takeoff_missing"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: Takeoff waypoint required");
-			return false;
-		}
-
-		break;
-
-	case 2:
-		result = _landing_valid;
-
-		if (!result) {
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Landing waypoint/pattern required.\t");
-			events::send(events::ID("navigator_mis_land_missing"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: Landing waypoint/pattern required");
-		}
-
-		break;
-
-	case 3:
-		result = _has_takeoff && _landing_valid;
-
-		if (!result) {
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Takeoff or Landing item missing.\t");
-			events::send(events::ID("navigator_mis_takeoff_or_land_missing"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: Takeoff or Landing item missing");
-		}
-
-		break;
-
-	case 4:
-		result = _has_takeoff == _landing_valid;
-
-		if (!result && (_has_takeoff)) {
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Landing item or remove Takeoff.\t");
-			events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: Add Landing item or remove Takeoff");
-
-		} else if (!result && (_landing_valid)) {
-			mavlink_log_critical(_mavlink_log_pub, "Mission rejected: Add Takeoff item or remove Landing.\t");
-			events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: Add Takeoff item or remove Landing");
-		}
-
-		break;
-
-	default:
-		result = true;
-		break;
-	}
-
-	return result;
-}
-
-
-bool FeasibilityChecker::checkDistanceToFirstWaypoint(mission_item_s &mission_item)
-{
-	if (_param_mis_dist_1wp <= 0.0f || !_home_lat_lon.isAllFinite()) {
-		/* param not set, check is ok */
-		return true;
-	}
-
-	if (!_first_waypoint_found && MissionBlock::item_contains_position(mission_item)) {
-		_first_waypoint_found = true;
-
-
-		/* check distance from current position to item */
-		float dist_to_1wp = get_distance_to_next_waypoint(
-					    mission_item.lat, mission_item.lon,
-					    _home_lat_lon(0), _home_lat_lon(1));
-
-		if (dist_to_1wp < _param_mis_dist_1wp) {
-
-			return true;
-
-		} else {
-			/* item is too far from home */
-			mavlink_log_critical(_mavlink_log_pub,
-					     "First waypoint too far away: %dm, %d max\t",
-					     (int)dist_to_1wp, (int)_param_mis_dist_1wp);
-			events::send<uint32_t, uint32_t>(events::ID("navigator_mis_first_wp_too_far"), {events::Log::Error, events::LogInternal::Info},
-							 "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp, (uint32_t)_param_mis_dist_1wp);
-
-			return false;
-		}
-	}
-
-	/* no waypoints found in mission, then we will not fly far away */
-	return true;
-}
-
-bool FeasibilityChecker::checkDistancesBetweenWaypoints(const mission_item_s &mission_item)
-{
-	if (_param_mis_dist_wps <= 0.0f) {
-		/* param not set, check is ok */
-		return true;
-	}
-
-
-	/* check only items with valid lat/lon */
-	if (!MissionBlock::item_contains_position(mission_item)) {
-		return true;
-	}
-
-	/* Compare it to last waypoint if already available. */
-	if (PX4_ISFINITE(_last_lat) && PX4_ISFINITE(_last_lon)) {
-		/* check distance from current position to item */
-		const float dist_between_waypoints = get_distance_to_next_waypoint(
-				mission_item.lat, mission_item.lon,
-				_last_lat, _last_lon);
-
-
-		if (dist_between_waypoints > _param_mis_dist_wps) {
-			/* distance between waypoints is too high */
-			mavlink_log_critical(_mavlink_log_pub,
-					     "Distance between waypoints too far: %d meters, %d max.\t",
-					     (int)dist_between_waypoints, (int)_param_mis_dist_wps);
-			events::send<uint32_t, uint32_t>(events::ID("navigator_mis_wp_dist_too_far"), {events::Log::Error, events::LogInternal::Info},
-							 "Distance between waypoints too far: {1m}, (maximum: {2m})", (uint32_t)dist_between_waypoints,
-							 (uint32_t)_param_mis_dist_wps);
-
-
-			return false;
-
-			/* do not allow waypoints that are literally on top of each other */
-
-			/* and do not allow condition gates that are at the same position as a navigation waypoint */
-
-		} else if (dist_between_waypoints < 0.05f &&
-			   (mission_item.nav_cmd == NAV_CMD_CONDITION_GATE || _last_cmd == NAV_CMD_CONDITION_GATE)) {
-
-			/* Waypoints and gate are at the exact same position, which indicates an
-			 * invalid mission and makes calculating the direction from one waypoint
-			 * to another impossible. */
-			mavlink_log_critical(_mavlink_log_pub,
-					     "Distance between waypoint and gate too close: %d meters\t",
-					     (int)dist_between_waypoints);
-			events::send<float, float>(events::ID("navigator_mis_wp_gate_too_close"), {events::Log::Error, events::LogInternal::Info},
-						   "Distance between waypoint and gate too close: {1:.3m} (minimum: {2:.3m})", dist_between_waypoints, 0.05f);
-
-
-			return false;
-		}
-	}
-
-	_last_lat = mission_item.lat;
-	_last_lon = mission_item.lon;
-	_last_cmd = mission_item.nav_cmd;
-
-	/* We ran through all waypoints and have not found any distances between waypoints that are too far. */
-	return true;
-}
-
-bool FeasibilityChecker::checkIfBelowHomeAltitude(const mission_item_s &mission_item, const int current_index)
-{
-	/* calculate the global waypoint altitude */
-	float wp_alt = (mission_item.altitude_is_relative) ? mission_item.altitude + _home_alt_msl : mission_item.altitude;
-
-	if (PX4_ISFINITE(_home_alt_msl) && _home_alt_msl > wp_alt && MissionBlock::item_contains_position(mission_item)) {
-
-
-
-		mavlink_log_critical(_mavlink_log_pub, "Warning: Waypoint %d below home\t", current_index + 1);
-		events::send<int16_t>(events::ID("navigator_mis_wp_below_home"), {events::Log::Warning, events::LogInternal::Info},
-				      "Waypoint {1} below home", current_index + 1);
-	}
-
-	return true;
-}

src/modules/navigator/MissionFeasibility/FeasibilityChecker.hpp

@@ -1,250 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2022 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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include "../navigation.h"
-#include <mathlib/mathlib.h>
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_land_detected.h>
-#include <uORB/Subscription.hpp>
-#include <px4_platform_common/module_params.h>
-
-class FeasibilityChecker : public ModuleParams
-{
-public:
-
-	FeasibilityChecker();
-
-	enum class VehicleType {
-		RotaryWing,
-		FixedWing,
-		Vtol,
-		Other
-	};
-
-	/**
-	 * @brief Run validity checks for mission item
-	 *
-	 *
-	 * @param mission_item The mission item to run the checks on
-	 * @param current_index The index of the current mission item in the mission
-	 * @param total_count The total number of mission items in the mission
-	 * @return False if fatal error occured and no other checks should be run.
-	*/
-	bool processNextItem(mission_item_s &mission_item, const int current_index, const int total_count);
-
-	void setMavlinkLogPub(orb_advert_t *mavlink_log_pub)
-	{
-		_mavlink_log_pub = mavlink_log_pub;
-	}
-
-	/**
-	 * @return True At least one check failed.
-	*/
-	bool someCheckFailed()
-	{
-		return _takeoff_failed ||
-		       _distance_first_waypoint_failed ||
-		       _distance_between_waypoints_failed ||
-		       _land_pattern_validity_failed ||
-		       _fixed_wing_land_approach_failed ||
-		       _below_home_alt_failed ||
-		       _mission_validity_failed ||
-		       _takeoff_land_available_failed;
-	}
-
-	/**
-	 * @brief Reset all data
-	*/
-	void reset();
-
-private:
-	orb_advert_t *_mavlink_log_pub{nullptr};
-
-	uORB::Subscription _home_pos_sub{ORB_ID(home_position)};
-	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
-
-	// parameters
-	float _param_fw_lnd_ang{0.f};
-	float _param_mis_dist_1wp{0.f};
-	float _param_mis_dist_wps{0.f};
-	float _param_nav_acc_rad{0.f};
-	int32_t _param_mis_takeoff_land_req{0};
-
-	bool _is_landed{false};
-	float _home_alt_msl{NAN};
-	matrix::Vector2d _home_lat_lon = matrix::Vector2d((double)NAN, (double)NAN);
-	VehicleType _vehicle_type{VehicleType::RotaryWing};
-
-	// internal flags to keep track of which checks failed
-	bool _mission_validity_failed{false};
-	bool _takeoff_failed{false};
-	bool _land_pattern_validity_failed{false};
-	bool _distance_first_waypoint_failed{false};
-	bool _distance_between_waypoints_failed{false};
-	bool _below_home_alt_failed{false};
-	bool _fixed_wing_land_approach_failed{false};
-	bool _takeoff_land_available_failed{false};
-
-	// internal checkTakeoff related variables
-	bool _found_item_with_position{false};
-	bool _has_vtol_takeoff{false};
-	bool _has_takeoff{false};
-
-	// internal checkFixedWingLanding related variables
-	bool _landing_valid{false};
-	int _do_land_start_index{-1};
-	int _landing_approach_index{-1};
-	mission_item_s _mission_item_previous = {};
-
-	// internal checkDistanceToFirstWaypoint variables
-	bool _first_waypoint_found{false};
-
-	// internal checkDistancesBetweenWaypoints variables
-	double _last_lat{(double)NAN};
-	double _last_lon{(double)NAN};
-	int _last_cmd{-1};
-
-	/**
-	 * @brief Update data from external topics, e.g home position
-	*/
-	void updateData();
-
-	// methods which are called for each mission item
-
-	/**
-	 * @brief Check general mission item validity, e.g. supported commands.
-	 *
-	 * @param mission_item The current mission item
-	 * @return False if the check failed.
-	*/
-	bool checkMissionItemValidity(mission_item_s &mission_item, const int current_index);
-
-	/**
-	 * @brief Check if takeoff is valid
-	 *
-	 * @param mission_item The current mission item
-	 * @return False if the check failed.
-	*/
-	bool checkTakeoff(mission_item_s &mission_item);
-
-	/**
-	 * @brief Check validity of landing pattern (fixed wing & vtol)
-	 *
-	 * @param mission_item The current mission item
-	 * @param current_index The current mission index
-	 * @param last_index The last index of the mission
-	 * @return False if the check failed.
-	*/
-	bool checkLandPatternValidity(mission_item_s &mission_item, const int current_index, const int last_index);
-
-	/**
-	 * @brief Check distance to first waypoint.
-	 *
-	 * @param mission_item The current mission item
-	 * @return False if the check failed.
-	*/
-	bool checkDistanceToFirstWaypoint(mission_item_s &mission_item);
-
-	/**
-	 * @brief Check distances between waypoints
-	 *
-	 * @param mission_item The current mission item
-	 * @return False if the check failed.
-	*/
-	bool checkDistancesBetweenWaypoints(const mission_item_s &mission_item);
-
-	/**
-	 * @brief Check if any waypoint is below the home altitude. Issues warning only.
-	 *
-	 * @param mission_item The current mission item
-	 * @param current_index The current mission index
-	 * @return Always returns true, only issues warning.
-	*/
-	bool checkIfBelowHomeAltitude(const mission_item_s &mission_item, const int current_index);
-
-	/**
-	 * @brief Check fixed wing land approach (fixed wing only)
-	 *
-	 * @param mission_item The current mission item
-	 * @param current_index The current mission index
-	 * @return False if the check failed.
-	*/
-	bool checkFixedWindLandApproach(mission_item_s &mission_item, const int current_index);
-
-	// methods which are called once at the end
-	/**
-	 * @brief Check if takeoff/landing are available according to MIS_TKO_LAND_REQ parameter
-	 *
-	 * @return False if the check failed.
-	*/
-	bool checkTakeoffLandAvailable();
-
-	/**
-	 * @brief Run checks which are common for all vehicle types
-	 *
-	 * @param mission_item The current mission item
-	 * @param current_index The current mission index
-	*/
-	void doCommonChecks(mission_item_s &mission_item, const int current_index);
-
-	/**
-	 * @brief Run checks which are only related to VTOL vehicles.
-	 *
-	 * @param mission_item The current mission item
-	 * @param current_index The current mission index
-	*/
-	void doVtolChecks(mission_item_s &mission_item, const int current_index, const int last_index);
-
-	/**
-	 * @brief Run checks which are only related to fixed wing vehicles.
-	 *
-	 * @param mission_item The current mission item
-	 * @param current_index The current mission index
-	 * @param last_index The last mission index
-	 * @return False if the check failed.
-	*/
-	void doFixedWingChecks(mission_item_s &mission_item, const int current_index, const int last_index);
-
-	/**
-	 * @brief Run checks which are only related to multirotor vehicles.
-	 *
-	 * @param mission_item The current mission item
-	 * @param current_index The current mission index
-	 * @return False if the check failed.
-	*/
-	void doMulticopterChecks(mission_item_s &mission_item, const int current_index);
-};

src/modules/navigator/MissionFeasibility/FeasibilityCheckerTest.cpp

@@ -1,491 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2022 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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-#include <gtest/gtest.h>
-#include "FeasibilityChecker.hpp"
-#include <uORB/topics/home_position.h>
-#include <uORB/topics/vehicle_land_detected.h>
-#include <lib/geo/geo.h>
-
-
-// to run: make tests TESTFILTER=FeasibilityChecker
-class FeasibilityCheckerTest : public ::testing::Test
-{
-public:
-	void SetUp() override
-	{
-		param_control_autosave(false);
-		param_reset_all();
-	}
-};
-
-class TestFeasibilityChecker : public FeasibilityChecker
-{
-public:
-	TestFeasibilityChecker() : FeasibilityChecker() {}
-	void paramsChanged() {FeasibilityChecker::updateParamsImpl();}
-
-	void publishHomePosition(double lat, double lon, float alt)
-	{
-		home_position_s home = {};
-		home.alt = alt;
-		home.valid_alt = true;
-		home.lat = lat;
-		home.lon = lon;
-		home.valid_hpos = true;
-		orb_advert_t home_pub = orb_advertise(ORB_ID(home_position), &home);
-		orb_publish(ORB_ID(home_position), home_pub, &home);
-	}
-
-	void publishLanded(bool landed)
-	{
-		vehicle_land_detected_s land_detected = {};
-		land_detected.landed = true;
-		orb_advert_t landed_pub = orb_advertise(ORB_ID(vehicle_land_detected), &land_detected);
-		orb_publish(ORB_ID(vehicle_land_detected), landed_pub, &land_detected);
-	}
-
-	void publishVehicleType(int vehicle_type)
-	{
-		vehicle_status_s status = {};
-		status.vehicle_type = vehicle_type;
-		orb_advert_t status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
-		orb_publish(ORB_ID(vehicle_status), status_pub, &status);
-	}
-
-};
-
-TEST_F(FeasibilityCheckerTest, instantiation)
-{
-	FeasibilityChecker checker();
-}
-
-TEST_F(FeasibilityCheckerTest, mission_item_validity)
-{
-	TestFeasibilityChecker checker;
-
-
-	checker.publishLanded(true);
-
-	// supported mission item should pass
-	mission_item_s mission_item = {};
-	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-	checker.processNextItem(mission_item, 0, 1);
-	ASSERT_EQ(checker.someCheckFailed(), false);
-
-	// when landed the first item cannot be a land item
-	checker.reset();
-	checker.publishHomePosition(0, 0, 100.f);
-	checker.publishLanded(true);
-	mission_item.nav_cmd = NAV_CMD_LAND;
-	bool ret = checker.processNextItem(mission_item, 0, 1);
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-	// mission item validity failed
-	ASSERT_EQ(ret, false);
-
-	checker.reset();
-	mission_item.nav_cmd = NAV_CMD_TAKEOFF;
-	mission_item.altitude_is_relative = true;
-	ret = checker.processNextItem(mission_item, 0, 5);
-
-	// home alt is not valid but we have a relative mission item, should fail immediately
-	ASSERT_EQ(ret, false);
-}
-
-TEST_F(FeasibilityCheckerTest, max_dist_between_waypoints)
-{
-	TestFeasibilityChecker checker;
-	checker.publishLanded(true);
-
-	param_t param = param_handle(px4::params::MIS_DIST_WPS);
-
-	float max_dist = 1000.0f;
-	param_set(param, &max_dist);
-	checker.paramsChanged();
-
-	mission_item_s mission_item = {};
-	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-
-	checker.processNextItem(mission_item, 0, 3);
-
-	// waypoints are within limits, check should pass
-	double lat_new, lon_new;
-	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 999, &lat_new, &lon_new);
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-	checker.processNextItem(mission_item, 1, 3);
-
-	ASSERT_EQ(checker.someCheckFailed(), false);
-
-	// waypoints are above limits, check should fail
-	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1001, &lat_new, &lon_new);
-
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-	checker.processNextItem(mission_item, 2, 3);
-
-	ASSERT_EQ(checker.someCheckFailed(), true);
-}
-
-
-TEST_F(FeasibilityCheckerTest, check_dist_first_waypoint)
-{
-	TestFeasibilityChecker checker;
-	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 0);
-
-	param_t param = param_handle(px4::params::MIS_DIST_1WP);
-
-	float max_dist = 500.0f;
-	param_set(param, &max_dist);
-	checker.paramsChanged();
-
-
-	mission_item_s mission_item = {};
-	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-	double lat_new, lon_new;
-	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 501, &lat_new, &lon_new);
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-	checker.processNextItem(mission_item, 0, 1);
-
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-	checker.reset();
-	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 0);
-	waypoint_from_heading_and_distance(0, 0, 0, 499, &lat_new, &lon_new);
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-	checker.processNextItem(mission_item, 0, 1);
-
-	ASSERT_EQ(checker.someCheckFailed(), false);
-}
-
-TEST_F(FeasibilityCheckerTest, check_below_home)
-{
-	TestFeasibilityChecker checker;
-	checker.publishLanded(true);
-	//checker.publishHomePosition(0,0,100);
-
-
-	mission_item_s mission_item = {};
-	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-	mission_item.altitude = 50;
-	mission_item.altitude_is_relative = true;
-
-	checker.processNextItem(mission_item, 0, 1);
-
-	// this is done to invalidate the home position
-	checker.reset();
-	checker.publishLanded(true);
-	checker.processNextItem(mission_item, 0, 1);
-
-	// cannot have relative altitude without valid home position
-	ASSERT_EQ(checker.someCheckFailed(), true);
-}
-
-TEST_F(FeasibilityCheckerTest, check_takeoff)
-{
-	TestFeasibilityChecker checker;
-	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 100);
-
-
-	// takeoff altitude is smaller than acceptance radius, should fail
-	mission_item_s mission_item = {};
-	mission_item.nav_cmd = NAV_CMD_TAKEOFF;
-	mission_item.altitude = -5.0f;
-	mission_item.altitude_is_relative = true;
-
-	checker.processNextItem(mission_item, 0, 1);
-
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-	checker.reset();
-	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 100);
-
-
-	// takeoff altitude is larger than home altitude, should pass
-	mission_item.altitude = 0.1f;
-	checker.processNextItem(mission_item, 0, 1);
-
-	ASSERT_EQ(checker.someCheckFailed(), false);
-
-
-	// takeoff item needs to be first item
-	checker.reset();
-	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 100);
-
-	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-
-	checker.processNextItem(mission_item, 0, 2);
-
-	mission_item.nav_cmd = NAV_CMD_TAKEOFF;
-
-	checker.processNextItem(mission_item, 1, 2);
-
-	ASSERT_EQ(checker.someCheckFailed(), true);
-}
-
-TEST_F(FeasibilityCheckerTest, fixed_wing_land_approach)
-{
-	TestFeasibilityChecker checker;
-	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 100);
-	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
-
-	mission_item_s mission_item = {};
-	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-	mission_item.altitude = 50;
-	mission_item.altitude_is_relative = true;
-
-	checker.processNextItem(mission_item, 0, 2);
-
-	mission_item.nav_cmd = NAV_CMD_LAND;
-	mission_item.altitude = 60;
-	checker.processNextItem(mission_item, 1, 2);
-
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-
-	// landing point should not be within loiter radius of previous waypoint
-	checker.reset();
-	checker.publishLanded(true);
-	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
-	checker.publishHomePosition(0, 0, 100);
-	mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
-	mission_item.altitude = 50;
-	mission_item.loiter_radius = 100;
-
-	checker.processNextItem(mission_item, 0, 2);
-
-	double lat_new, lon_new;
-	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 99, &lat_new, &lon_new);
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-	mission_item.nav_cmd = NAV_CMD_LAND;
-	mission_item.altitude = 40;
-
-	checker.processNextItem(mission_item, 1, 2);
-
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-	// only loiter to alt or plain waypoint type are allowed before landing
-	checker.reset();
-	checker.publishLanded(true);
-	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
-	checker.publishHomePosition(0, 0, 100);
-	mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-	mission_item.altitude = 50;
-	mission_item.loiter_radius = 50;
-
-	checker.processNextItem(mission_item, 0, 2);
-
-	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 99, &lat_new, &lon_new);
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-	mission_item.nav_cmd = NAV_CMD_LAND;
-	mission_item.altitude = 40;
-
-	checker.processNextItem(mission_item, 1, 2);
-
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-	// fail the glide slope
-	checker.reset();
-	checker.publishLanded(true);
-	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
-	checker.publishHomePosition(0, 0, 100);
-	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-	mission_item.altitude = 50;
-	mission_item.loiter_radius = 50;
-
-	checker.processNextItem(mission_item, 0, 2);
-
-	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1, &lat_new, &lon_new);
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-	mission_item.nav_cmd = NAV_CMD_LAND;
-	mission_item.altitude = 40;
-
-	checker.processNextItem(mission_item, 1, 2);
-
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-	// fixed wing land approach checks should not be executed for rotary wing
-	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
-	checker.reset();
-	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 100);
-	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-	mission_item.altitude = 50;
-	mission_item.loiter_radius = 50;
-
-	checker.processNextItem(mission_item, 0, 2);
-
-	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 1, &lat_new, &lon_new);
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-	mission_item.nav_cmd = NAV_CMD_LAND;
-	mission_item.altitude = 40;
-
-	checker.processNextItem(mission_item, 1, 2);
-
-	ASSERT_EQ(checker.someCheckFailed(), false);
-}
-
-TEST_F(FeasibilityCheckerTest, fixed_wing_landing)
-{
-	// multiple do land start are not allowed
-	TestFeasibilityChecker checker;
-	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 100);
-	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
-
-	mission_item_s mission_item = {};
-	mission_item.nav_cmd = NAV_CMD_DO_LAND_START;
-
-	checker.processNextItem(mission_item, 0, 2);
-	checker.processNextItem(mission_item, 1, 2);
-
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-
-	// cannot start with land waypoint
-	checker.reset();
-	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
-	mission_item.nav_cmd = NAV_CMD_LAND;
-	checker.processNextItem(mission_item, 0, 2);
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-	// cannot have land start before RTL
-	checker.reset();
-	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
-	mission_item.nav_cmd = NAV_CMD_DO_LAND_START;
-	checker.processNextItem(mission_item, 0, 2);
-
-	mission_item.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
-	checker.processNextItem(mission_item, 1, 2);
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-	// cannot have land start after landing waypoint
-	checker.reset();
-	checker.publishHomePosition(0, 0, 100);
-	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
-	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-	mission_item.altitude = 10;
-	mission_item.altitude_is_relative = true;
-	checker.processNextItem(mission_item, 0, 3);
-
-	mission_item.nav_cmd = NAV_CMD_LAND;
-	mission_item.altitude = 0;
-	double lat_new, lon_new;
-	waypoint_from_heading_and_distance(mission_item.lat, mission_item.lon, 0, 200, &lat_new, &lon_new);
-	mission_item.lat = lat_new;
-	mission_item.lon = lon_new;
-	checker.processNextItem(mission_item, 1, 3);
-
-	mission_item.nav_cmd = NAV_CMD_DO_LAND_START;
-	checker.processNextItem(mission_item, 2, 3);
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-
-	checker.reset();
-	param_t param = param_handle(px4::params::MIS_TKO_LAND_REQ);
-	// not takeoff land requiremntes, check should pass
-	int param_val = 0;
-	param_set(param, &param_val);
-	checker.paramsChanged();
-	checker.publishHomePosition(0, 0, 100);
-	checker.publishVehicleType(vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
-	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-	checker.processNextItem(mission_item, 0, 3);
-	mission_item.nav_cmd = NAV_CMD_DO_LAND_START;
-	checker.processNextItem(mission_item, 1, 3);
-	mission_item.nav_cmd = NAV_CMD_LAND;
-	checker.processNextItem(mission_item, 2, 3);
-	ASSERT_EQ(checker.someCheckFailed(), false);
-}
-
-TEST_F(FeasibilityCheckerTest, check_takeoff_land_requirements)
-{
-
-	TestFeasibilityChecker checker;
-	checker.publishLanded(true);
-	checker.publishHomePosition(0, 0, 0);
-
-	param_t param = param_handle(px4::params::MIS_TKO_LAND_REQ);
-
-	// not takeoff land requiremntes, check should pass
-	int param_val = 0;
-	param_set(param, &param_val);
-	checker.paramsChanged();
-
-	mission_item_s mission_item = {};
-	mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-
-	checker.processNextItem(mission_item, 0, 1);
-	ASSERT_EQ(checker.someCheckFailed(), false);
-
-	// require takeoff, check should fail
-	param_val = 1;
-	param_set(param, &param_val);
-	checker.paramsChanged();
-	checker.processNextItem(mission_item, 0, 1);
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-	// require landing, check should fail
-	param_val = 2;
-	param_set(param, &param_val);
-	checker.paramsChanged();
-	checker.processNextItem(mission_item, 0, 1);
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-	// require both takeoff and landing, check should fail
-	param_val = 3;
-	param_set(param, &param_val);
-	checker.paramsChanged();
-	checker.processNextItem(mission_item, 0, 1);
-	ASSERT_EQ(checker.someCheckFailed(), true);
-
-	// require takeoff and land or none, this should pass as we don't have either
-	param_val = 4;
-	param_set(param, &param_val);
-	checker.paramsChanged();
-	checker.processNextItem(mission_item, 0, 1);
-	ASSERT_EQ(checker.someCheckFailed(), false);
-}

src/modules/navigator/mission.cpp

@@ -1747,7 +1747,9 @@ Mission::check_mission_valid(bool force)
 		MissionFeasibilityChecker _missionFeasibilityChecker(_navigator);
 
 		_navigator->get_mission_result()->valid =
-			_missionFeasibilityChecker.checkMissionFeasible(_mission);
+			_missionFeasibilityChecker.checkMissionFeasible(_mission,
+					_param_mis_dist_1wp.get(),
+					_param_mis_dist_wps.get());
 
 		_navigator->get_mission_result()->seq_total = _mission.count;
 		_navigator->increment_mission_instance_count();

src/modules/navigator/mission_feasibility_checker.cpp

@@ -41,7 +41,6 @@
  */
 
 #include "mission_feasibility_checker.h"
-#include "MissionFeasibility/FeasibilityChecker.hpp"
 
 #include "mission_block.h"
 #include "navigator.h"
@@ -54,50 +53,58 @@
 #include <px4_platform_common/events.h>
 
 bool
-MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission)
+MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
+		float max_distance_to_1st_waypoint, float max_distance_between_waypoints)
 {
 	// Reset warning flag
 	_navigator->get_mission_result()->warning = false;
 
-	// first check if we have a valid position
-	const bool home_valid = _navigator->home_global_position_valid();
-	const bool home_alt_valid = _navigator->home_alt_valid();
-
 	// trivial case: A mission with length zero cannot be valid
 	if ((int)mission.count <= 0) {
 		return false;
 	}
 
-	if (!home_alt_valid) {
-		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.\t");
-		events::send(events::ID("navigator_mis_no_pos_lock"), events::Log::Info, "Not yet ready for mission, no position lock");
-		return false;
-	}
-
 	bool failed = false;
 
-	for (size_t i = 0; i < mission.count; i++) {
-		struct mission_item_s missionitem = {};
-		const ssize_t len = sizeof(struct mission_item_s);
+	// first check if we have a valid position
+	const bool home_valid = _navigator->home_global_position_valid();
+	const bool home_alt_valid = _navigator->home_alt_valid();
 
-		if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
-			_navigator->get_mission_result()->warning = true;
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			return false;
-		}
-
-		if (!_feasibility_checker.processNextItem(missionitem, i, mission.count)) {
-			failed = true;
-			break;
-		}
+	if (!home_alt_valid) {
+		failed = true;
+		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.\t");
+		events::send(events::ID("navigator_mis_no_pos_lock"), events::Log::Info, "Not yet ready for mission, no position lock");
 
+	} else {
+		failed |= !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint);
 	}
 
-	failed |= _feasibility_checker.someCheckFailed();
+	const float home_alt = _navigator->get_home_position()->alt;
 
-	failed |= !checkGeofence(mission, _navigator->get_home_position()->alt, home_valid);
+	// reset for next check
+	_has_takeoff = false;
+	_has_landing = false;
 
-	_navigator->get_mission_result()->warning = failed;
+	// run generic (for all vehicle types) checks
+	failed |= !checkMissionItemValidity(mission);
+	failed |= !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints);
+	failed |= !checkGeofence(mission, home_alt, home_valid);
+	failed |= !checkHomePositionAltitude(mission, home_alt, home_alt_valid);
+	failed |= !checkTakeoff(mission, home_alt);
+
+	// run type-specifc landing checks, which also include seting _has_landing that is used in checkTakeoffLandAvailable()
+	if (_navigator->get_vstatus()->is_vtol) {
+		failed |= !checkVTOLLanding(mission);
+
+	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+		failed |= !checkFixedWingLanding(mission);
+
+	} else {
+		// if neither VTOL nor FW, only check if mission has landing but don't check that one for validity
+		_has_landing = hasMissionLanding(mission);
+	}
+
+	failed |= !checkTakeoffLandAvailable();
 
 	return !failed;
 }
@@ -146,3 +153,749 @@ MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_al
 
 	return true;
 }
+
+bool
+MissionFeasibilityChecker::checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid)
+{
+	/* Check if all waypoints are above the home altitude */
+	for (size_t i = 0; i < mission.count; i++) {
+		struct mission_item_s missionitem = {};
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
+			_navigator->get_mission_result()->warning = true;
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return false;
+		}
+
+		/* reject relative alt without home set */
+		if (missionitem.altitude_is_relative && !home_alt_valid && MissionBlock::item_contains_position(missionitem)) {
+
+			_navigator->get_mission_result()->warning = true;
+
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No home pos, WP %zu uses rel alt\t", i + 1);
+			events::send<int16_t>(events::ID("navigator_mis_no_home_rel_alt"), {events::Log::Error, events::LogInternal::Info},
+					      "Mission rejected: No home position, waypoint {1} uses relative altitude",
+					      i + 1);
+			return false;
+
+		}
+
+		/* calculate the global waypoint altitude */
+		float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
+
+		if (home_alt_valid && home_alt > wp_alt && MissionBlock::item_contains_position(missionitem)) {
+
+			_navigator->get_mission_result()->warning = true;
+
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Warning: Waypoint %zu below home\t", i + 1);
+			events::send<int16_t>(events::ID("navigator_mis_wp_below_home"), {events::Log::Warning, events::LogInternal::Info},
+					      "Waypoint {1} below home", i + 1);
+		}
+	}
+
+	return true;
+}
+
+bool
+MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
+{
+	// do not allow mission if we find unsupported item
+	for (size_t i = 0; i < mission.count; i++) {
+		struct mission_item_s missionitem;
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
+			// not supposed to happen unless the datamanager can't access the SD card, etc.
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Cannot access SD card\t");
+			events::send(events::ID("navigator_mis_sd_failure"), events::Log::Error,
+				     "Mission rejected: Cannot access mission storage");
+			return false;
+		}
+
+		// check if we find unsupported items 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_TIME_LIMIT &&
+		    missionitem.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
+		    missionitem.nav_cmd != NAV_CMD_LAND &&
+		    missionitem.nav_cmd != NAV_CMD_TAKEOFF &&
+		    missionitem.nav_cmd != NAV_CMD_LOITER_TO_ALT &&
+		    missionitem.nav_cmd != NAV_CMD_VTOL_TAKEOFF &&
+		    missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
+		    missionitem.nav_cmd != NAV_CMD_DELAY &&
+		    missionitem.nav_cmd != NAV_CMD_CONDITION_GATE &&
+		    missionitem.nav_cmd != NAV_CMD_DO_WINCH &&
+		    missionitem.nav_cmd != NAV_CMD_DO_GRIPPER &&
+		    missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
+		    missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
+		    missionitem.nav_cmd != NAV_CMD_DO_SET_HOME &&
+		    missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
+		    missionitem.nav_cmd != NAV_CMD_DO_SET_ACTUATOR &&
+		    missionitem.nav_cmd != NAV_CMD_DO_LAND_START &&
+		    missionitem.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL &&
+		    missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
+		    missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE &&
+		    missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
+		    missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE &&
+		    missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE &&
+		    missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO &&
+		    missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
+		    missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
+		    missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW &&
+		    missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE &&
+		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&
+		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION &&
+		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET &&
+		    missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_NONE &&
+		    missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
+		    missionitem.nav_cmd != NAV_CMD_OBLIQUE_SURVEY &&
+		    missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
+		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
+		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
+		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
+		    missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
+
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d\t",
+					     (int)(i + 1),
+					     (int)missionitem.nav_cmd);
+			events::send<uint16_t, uint16_t>(events::ID("navigator_mis_unsup_cmd"), {events::Log::Error, events::LogInternal::Warning},
+							 "Mission rejected: item {1}: unsupported command: {2}", i + 1, missionitem.nav_cmd);
+			return false;
+		}
+
+		/* Check non navigation item */
+		if (missionitem.nav_cmd == NAV_CMD_DO_SET_SERVO) {
+
+			/* 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\t",
+						     (int)missionitem.params[0]);
+				events::send<uint32_t>(events::ID("navigator_mis_act_index"), {events::Log::Error, events::LogInternal::Warning},
+						       "Actuator number {1} 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\t", (int)missionitem.params[1]);
+				events::send<uint32_t, uint32_t>(events::ID("navigator_mis_act_range"), {events::Log::Error, events::LogInternal::Warning},
+								 "Actuator value {1} is out of bounds -{2}..{2}", (int)missionitem.params[1], PWM_DEFAULT_MAX);
+				return false;
+			}
+		}
+
+		// check if the mission starts with a land command while the vehicle is landed
+		if ((i == 0) && missionitem.nav_cmd == NAV_CMD_LAND && _navigator->get_land_detected()->landed) {
+
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with landing\t");
+			events::send(events::ID("navigator_mis_starts_w_landing"), {events::Log::Error, events::LogInternal::Info},
+				     "Mission rejected: starts with landing");
+			return false;
+		}
+	}
+
+	return true;
+}
+
+bool
+MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt)
+{
+	bool takeoff_first = false;
+	int takeoff_index = -1;
+
+	for (size_t i = 0; i < mission.count; i++) {
+		struct mission_item_s missionitem = {};
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		if (dm_read((dm_item_t)mission.dataman_id, 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 || missionitem.nav_cmd == NAV_CMD_VTOL_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 = _navigator->get_default_acceptance_radius();
+
+			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!\t");
+				/* EVENT
+				 * @description The minimum takeoff altitude is the acceptance radius plus 1m.
+				 */
+				events::send<float>(events::ID("navigator_mis_takeoff_too_low"), {events::Log::Error, events::LogInternal::Info},
+						    "Mission rejected: takeoff altitude too low! Minimum: {1:.1m_v}", acceptance_radius + 1.f);
+				return false;
+			}
+
+			// tell that mission has a takeoff waypoint
+			_has_takeoff = true;
+
+			// tell that a takeoff waypoint is the first "waypoint"
+			// mission item
+			if (i == 0) {
+				takeoff_first = true;
+
+			} else if (takeoff_index == -1) {
+				// stores the index of the first takeoff waypoint
+				takeoff_index = i;
+			}
+		}
+	}
+
+	if (takeoff_index != -1) {
+		// checks if all the mission items before the first takeoff waypoint
+		// are not waypoints or position-related items;
+		// this means that, before a takeoff waypoint, one can set
+		// one of the bellow mission items
+		for (size_t i = 0; i < (size_t)takeoff_index; i++) {
+			struct mission_item_s missionitem = {};
+			const ssize_t len = sizeof(struct mission_item_s);
+
+			if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
+				/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+				return false;
+			}
+
+			takeoff_first = !(missionitem.nav_cmd != NAV_CMD_IDLE &&
+					  missionitem.nav_cmd != NAV_CMD_DELAY &&
+					  missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
+					  missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
+					  missionitem.nav_cmd != NAV_CMD_DO_SET_HOME &&
+					  missionitem.nav_cmd != NAV_CMD_DO_SET_SERVO &&
+					  missionitem.nav_cmd != NAV_CMD_DO_LAND_START &&
+					  missionitem.nav_cmd != NAV_CMD_DO_TRIGGER_CONTROL &&
+					  missionitem.nav_cmd != NAV_CMD_DO_DIGICAM_CONTROL &&
+					  missionitem.nav_cmd != NAV_CMD_IMAGE_START_CAPTURE &&
+					  missionitem.nav_cmd != NAV_CMD_IMAGE_STOP_CAPTURE &&
+					  missionitem.nav_cmd != NAV_CMD_VIDEO_START_CAPTURE &&
+					  missionitem.nav_cmd != NAV_CMD_VIDEO_STOP_CAPTURE &&
+					  missionitem.nav_cmd != NAV_CMD_DO_CONTROL_VIDEO &&
+					  missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONFIGURE &&
+					  missionitem.nav_cmd != NAV_CMD_DO_MOUNT_CONTROL &&
+					  missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW &&
+					  missionitem.nav_cmd != NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE &&
+					  missionitem.nav_cmd != NAV_CMD_DO_SET_ROI &&
+					  missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_LOCATION &&
+					  missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_WPNEXT_OFFSET &&
+					  missionitem.nav_cmd != NAV_CMD_DO_SET_ROI_NONE &&
+					  missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_DIST &&
+					  missionitem.nav_cmd != NAV_CMD_OBLIQUE_SURVEY &&
+					  missionitem.nav_cmd != NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL &&
+					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
+					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
+					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
+					  missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION);
+		}
+	}
+
+	if (_has_takeoff && !takeoff_first) {
+		// check if the takeoff waypoint is the first waypoint item on the mission
+		// i.e, an item with position/attitude change modification
+		// if it is not, the mission should be rejected
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff not first waypoint item\t");
+		events::send(events::ID("navigator_mis_takeoff_not_first"), {events::Log::Error, events::LogInternal::Info},
+			     "Mission rejected: takeoff is not the first waypoint item");
+		return false;
+	}
+
+	// all checks have passed
+	return true;
+}
+
+bool
+MissionFeasibilityChecker::hasMissionLanding(const mission_s &mission)
+{
+	// Go through all mission items and search for a landing waypoint.
+	// For MC we currently do not run any checks on the validity of the planned landing.
+
+	bool mission_landing_found = false;
+
+	for (size_t i = 0; i < mission.count; i++) {
+		struct mission_item_s missionitem;
+		const ssize_t len = sizeof(missionitem);
+
+		if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return false;
+		}
+
+		if (missionitem.nav_cmd == NAV_CMD_LAND) {
+			mission_landing_found = true;
+		}
+	}
+
+	return mission_landing_found;
+}
+
+bool
+MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission)
+{
+	/* 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;
+
+	size_t do_land_start_index = 0;
+	size_t landing_approach_index = 0;
+
+	for (size_t i = 0; i < mission.count; i++) {
+		struct mission_item_s missionitem;
+		const ssize_t len = sizeof(missionitem);
+
+		if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return false;
+		}
+
+		// if DO_LAND_START found then require valid landing AFTER
+		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
+			if (_has_landing) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.\t");
+				events::send(events::ID("navigator_mis_multiple_land"), {events::Log::Error, events::LogInternal::Info},
+					     "Mission rejected: more than one land start commands");
+				return false;
+
+			} else {
+				_has_landing = true;
+				do_land_start_index = i;
+			}
+		}
+
+		if (missionitem.nav_cmd == NAV_CMD_LAND) {
+			mission_item_s missionitem_previous {};
+
+			_has_landing = true;
+
+			float param_fw_lnd_ang = 0.0f;
+			const param_t param_handle_fw_lnd_ang = param_find("FW_LND_ANG");
+
+			if (param_handle_fw_lnd_ang == PARAM_INVALID) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: FW_LND_ANG parameter is missing.\t");
+				events::send(events::ID("navigator_mis_land_angle_param_missing"), {events::Log::Error, events::LogInternal::Info},
+					     "Mission rejected: FW_LND_ANG parameter is missing");
+				return false;
+
+			} else {
+				param_get(param_handle_fw_lnd_ang, &param_fw_lnd_ang);
+			}
+
+			if (i > 0) {
+				landing_approach_index = i - 1;
+
+				if (dm_read((dm_item_t)mission.dataman_id, landing_approach_index, &missionitem_previous, len) != len) {
+					/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+					return false;
+				}
+
+				if (MissionBlock::item_contains_position(missionitem_previous)) {
+
+					const float land_alt_amsl = missionitem.altitude_is_relative ? missionitem.altitude +
+								    _navigator->get_home_position()->alt : missionitem.altitude;
+					const float entrance_alt_amsl = missionitem_previous.altitude_is_relative ? missionitem_previous.altitude +
+									_navigator->get_home_position()->alt : missionitem_previous.altitude;
+					const float relative_approach_altitude = entrance_alt_amsl - land_alt_amsl;
+
+					if (relative_approach_altitude < FLT_EPSILON) {
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+								     "Mission rejected: the approach waypoint must be above the landing point.\t");
+						events::send(events::ID("navigator_mis_approach_wp_below_land"), {events::Log::Error, events::LogInternal::Info},
+							     "Mission rejected: the approach waypoint must be above the landing point");
+						return false;
+					}
+
+					float landing_approach_distance;
+
+					if (missionitem_previous.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
+						// assume this is a fixed-wing landing pattern with orbit to alt followed
+						// by tangent exit to landing approach and touchdown at landing waypoint
+
+						const float distance_orbit_center_to_land = get_distance_to_next_waypoint(missionitem_previous.lat,
+								missionitem_previous.lon, missionitem.lat, missionitem.lon);
+						const float orbit_radius = fabsf(missionitem_previous.loiter_radius);
+
+						if (distance_orbit_center_to_land <= orbit_radius) {
+							mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+									     "Mission rejected: the landing point must be outside the orbit radius.\t");
+							events::send(events::ID("navigator_mis_land_wp_inside_orbit_radius"), {events::Log::Error, events::LogInternal::Info},
+								     "Mission rejected: the landing point must be outside the orbit radius");
+							return false;
+						}
+
+						landing_approach_distance = sqrtf(distance_orbit_center_to_land * distance_orbit_center_to_land - orbit_radius *
+										  orbit_radius);
+
+					} else if (missionitem_previous.nav_cmd == NAV_CMD_WAYPOINT) {
+						// approaching directly from waypoint position
+
+						const float waypoint_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon,
+										missionitem.lat, missionitem.lon);
+						landing_approach_distance = waypoint_distance;
+
+					} else {
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+								     "Mission rejected: unsupported landing approach entrance waypoint type. Only ORBIT_TO_ALT or WAYPOINT allowed.\t");
+						events::send(events::ID("navigator_mis_unsupported_landing_approach_wp"), {events::Log::Error, events::LogInternal::Info},
+							     "Mission rejected: unsupported landing approach entrance waypoint type. Only ORBIT_TO_ALT or WAYPOINT allowed");
+						return false;
+					}
+
+					const float glide_slope = relative_approach_altitude / landing_approach_distance;
+
+					// respect user setting as max glide slope, but account for floating point
+					// rounding on next check with small (arbitrary) 0.1 deg buffer, as the
+					// landing angle parameter is what is typically used for steepest glide
+					// in landing config
+					const float max_glide_slope = tanf(math::radians(param_fw_lnd_ang + 0.1f));
+
+					if (glide_slope > max_glide_slope) {
+
+						const uint8_t land_angle_left_of_decimal = (uint8_t)param_fw_lnd_ang;
+						const uint8_t land_angle_first_after_decimal = (uint8_t)((param_fw_lnd_ang - floorf(
+									param_fw_lnd_ang)) * 10.0f);
+
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+								     "Mission rejected: the landing glide slope is steeper than the vehicle setting of %d.%d degrees.\t",
+								     (int)land_angle_left_of_decimal, (int)land_angle_first_after_decimal);
+						events::send<uint8_t, uint8_t>(events::ID("navigator_mis_glide_slope_too_steep"), {events::Log::Error, events::LogInternal::Info},
+									       "Mission rejected: the landing glide slope is steeper than the vehicle setting of {1}.{2} degrees",
+									       land_angle_left_of_decimal, land_angle_first_after_decimal);
+
+						const uint32_t acceptable_entrance_alt = (uint32_t)(max_glide_slope * landing_approach_distance);
+						const uint32_t acceptable_landing_dist = (uint32_t)ceilf(relative_approach_altitude / max_glide_slope);
+
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+								     "Reduce the glide slope, lower the entrance altitude %d meters, or increase the landing approach distance %d meters.\t",
+								     (int)acceptable_entrance_alt, (int)acceptable_landing_dist);
+						events::send<uint32_t, uint32_t>(events::ID("navigator_mis_correct_glide_slope"), {events::Log::Error, events::LogInternal::Info},
+										 "Reduce the glide slope, lower the entrance altitude {1} meters, or increase the landing approach distance {2} meters",
+										 acceptable_entrance_alt, acceptable_landing_dist);
+
+						return false;
+					}
+
+					landing_valid = true;
+
+				} else {
+					// mission item before land doesn't have a position
+					mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: need landing approach.\t");
+					events::send(events::ID("navigator_mis_req_landing_approach"), {events::Log::Error, events::LogInternal::Info},
+						     "Mission rejected: landing approach is required");
+					return false;
+				}
+
+			} else {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint.\t");
+				events::send(events::ID("navigator_mis_starts_w_landing2"), {events::Log::Error, events::LogInternal::Info},
+					     "Mission rejected: starts with landing");
+				return false;
+			}
+
+		} else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
+			if (_has_landing && do_land_start_index < i) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+						     "Mission rejected: land start item before RTL item not possible.\t");
+				events::send(events::ID("navigator_mis_land_before_rtl"), {events::Log::Error, events::LogInternal::Info},
+					     "Mission rejected: land start item before RTL item is not possible");
+				return false;
+			}
+		}
+	}
+
+	if (_has_landing && (!landing_valid || (do_land_start_index > landing_approach_index))) {
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.\t");
+		events::send(events::ID("navigator_mis_invalid_land"), {events::Log::Error, events::LogInternal::Info},
+			     "Mission rejected: invalid land start");
+		return false;
+	}
+
+	/* No landing waypoints or no waypoints */
+	return true;
+}
+
+bool
+MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission)
+{
+	// Go through all mission items and search for a landing waypoint, then run some checks on it
+
+	size_t do_land_start_index = 0;
+	size_t landing_approach_index = 0;
+
+	for (size_t i = 0; i < mission.count; i++) {
+		struct mission_item_s missionitem;
+		const ssize_t len = sizeof(missionitem);
+
+		if (dm_read((dm_item_t)mission.dataman_id, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return false;
+		}
+
+		// if DO_LAND_START found then require valid landing AFTER
+		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
+			if (_has_landing) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.\t");
+				events::send(events::ID("navigator_mis_multi_land"), {events::Log::Error, events::LogInternal::Info},
+					     "Mission rejected: more than one land start commands");
+				return false;
+
+			} else {
+				_has_landing = true;
+				do_land_start_index = i;
+			}
+		}
+
+		if (missionitem.nav_cmd == NAV_CMD_LAND || missionitem.nav_cmd == NAV_CMD_VTOL_LAND) {
+			mission_item_s missionitem_previous {};
+
+			_has_landing = true;
+
+			if (i > 0) {
+				landing_approach_index = i - 1;
+
+				if (dm_read((dm_item_t)mission.dataman_id, landing_approach_index, &missionitem_previous, len) != len) {
+					/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+					return false;
+				}
+
+
+			} else {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with land waypoint.\t");
+				events::send(events::ID("navigator_mis_starts_w_land"), {events::Log::Error, events::LogInternal::Info},
+					     "Mission rejected: starts with land waypoint");
+				return false;
+			}
+
+		} else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
+			if (_has_landing && do_land_start_index < i) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+						     "Mission rejected: land start item before RTL item not possible.\t");
+				events::send(events::ID("navigator_mis_land_before_rtl2"), {events::Log::Error, events::LogInternal::Info},
+					     "Mission rejected: land start item before RTL item is not possible");
+				return false;
+			}
+		}
+	}
+
+	if (_has_landing && (do_land_start_index > landing_approach_index)) {
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.\t");
+		events::send(events::ID("navigator_mis_invalid_land2"), {events::Log::Error, events::LogInternal::Info},
+			     "Mission rejected: invalid land start");
+		return false;
+	}
+
+	/* No landing waypoints or no waypoints */
+	return true;
+}
+
+bool
+MissionFeasibilityChecker::checkTakeoffLandAvailable()
+{
+	bool result = true;
+
+	switch (_navigator->get_takeoff_land_required()) {
+	case 0:
+		result = true;
+		break;
+
+	case 1:
+		result = _has_takeoff;
+
+		if (!result) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff waypoint required.\t");
+			events::send(events::ID("navigator_mis_takeoff_missing"), {events::Log::Error, events::LogInternal::Info},
+				     "Mission rejected: Takeoff waypoint required");
+			return false;
+		}
+
+		break;
+
+	case 2:
+		result = _has_landing;
+
+		if (!result) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Landing waypoint/pattern required.\t");
+			events::send(events::ID("navigator_mis_land_missing"), {events::Log::Error, events::LogInternal::Info},
+				     "Mission rejected: Landing waypoint/pattern required");
+		}
+
+		break;
+
+	case 3:
+		result = _has_takeoff && _has_landing;
+
+		if (!result) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff or Landing item missing.\t");
+			events::send(events::ID("navigator_mis_takeoff_or_land_missing"), {events::Log::Error, events::LogInternal::Info},
+				     "Mission rejected: Takeoff or Landing item missing");
+		}
+
+		break;
+
+	case 4:
+		result = _has_takeoff == _has_landing;
+
+		if (!result && (_has_takeoff)) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Add Landing item or remove Takeoff.\t");
+			events::send(events::ID("navigator_mis_add_land_or_rm_to"), {events::Log::Error, events::LogInternal::Info},
+				     "Mission rejected: Add Landing item or remove Takeoff");
+
+		} else if (!result && (_has_landing)) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Add Takeoff item or remove Landing.\t");
+			events::send(events::ID("navigator_mis_add_to_or_rm_land"), {events::Log::Error, events::LogInternal::Info},
+				     "Mission rejected: Add Takeoff item or remove Landing");
+		}
+
+		break;
+
+	default:
+		result = true;
+		break;
+	}
+
+	return result;
+}
+
+bool
+MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance)
+{
+	if (max_distance <= 0.0f) {
+		/* param not set, check is ok */
+		return true;
+	}
+
+	/* find first waypoint (with lat/lon) item in datamanager */
+	for (size_t i = 0; i < mission.count; i++) {
+
+		struct mission_item_s mission_item {};
+
+		if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
+			/* error reading, mission is invalid */
+			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.\t");
+			events::send(events::ID("navigator_mis_storage_failure"), events::Log::Error,
+				     "Error reading mission storage");
+			return false;
+		}
+
+		/* check only items with valid lat/lon */
+		if (!MissionBlock::item_contains_position(mission_item)) {
+			continue;
+		}
+
+		/* check distance from current position to item */
+		float dist_to_1wp = get_distance_to_next_waypoint(
+					    mission_item.lat, mission_item.lon,
+					    _navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
+
+		if (dist_to_1wp < max_distance) {
+
+			return true;
+
+		} else {
+			/* item is too far from home */
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+					     "First waypoint too far away: %dm, %d max\t",
+					     (int)dist_to_1wp, (int)max_distance);
+			events::send<uint32_t, uint32_t>(events::ID("navigator_mis_first_wp_too_far"), {events::Log::Error, events::LogInternal::Info},
+							 "First waypoint too far away: {1m} (maximum: {2m})", (uint32_t)dist_to_1wp, (uint32_t)max_distance);
+
+			_navigator->get_mission_result()->warning = true;
+			return false;
+		}
+	}
+
+	/* no waypoints found in mission, then we will not fly far away */
+	return true;
+}
+
+bool
+MissionFeasibilityChecker::checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance)
+{
+	if (max_distance <= 0.0f) {
+		/* param not set, check is ok */
+		return true;
+	}
+
+	double last_lat = (double)NAN;
+	double last_lon = (double)NAN;
+	int last_cmd = 0;
+
+	/* Go through all waypoints */
+	for (size_t i = 0; i < mission.count; i++) {
+
+		struct mission_item_s mission_item {};
+
+		if (!(dm_read((dm_item_t)mission.dataman_id, i, &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s))) {
+			/* error reading, mission is invalid */
+			mavlink_log_info(_navigator->get_mavlink_log_pub(), "Error reading offboard mission.\t");
+			events::send(events::ID("navigator_mis_storage_failure2"), events::Log::Error,
+				     "Error reading mission storage");
+			return false;
+		}
+
+		/* check only items with valid lat/lon */
+		if (!MissionBlock::item_contains_position(mission_item)) {
+			continue;
+		}
+
+		/* Compare it to last waypoint if already available. */
+		if (PX4_ISFINITE(last_lat) && PX4_ISFINITE(last_lon)) {
+
+			/* check distance from current position to item */
+			const float dist_between_waypoints = get_distance_to_next_waypoint(
+					mission_item.lat, mission_item.lon,
+					last_lat, last_lon);
+
+
+			if (dist_between_waypoints > max_distance) {
+				/* distance between waypoints is too high */
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+						     "Distance between waypoints too far: %d meters, %d max.\t",
+						     (int)dist_between_waypoints, (int)max_distance);
+				events::send<uint32_t, uint32_t>(events::ID("navigator_mis_wp_dist_too_far"), {events::Log::Error, events::LogInternal::Info},
+								 "Distance between waypoints too far: {1m}, (maximum: {2m})", (uint32_t)dist_between_waypoints, (uint32_t)max_distance);
+
+				_navigator->get_mission_result()->warning = true;
+				return false;
+
+				/* do not allow waypoints that are literally on top of each other */
+
+				/* and do not allow condition gates that are at the same position as a navigation waypoint */
+
+			} else if (dist_between_waypoints < 0.05f &&
+				   (mission_item.nav_cmd == NAV_CMD_CONDITION_GATE || last_cmd == NAV_CMD_CONDITION_GATE)) {
+
+				/* Waypoints and gate are at the exact same position, which indicates an
+				 * invalid mission and makes calculating the direction from one waypoint
+				 * to another impossible. */
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+						     "Distance between waypoint and gate too close: %d meters\t",
+						     (int)dist_between_waypoints);
+				events::send<float, float>(events::ID("navigator_mis_wp_gate_too_close"), {events::Log::Error, events::LogInternal::Info},
+							   "Distance between waypoint and gate too close: {1:.3m} (minimum: {2:.3m})", dist_between_waypoints, 0.05f);
+
+				_navigator->get_mission_result()->warning = true;
+				return false;
+			}
+		}
+
+		last_lat = mission_item.lat;
+		last_lon = mission_item.lon;
+		last_cmd = mission_item.nav_cmd;
+	}
+
+	/* We ran through all waypoints and have not found any distances between waypoints that are too far. */
+	return true;
+}

src/modules/navigator/mission_feasibility_checker.h

@@ -45,7 +45,6 @@
 #include <dataman/dataman.h>
 #include <uORB/topics/mission.h>
 #include <px4_platform_common/module_params.h>
-#include "MissionFeasibility/FeasibilityChecker.hpp"
 
 class Geofence;
 class Navigator;
@@ -54,18 +53,106 @@ class MissionFeasibilityChecker: public ModuleParams
 {
 private:
 	Navigator *_navigator{nullptr};
-	FeasibilityChecker _feasibility_checker;
 
+	/**
+	 * @brief Check geofence validity (if available)
+	 *
+	 * Check includes:
+	 * - home position is valid if required
+	 * - home position is valid if geofence is set
+	 * - all mission items don't violate set geofence
+	 *
+	 * @param mission Mission struct
+	 * @param home_alt Home altitude [m AMSL]
+	 * @param home_valid Home valid
+	 * @return True if checks passed, False if not
+	 */
 	bool checkGeofence(const mission_s &mission, float home_alt, bool home_valid);
 
-public:
-	MissionFeasibilityChecker(Navigator *navigator) :
-		ModuleParams(nullptr),
-		_navigator(navigator),
-		_feasibility_checker()
-	{
+	/**
+	 * @brief Check if Home altitude is valid if waypoints have relative altitude, and that waypoints are above it
+	 *
+	 * @param mission Mission struct
+	 * @param home_alt Home altitude [m AMSL]
+	 * @param home_alt_valid Home altitude valid
+	 * @return True if checks passed, False if not
+	 */
+	bool checkHomePositionAltitude(const mission_s &mission, float home_alt, bool home_alt_valid);
 
-	}
+	/**
+	 * @brief Check if all mission items are supported
+	 *
+	 * @param mission Mission struct
+	 * @return True if all set mission items are supported, False if not
+	 */
+	bool checkMissionItemValidity(const mission_s &mission);
+
+	/**
+	 * @brief Check if distance to first waypoint is below threshold
+	 *
+	 * @param mission Mission struct
+	 * @param max_distance Maximally allowed distance to first waypoint [m]
+	 * @return True if distance is below threshold, False if not
+	 */
+	bool checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance);
+
+	/**
+	 * @brief Check if distance between consecutive waypoints is below threshold
+	 *
+	 * @param mission Mission struct
+	 * @param max_distance Maximally allowed distance between consecutive waypoints [m]
+	 * @return True if all distances between waypoints are below threshold, False if not
+	 */
+	bool checkDistancesBetweenWaypoints(const mission_s &mission, float max_distance);
+
+	/**
+	 * @brief Check if mission contains takeoff (safe in _has_takeoff), and if that takeoff is valid
+	 *
+	 * @param mission Mission struct
+	 * @param home_alt Home altitude [m AMSL]
+	 * @return True if the planned takeoff is valid, or no takeoff planned, False otherwise
+	 */
+	bool checkTakeoff(const mission_s &mission, float home_alt);
+
+	/**
+	 * @brief Check if requirement for availability of planned takeoff and/or landing is fullfilled
+	 *
+	 * The requirement is controlled through parameter MIS_TKO_LAND_REQ.
+	 * Only checks for availability of takeoff/landing, not their validity
+	 *
+	 * @return True if check passes, False if not
+	 */
+	bool checkTakeoffLandAvailable();
+
+	/**
+	 * @brief Check if mission contains landing
+	 *
+	 * @param mission Mission struct
+	 * @return True if mission contains a landing, False if otherwise
+	 */
+	bool hasMissionLanding(const mission_s &mission);
+
+	/**
+	 * @brief Fixed-wing vehicle: Check if mission contains landing (safe in _has_landing), and if that landing is valid
+	 *
+	 * @param mission Mission struct
+	 * @return True if the planned landing is valid, or no landing planned, False otherwise
+	 */
+	bool checkFixedWingLanding(const mission_s &mission);
+
+	/**
+	 * @brief VTOL vehicle: Check if mission contains landing (safe in _has_landing), and if that landing is valid
+	 *
+	 * @param mission Mission struct
+	 * @return True if the planned landing is valid, or no landing planned, False otherwise
+	 */
+	bool checkVTOLLanding(const mission_s &mission);
+
+	bool _has_takeoff{false};
+	bool _has_landing{false};
+
+public:
+	MissionFeasibilityChecker(Navigator *navigator) : ModuleParams(nullptr), _navigator(navigator) {}
 	~MissionFeasibilityChecker() = default;
 
 	MissionFeasibilityChecker(const MissionFeasibilityChecker &) = delete;
@@ -74,5 +161,6 @@ public:
 	/*
 	 * Returns true if mission is feasible and false otherwise
 	 */
-	bool checkMissionFeasible(const mission_s &mission);
+	bool checkMissionFeasible(const mission_s &mission,
+				  float max_distance_to_1st_waypoint, float max_distance_between_waypoints);
 };