text: /****************************************************************************
   *
   *   Copyright (c) 2013-2019 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.
   *
   ****************************************************************************/
  /**
   * @file mission_feasibility_checker.cpp
   * Provides checks if mission is feasible given the navigation capabilities
   *
   * @author Lorenz Meier <lm@inf.ethz.ch>
   * @author Thomas Gubler <thomasgubler@student.ethz.ch>
   * @author Sander Smeets <sander@droneslab.com>
   * @author Nuno Marques <nuno.marques@dronesolutions.io>
   */
  
  #include "mission_feasibility_checker.h"
  
  #include "mission_block.h"
  #include "navigator.h"
  
  #include <drivers/drv_pwm_output.h>
  #include <lib/geo/geo.h>
  #include <lib/mathlib/mathlib.h>
  #include <lib/landing_slope/Landingslope.hpp>
  #include <systemlib/mavlink_log.h>
  #include <uORB/Subscription.hpp>
  #include <uORB/topics/position_controller_landing_status.h>
  #include <px4_platform_common/events.h>
  
  bool
  MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
  		float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
  		bool land_start_req)
  {
  	// Reset warning flag
  	_navigator->get_mission_result()->warning = false;
  
  	// trivial case: A mission with length zero cannot be valid
  	if ((int)mission.count <= 0) {
  		return false;
  	}
  
  	bool failed = false;
  
  	// first check if we have a valid position
  	const bool home_valid = _navigator->home_position_valid();
  	const bool home_alt_valid = _navigator->home_alt_valid();
  
  	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 = failed || !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint);
  	}
  
  	const float home_alt = _navigator->get_home_position()->alt;
  
  	// check if all mission item commands are supported
  	failed = failed || !checkMissionItemValidity(mission);
  	failed = failed || !checkDistancesBetweenWaypoints(mission, max_distance_between_waypoints);
  	failed = failed || !checkGeofence(mission, home_alt, home_valid);
  	failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid);
  
  	if (_navigator->get_vstatus()->is_vtol) {
  		failed = failed || !checkVTOL(mission, home_alt, false);
  
  	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
  		failed = failed || !checkRotarywing(mission, home_alt);
  
  	} else {
  		failed = failed || !checkFixedwing(mission, home_alt, land_start_req);
  	}
  
  	return !failed;
  }
  
  bool
  MissionFeasibilityChecker::checkRotarywing(const mission_s &mission, float home_alt)
  {
  	/*
  	 * Perform check and issue feedback to the user
  	 * Mission is only marked as feasible if takeoff check passes
  	 */
  	return checkTakeoff(mission, home_alt);
  }
  
  bool
  MissionFeasibilityChecker::checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req)
  {
  	/* Perform checks and issue feedback to the user for all checks */
  	bool resTakeoff = checkTakeoff(mission, home_alt);
  	bool resLanding = checkFixedWingLanding(mission, land_start_req);
  
  	/* Mission is only marked as feasible if all checks return true */
  	return (resTakeoff && resLanding);
  }
  
  bool
  MissionFeasibilityChecker::checkVTOL(const mission_s &mission, float home_alt, bool land_start_req)
  {
  	/* Perform checks and issue feedback to the user for all checks */
  	bool resTakeoff = checkTakeoff(mission, home_alt);
  	bool resLanding = checkVTOLLanding(mission, land_start_req);
  
  	/* Mission is only marked as feasible if all checks return true */
  	return (resTakeoff && resLanding);
  }
  
  bool
  MissionFeasibilityChecker::checkGeofence(const mission_s &mission, float home_alt, bool home_valid)
  {
  	if (_navigator->get_geofence().isHomeRequired() && !home_valid) {
  		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position\t");
  		events::send(events::ID("navigator_mis_geofence_no_home"), {events::Log::Error, events::LogInternal::Info},
  			     "Geofence requires a valid home position");
  		return false;
  	}
  
  	/* Check if all mission items are inside the geofence (if we have a valid geofence) */
  	if (_navigator->get_geofence().valid()) {
  		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.altitude_is_relative && !home_valid) {
  				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position\t");
  				events::send(events::ID("navigator_mis_geofence_no_home2"), {events::Log::Error, events::LogInternal::Info},
  					     "Geofence requires a valid home position");
  				return false;
  			}
  
  			// Geofence function checks against home altitude amsl
  			missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude;
  
  			if (MissionBlock::item_contains_position(missionitem) && !_navigator->get_geofence().check(missionitem)) {
  
  				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %zu\t", i + 1);
  				events::send<int16_t>(events::ID("navigator_mis_geofence_violation"), {events::Log::Error, events::LogInternal::Info},
  						      "Geofence violation for waypoint {1}",
  						      i + 1);
  				return false;
  			}
  		}
  	}
  
  	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_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) {
  
  			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 has_takeoff = false;
  	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) {
  			// 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 (_navigator->get_takeoff_required() && _navigator->get_land_detected()->landed) {
  		// check for a takeoff waypoint, after the above conditions have been met
  		// MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission
  		// while the vehicle is flying and it does not require a takeoff waypoint
  		if (!has_takeoff) {
  			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;
  
  		} else if (!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::checkFixedWingLanding(const mission_s &mission, bool land_start_req)
  {
  	/* Go through all mission items and search for a landing waypoint
  	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
  
  	bool landing_valid = false;
  
  	bool land_start_found = false;
  	size_t do_land_start_index = 0;
  	size_t landing_approach_index = 0;
  
  	for (size_t i = 0; i < 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 (land_start_found) {
  				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 {
  				land_start_found = true;
  				do_land_start_index = i;
  			}
  		}
  
  		if (missionitem.nav_cmd == NAV_CMD_LAND) {
  			mission_item_s missionitem_previous {};
  
  			if (i > 0) {
  				landing_approach_index = i - 1;
  
  				if (dm_read((dm_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)) {
  
  					uORB::SubscriptionData<position_controller_landing_status_s> landing_status{ORB_ID(position_controller_landing_status)};
  
  					const bool landing_status_valid = (landing_status.get().timestamp > 0);
  					const float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon,
  								  missionitem.lat, missionitem.lon);
  
  					if (landing_status_valid && (wp_distance > landing_status.get().flare_length)) {
  						/* Last wp is before flare region */
  
  						const float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
  
  						if (delta_altitude < 0) {
  
  							const float horizontal_slope_displacement = landing_status.get().horizontal_slope_displacement;
  							const float slope_angle_rad = landing_status.get().slope_angle_rad;
  							const float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
  										    horizontal_slope_displacement, slope_angle_rad);
  
  							if (missionitem_previous.altitude > slope_alt_req + 1.0f) {
  								/* Landing waypoint is above altitude of slope at the given waypoint distance (with small tolerance for floating point discrepancies) */
  								const float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude,
  											      missionitem.altitude, horizontal_slope_displacement, slope_angle_rad);
  
  								mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach.\t");
  								mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %d m or move further away by %d m.\t",
  										     (int)ceilf(slope_alt_req - missionitem_previous.altitude),
  										     (int)ceilf(wp_distance_req - wp_distance));
  								/* EVENT
  								 * @description
  								 * The landing waypoint must be above the altitude of slope at the given waypoint distance.
  								 * Move it down {1m_v} or move it further away by {2m}.
  								 */
  								events::send<int32_t, int32_t>(events::ID("navigator_mis_land_approach"), {events::Log::Error, events::LogInternal::Info},
  											       "Mission rejected: adjust landing approach",
  											       (int)ceilf(slope_alt_req - missionitem_previous.altitude),
  											       (int)ceilf(wp_distance_req - wp_distance));
  
  								return false;
  							}
  
  						} else {
  							/* Landing waypoint is above last waypoint */
  							mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint.\t");
  							events::send(events::ID("navigator_mis_land_too_high"), {events::Log::Error, events::LogInternal::Info},
  								     "Mission rejected: landing waypoint is above the last waypoint");
  							return false;
  						}
  
  					} else {
  						/* Last wp is in flare region */
  						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare.\t");
  						events::send(events::ID("navigator_mis_land_within_flare"), {events::Log::Error, events::LogInternal::Info},
  							     "Mission rejected: waypoint is within landing flare");
  						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 (land_start_found && 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 (land_start_req && !land_start_found) {
  		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.\t");
  		events::send(events::ID("navigator_mis_land_missing"), {events::Log::Error, events::LogInternal::Info},
  			     "Mission rejected: landing pattern required");
  		return false;
  	}
  
  	if (land_start_found && (!landing_valid || (do_land_start_index > landing_approach_index))) {
  		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.\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, bool land_start_req)
  {
  	/* Go through all mission items and search for a landing waypoint
  	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
  
  	bool land_start_found = 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 (land_start_found) {
  				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 {
  				land_start_found = 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 {};
  
  			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 (land_start_found && 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 (land_start_req && !land_start_found) {
  		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.\t");
  		events::send(events::ID("navigator_mis_land_missing2"), {events::Log::Error, events::LogInternal::Info},
  			     "Mission rejected: landing pattern required");
  		return false;
  	}
  
  	if (land_start_found && (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::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;
  }
  