Navigator simplify mission

src/modules/navigator/datalinkloss.cpp

@@ -101,7 +101,7 @@ DataLinkLoss::set_dll_item()
 {
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-	set_previous_pos_setpoint();
+	pos_sp_triplet->previous = pos_sp_triplet->current;
 	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_dll_state) {

src/modules/navigator/enginefailure.cpp

@@ -86,7 +86,7 @@ EngineFailure::set_ef_item()
 {
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-	set_previous_pos_setpoint();
+	pos_sp_triplet->previous = pos_sp_triplet->current;
 	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_ef_state) {

src/modules/navigator/mission.cpp

@@ -61,13 +61,10 @@
 Mission::Mission(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_param_onboard_enabled(this, "MIS_ONBOARD_EN", false),
-	_param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false),
 	_param_dist_1wp(this, "MIS_DIST_1WP", false),
 	_param_dist_between_wps(this, "MIS_DIST_WPS", false),
 	_param_altmode(this, "MIS_ALTMODE", false),
-	_param_yawmode(this, "MIS_YAWMODE", false),
-	_param_fw_climbout_diff(this, "FW_CLMBOUT_DIFF", false),
-	_missionFeasibilityChecker(navigator)
+	_param_yawmode(this, "MIS_YAWMODE", false)
 {
 }
 
@@ -487,7 +484,7 @@ void
 Mission::set_mission_items()
 {
 	/* reset the altitude foh (first order hold) logic, if altitude foh is enabled (param) a new foh element starts now */
-	altitude_sp_foh_reset();
+	_min_current_sp_distance_xy = FLT_MAX;
 
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
@@ -544,7 +541,7 @@ Mission::set_mission_items()
 		_mission_type = MISSION_TYPE_NONE;
 
 		/* set loiter mission item and ensure that there is a minimum clearance from home */
-		set_loiter_item(&_mission_item, _param_takeoff_alt.get());
+		set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt());
 
 		/* update position setpoint triplet  */
 		pos_sp_triplet->previous.valid = false;
@@ -555,7 +552,9 @@ Mission::set_mission_items()
 		/* reuse setpoint for LOITER only if it's not IDLE */
 		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER);
 
-		set_mission_finished();
+		// set mission finished
+		_navigator->get_mission_result()->finished = true;
+		_navigator->set_mission_result_updated();
 
 		if (!user_feedback_done) {
 			/* only tell users that we got no mission if there has not been any
@@ -585,12 +584,11 @@ Mission::set_mission_items()
 
 		/* force vtol land */
 		if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) {
-
 			_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
 		}
 
 		/* we have a new position item so set previous position setpoint to current */
-		set_previous_pos_setpoint();
+		pos_sp_triplet->previous = pos_sp_triplet->current;
 
 		/* do takeoff before going to setpoint if needed and not already in takeoff */
 		/* in fixed-wing this whole block will be ignored and a takeoff item is always propagated */
@@ -601,7 +599,7 @@ Mission::set_mission_items()
 			new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
 
 			/* use current mission item as next position item */
-			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
+			mission_item_next_position = _mission_item;
 			mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT;
 			has_next_position_item = true;
 
@@ -707,7 +705,7 @@ Mission::set_mission_items()
 			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 
 			/* use current mission item as next position item */
-			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
+			mission_item_next_position = _mission_item;
 			has_next_position_item = true;
 
 			float altitude = _navigator->get_global_position()->alt;
@@ -747,7 +745,7 @@ Mission::set_mission_items()
 			new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 
 			/* use current mission item as next position item */
-			memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s));
+			mission_item_next_position = _mission_item;
 			has_next_position_item = true;
 
 			/*
@@ -813,7 +811,7 @@ Mission::set_mission_items()
 			new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 
 			/* set position setpoint to target during the transition */
-			set_previous_pos_setpoint();
+			pos_sp_triplet->previous = pos_sp_triplet->current;
 			generate_waypoint_from_heading(&pos_sp_triplet->current, pos_sp_triplet->current.yaw);
 		}
 
@@ -889,10 +887,8 @@ Mission::set_mission_items()
 	if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) {
 
 		_distance_current_previous = get_distance_to_next_waypoint(
-						     pos_sp_triplet->current.lat,
-						     pos_sp_triplet->current.lon,
-						     pos_sp_triplet->previous.lat,
-						     pos_sp_triplet->previous.lon);
+						     pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
+						     pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon);
 	}
 
 	_navigator->set_position_setpoint_triplet_updated();
@@ -1006,10 +1002,10 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
 
 	/* takeoff to at least MIS_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
 	if (_navigator->get_land_detected()->landed) {
-		takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
+		takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _navigator->get_takeoff_min_alt());
 
 	} else {
-		takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
+		takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _navigator->get_takeoff_min_alt());
 	}
 
 	return takeoff_alt;
@@ -1082,10 +1078,8 @@ Mission::heading_sp_update()
 		/* stop if positions are close together to prevent excessive yawing */
 		if (d_current > _navigator->get_acceptance_radius()) {
 			float yaw = get_bearing_to_next_waypoint(
-					    point_from_latlon[0],
-					    point_from_latlon[1],
-					    point_to_latlon[0],
-					    point_to_latlon[1]);
+					    point_from_latlon[0], point_from_latlon[1],
+					    point_to_latlon[0], point_to_latlon[1]);
 
 			/* always keep the back of the rotary wing pointing towards home */
 			if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
@@ -1172,12 +1166,6 @@ Mission::altitude_sp_foh_update()
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
-void
-Mission::altitude_sp_foh_reset()
-{
-	_min_current_sp_distance_xy = FLT_MAX;
-}
-
 void
 Mission::cruising_speed_sp_update()
 {
@@ -1209,7 +1197,7 @@ Mission::do_abort_landing()
 	// loiter at the larger of MIS_LTRMIN_ALT above the landing point
 	//  or 2 * FW_CLMBOUT_DIFF above the current altitude
 	float alt_landing = get_absolute_altitude_for_item(_mission_item);
-	float min_climbout = _navigator->get_global_position()->alt + (2 * _param_fw_climbout_diff.get());
+	float min_climbout = _navigator->get_global_position()->alt + 20.0f;
 	float alt_sp = math::max(alt_landing + _navigator->get_loiter_min_alt(), min_climbout);
 
 	// turn current landing waypoint into an indefinite loiter
@@ -1457,18 +1445,13 @@ Mission::set_current_offboard_mission_item()
 	save_offboard_mission_state();
 }
 
-void
-Mission::set_mission_finished()
-{
-	_navigator->get_mission_result()->finished = true;
-	_navigator->set_mission_result_updated();
-}
-
 void
 Mission::check_mission_valid(bool force)
 {
 	if ((!_home_inited && _navigator->home_position_valid()) || force) {
 
+		MissionFeasibilityChecker _missionFeasibilityChecker(_navigator);
+
 		_navigator->get_mission_result()->valid =
 			_missionFeasibilityChecker.checkMissionFeasible(_offboard_mission,
 					_param_dist_1wp.get(),
@@ -1554,12 +1537,9 @@ void
 Mission::generate_waypoint_from_heading(struct position_setpoint_s *setpoint, float yaw)
 {
 	waypoint_from_heading_and_distance(
-		_navigator->get_global_position()->lat,
-		_navigator->get_global_position()->lon,
-		yaw,
-		1000000.0f,
-		&(setpoint->lat),
-		&(setpoint->lon));
+		_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+		yaw, 1000000.0f,
+		&(setpoint->lat), &(setpoint->lon));
 	setpoint->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 	setpoint->yaw = yaw;
 }

src/modules/navigator/mission.h

@@ -157,11 +157,6 @@ private:
 	 */
 	void altitude_sp_foh_update();
 
-	/**
-	 * Resets the altitude sp foh logic
-	 */
-	void altitude_sp_foh_reset();
-
 	/**
 	 * Update the cruising speed setpoint.
 	 */
@@ -211,11 +206,6 @@ private:
 	 */
 	void set_current_offboard_mission_item();
 
-	/**
-	 * Set that the mission is finished if one exists or that none exists
-	 */
-	void set_mission_finished();
-
 	/**
 	 * Check whether a mission is ready to go
 	 */
@@ -243,12 +233,10 @@ private:
 	bool find_offboard_land_start();
 
 	control::BlockParamInt _param_onboard_enabled;
-	control::BlockParamFloat _param_takeoff_alt;
 	control::BlockParamFloat _param_dist_1wp;
 	control::BlockParamFloat _param_dist_between_wps;
 	control::BlockParamInt _param_altmode;
 	control::BlockParamInt _param_yawmode;
-	control::BlockParamFloat _param_fw_climbout_diff;
 
 	struct mission_s _onboard_mission {};
 	struct mission_s _offboard_mission {};
@@ -272,8 +260,6 @@ private:
 	bool _home_inited{false};
 	bool _need_mission_reset{false};
 
-	MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */
-
 	float _min_current_sp_distance_xy{FLT_MAX}; /**< minimum distance which was achieved to the current waypoint  */
 
 	float _distance_current_previous{0.0f}; /**< distance from previous to current sp in pos_sp_triplet,

src/modules/navigator/mission_block.cpp

@@ -557,16 +557,6 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	return sp->valid;
 }
 
-void
-MissionBlock::set_previous_pos_setpoint()
-{
-	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	if (pos_sp_triplet->current.valid) {
-		memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
-	}
-}
-
 void
 MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 {

src/modules/navigator/mission_block.h

@@ -90,11 +90,6 @@ protected:
 	 */
 	bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp);
 
-	/**
-	 * Set previous position setpoint to current setpoint
-	 */
-	void set_previous_pos_setpoint();
-
 	/**
 	 * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
 	 */

src/modules/navigator/mission_feasibility_checker.cpp

@@ -55,59 +55,46 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
 		float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
 		bool land_start_req)
 {
-	dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
+	const dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(mission.dataman_id);
 	const size_t nMissionItems = mission.count;
 
-	const bool isRotarywing = (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol);
-
-	Geofence &geofence = _navigator->get_geofence();
-	fw_pos_ctrl_status_s *fw_pos_ctrl_status = _navigator->get_fw_pos_ctrl_status();
-	const float home_alt = _navigator->get_home_position()->alt;
-	const bool home_valid = _navigator->home_position_valid();
-	const bool home_alt_valid = _navigator->home_alt_valid();
-
-	bool &warning_issued = _navigator->get_mission_result()->warning;
-	const float default_acceptance_rad  = _navigator->get_default_acceptance_radius();
-	const float default_altitude_acceptance_rad  = _navigator->get_altitude_acceptance_radius();
-	const bool landed = _navigator->get_land_detected()->landed;
-
 	bool failed = false;
 	bool warned = 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;
 		warned = true;
 		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Not yet ready for mission, no position lock.");
 
 	} else {
-		failed = failed ||
-			 !checkDistanceToFirstWaypoint(dm_current, nMissionItems, max_distance_to_1st_waypoint, warning_issued);
+		failed = failed || !checkDistanceToFirstWaypoint(dm_current, nMissionItems, 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(dm_current, nMissionItems, landed);
-	failed = failed
-		 || !checkDistancesBetweenWaypoints(dm_current, nMissionItems, max_distance_between_waypoints, warning_issued);
-	failed = failed || !checkGeofence(dm_current, nMissionItems, geofence, home_alt, home_valid);
+	failed = failed || !checkMissionItemValidity(dm_current, nMissionItems);
+	failed = failed || !checkDistancesBetweenWaypoints(dm_current, nMissionItems, max_distance_between_waypoints);
+	failed = failed || !checkGeofence(dm_current, nMissionItems, home_alt, home_valid);
 	failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_alt_valid, warned);
 
-	if (isRotarywing) {
-		failed = failed
-			 || !checkRotarywing(dm_current, nMissionItems, home_alt, home_alt_valid, default_altitude_acceptance_rad);
+	// VTOL always respects rotary wing feasibility
+	if (_navigator->get_vstatus()->is_rotary_wing || _navigator->get_vstatus()->is_vtol) {
+		failed = failed || !checkRotarywing(dm_current, nMissionItems, home_alt, home_alt_valid);
 
 	} else {
-		failed = failed
-			 || !checkFixedwing(dm_current, nMissionItems, fw_pos_ctrl_status, home_alt, home_alt_valid,
-					    default_acceptance_rad, land_start_req);
+		failed = failed || !checkFixedwing(dm_current, nMissionItems, home_alt, home_alt_valid, land_start_req);
 	}
 
 	return !failed;
 }
 
 bool
-MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMissionItems,
-		float home_alt, bool home_alt_valid, float default_altitude_acceptance_rad)
+MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid)
 {
 	for (size_t i = 0; i < nMissionItems; i++) {
 		struct mission_item_s missionitem = {};
@@ -122,10 +109,10 @@ MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMission
 		if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) {
 			// make sure that the altitude of the waypoint is at least one meter larger than the altitude 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;
+			const float takeoff_alt = missionitem.altitude_is_relative ? missionitem.altitude : missionitem.altitude - home_alt;
 
 			// check if we should use default acceptance radius
-			float acceptance_radius = default_altitude_acceptance_rad;
+			float acceptance_radius = _navigator->get_altitude_acceptance_radius();
 
 			// if a specific acceptance radius has been defined, use that one instead
 			if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) {
@@ -144,30 +131,27 @@ MissionFeasibilityChecker::checkRotarywing(dm_item_t dm_current, size_t nMission
 }
 
 bool
-MissionFeasibilityChecker::checkFixedwing(dm_item_t dm_current, size_t nMissionItems,
-		fw_pos_ctrl_status_s *fw_pos_ctrl_status, float home_alt, bool home_alt_valid,
-		float default_acceptance_rad, bool land_start_req)
+MissionFeasibilityChecker::checkFixedwing(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid,
+		bool land_start_req)
 {
 	/* Perform checks and issue feedback to the user for all checks */
-	bool resTakeoff = checkFixedWingTakeoff(dm_current, nMissionItems, home_alt, home_alt_valid, land_start_req);
-	bool resLanding = checkFixedWingLanding(dm_current, nMissionItems, fw_pos_ctrl_status, land_start_req);
+	bool resTakeoff = checkFixedWingTakeoff(dm_current, nMissionItems, home_alt, home_valid);
+	bool resLanding = checkFixedWingLanding(dm_current, nMissionItems, land_start_req);
 
 	/* Mission is only marked as feasible if all checks return true */
 	return (resTakeoff && resLanding);
 }
 
 bool
-MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt,
-		bool home_valid)
+MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid)
 {
-
-	if (geofence.isHomeRequired() && !home_valid) {
+	if (_navigator->get_geofence().isHomeRequired() && !home_valid) {
 		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence requires valid home position");
 		return false;
 	}
 
 	/* Check if all mission items are inside the geofence (if we have a valid geofence) */
-	if (geofence.valid()) {
+	if (_navigator->get_geofence().valid()) {
 		for (size_t i = 0; i < nMissionItems; i++) {
 			struct mission_item_s missionitem = {};
 			const ssize_t len = sizeof(missionitem);
@@ -185,8 +169,7 @@ MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionIt
 			// 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) &&
-			    !geofence.check(missionitem)) {
+			if (MissionBlock::item_contains_position(missionitem) && !_navigator->get_geofence().check(missionitem)) {
 
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %d", i + 1);
 				return false;
@@ -198,8 +181,8 @@ MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionIt
 }
 
 bool
-MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems,
-		float home_alt, bool home_alt_valid, bool &warning_issued, bool throw_error)
+MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt,
+		bool home_alt_valid, bool throw_error)
 {
 	/* Check if all waypoints are above the home altitude */
 	for (size_t i = 0; i < nMissionItems; i++) {
@@ -207,15 +190,15 @@ MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_
 		const ssize_t len = sizeof(struct mission_item_s);
 
 		if (dm_read(dm_current, i, &missionitem, len) != len) {
-			warning_issued = true;
+			_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)) {
+		if (missionitem.altitude_is_relative && !home_valid && MissionBlock::item_contains_position(missionitem)) {
 
-			warning_issued = true;
+			_navigator->get_mission_result()->warning = true;
 
 			if (throw_error) {
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: No home pos, WP %d uses rel alt", i + 1);
@@ -232,7 +215,7 @@ MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_
 
 		if ((home_alt > wp_alt) && MissionBlock::item_contains_position(missionitem)) {
 
-			warning_issued = true;
+			_navigator->get_mission_result()->warning = true;
 
 			if (throw_error) {
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Waypoint %d below home", i + 1);
@@ -249,7 +232,7 @@ MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_
 }
 
 bool
-MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool landed)
+MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems)
 {
 	// do not allow mission if we find unsupported item
 	for (size_t i = 0; i < nMissionItems; i++) {
@@ -317,7 +300,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t
 		}
 
 		// check if the mission starts with a land command while the vehicle is landed
-		if (missionitem.nav_cmd == NAV_CMD_LAND && i == 0 && 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");
 			return false;
@@ -328,8 +311,8 @@ MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t
 }
 
 bool
-MissionFeasibilityChecker::checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems,
-		float home_alt, bool home_alt_valid, float default_acceptance_rad)
+MissionFeasibilityChecker::checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems, float home_alt,
+		bool home_alt_valid)
 {
 	for (size_t i = 0; i < nMissionItems; i++) {
 		struct mission_item_s missionitem = {};
@@ -350,7 +333,7 @@ MissionFeasibilityChecker::checkFixedWingTakeoff(dm_item_t dm_current, size_t nM
 					    : missionitem.altitude - home_alt;
 
 			// check if we should use default acceptance radius
-			float acceptance_radius = default_acceptance_rad;
+			float acceptance_radius = _navigator->get_default_acceptance_radius();
 
 			if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) {
 				acceptance_radius = missionitem.acceptance_radius;
@@ -368,8 +351,7 @@ MissionFeasibilityChecker::checkFixedWingTakeoff(dm_item_t dm_current, size_t nM
 }
 
 bool
-MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems,
-		fw_pos_ctrl_status_s *fw_pos_ctrl_status, bool land_start_req)
+MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems, 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 */
@@ -413,27 +395,33 @@ MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nM
 				}
 
 				if (MissionBlock::item_contains_position(missionitem_previous)) {
-					float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon, missionitem.lat,
-							    missionitem.lon);
 
-					float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
-							      fw_pos_ctrl_status->landing_horizontal_slope_displacement, fw_pos_ctrl_status->landing_slope_angle_rad);
+					const bool fw_status_valid = (_navigator->get_fw_pos_ctrl_status()->timestamp > 0);
+					const float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon,
+								  missionitem.lat, missionitem.lon);
 
-					float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude, missionitem.altitude,
-								fw_pos_ctrl_status->landing_horizontal_slope_displacement, fw_pos_ctrl_status->landing_slope_angle_rad);
-
-					if (wp_distance > fw_pos_ctrl_status->landing_flare_length) {
+					if (fw_status_valid && (wp_distance > _navigator->get_fw_pos_ctrl_status()->landing_flare_length)) {
 						/* Last wp is before flare region */
 
 						const float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
 
 						if (delta_altitude < 0) {
+
+							const float horizontal_slope_displacement = _navigator->get_fw_pos_ctrl_status()->landing_horizontal_slope_displacement;
+							const float slope_angle_rad = _navigator->get_fw_pos_ctrl_status()->landing_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) {
 								/* Landing waypoint is above altitude of slope at the given waypoint distance */
 								mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach.");
-								mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %.1fm or move further away by %.1fm.",
-										     (double)(slope_alt_req - missionitem_previous.altitude),
-										     (double)(wp_distance_req - wp_distance));
+
+								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(), "Move down %d m or move further away by %d m.",
+										     (int)ceilf(slope_alt_req - missionitem_previous.altitude),
+										     (int)ceilf(wp_distance_req - wp_distance));
 
 								return false;
 							}
@@ -480,8 +468,7 @@ MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nM
 }
 
 bool
-MissionFeasibilityChecker::checkDistanceToFirstWaypoint(dm_item_t dm_current, size_t nMissionItems,
-		float max_distance, bool &warning_issued)
+MissionFeasibilityChecker::checkDistanceToFirstWaypoint(dm_item_t dm_current, size_t nMissionItems, float max_distance)
 {
 	if (max_distance <= 0.0f) {
 		/* param not set, check is ok */
@@ -515,7 +502,8 @@ MissionFeasibilityChecker::checkDistanceToFirstWaypoint(dm_item_t dm_current, si
 				/* allow at 2/3 distance, but warn */
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
 						     "First waypoint far away: %d meters.", (int)dist_to_1wp);
-				warning_issued = true;
+
+				_navigator->get_mission_result()->warning = true;
 			}
 
 			return true;
@@ -525,7 +513,8 @@ MissionFeasibilityChecker::checkDistanceToFirstWaypoint(dm_item_t dm_current, si
 			mavlink_log_critical(_navigator->get_mavlink_log_pub(),
 					     "First waypoint too far away: %d meters, %d max.",
 					     (int)dist_to_1wp, (int)max_distance);
-			warning_issued = true;
+
+			_navigator->get_mission_result()->warning = true;
 			return false;
 		}
 	}
@@ -536,7 +525,7 @@ MissionFeasibilityChecker::checkDistanceToFirstWaypoint(dm_item_t dm_current, si
 
 bool
 MissionFeasibilityChecker::checkDistancesBetweenWaypoints(dm_item_t dm_current, size_t nMissionItems,
-		float max_distance, bool &warning_issued)
+		float max_distance)
 {
 	if (max_distance <= 0.0f) {
 		/* param not set, check is ok */
@@ -566,18 +555,18 @@ MissionFeasibilityChecker::checkDistancesBetweenWaypoints(dm_item_t dm_current,
 		if (PX4_ISFINITE(last_lat) && PX4_ISFINITE(last_lon)) {
 
 			/* check distance from current position to item */
-			float dist_between_waypoints = get_distance_to_next_waypoint(
-							       mission_item.lat, mission_item.lon,
-							       last_lat, last_lon);
+			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) {
 
 				if (dist_between_waypoints > ((max_distance * 3) / 2)) {
 					/* allow at 2/3 distance, but warn */
 					mavlink_log_critical(_navigator->get_mavlink_log_pub(),
-							     "Distance between waypoints very far: %d meters.",
-							     (int)dist_between_waypoints);
-					warning_issued = true;
+							     "Distance between waypoints very far: %d meters.", (int)dist_between_waypoints);
+
+					_navigator->get_mission_result()->warning = true;
 				}
 
 			} else {
@@ -585,7 +574,8 @@ MissionFeasibilityChecker::checkDistancesBetweenWaypoints(dm_item_t dm_current,
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
 						     "Distance between waypoints too far: %d meters, %d max.",
 						     (int)dist_between_waypoints, (int)max_distance);
-				warning_issued = true;
+
+				_navigator->get_mission_result()->warning = true;
 				return false;
 			}
 		}

src/modules/navigator/mission_feasibility_checker.h

@@ -55,29 +55,24 @@ private:
 	Navigator *_navigator{nullptr};
 
 	/* Checks for all airframes */
-	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
+	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid);
 
 	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid,
-				       bool &warning_issued, bool throw_error = false);
+				       bool throw_error);
 
-	bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed);
+	bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
 
-	bool checkDistanceToFirstWaypoint(dm_item_t dm_current, size_t nMissionItems, float max_distance, bool &warning_issued);
-	bool checkDistancesBetweenWaypoints(dm_item_t dm_current, size_t nMissionItems, float max_distance,
-					    bool &warning_issued);
+	bool checkDistanceToFirstWaypoint(dm_item_t dm_current, size_t nMissionItems, float max_distance);
+	bool checkDistancesBetweenWaypoints(dm_item_t dm_current, size_t nMissionItems, float max_distance);
 
 	/* Checks specific to fixedwing airframes */
-	bool checkFixedwing(dm_item_t dm_current, size_t nMissionItems, fw_pos_ctrl_status_s *fw_pos_ctrl_status,
-			    float home_alt, bool home_alt_valid, float default_acceptance_rad, bool land_start_req);
+	bool checkFixedwing(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid, bool land_start_req);
 
-	bool checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid,
-				   float default_acceptance_rad);
-	bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems, fw_pos_ctrl_status_s *fw_pos_ctrl_status,
-				   bool land_start_req);
+	bool checkFixedWingTakeoff(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid);
+	bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems, bool land_start_req);
 
 	/* Checks specific to rotarywing airframes */
-	bool checkRotarywing(dm_item_t dm_current, size_t nMissionItems,
-			     float home_alt, bool home_alt_valid, float default_altitude_acceptance_rad);
+	bool checkRotarywing(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_alt_valid);
 
 public:
 	MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {}

src/modules/navigator/rcloss.cpp

@@ -91,7 +91,7 @@ RCLoss::set_rcl_item()
 {
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-	set_previous_pos_setpoint();
+	pos_sp_triplet->previous = pos_sp_triplet->current;
 	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rcl_state) {