rtl+mission: remove do_need_move_to_land and handleLanding duplicated code to reduce flash

src/modules/navigator/mission.cpp

@@ -155,21 +155,6 @@ bool Mission::setNextMissionItem()
 	return (goToNextItem(true) == PX4_OK);
 }
 
-bool
-Mission::do_need_move_to_land()
-{
-	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
-	    && (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND)) {
-
-		float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
-				  _global_pos_sub.get().lat, _global_pos_sub.get().lon);
-
-		return d_current > _navigator->get_acceptance_radius();
-	}
-
-	return false;
-}
-
 bool
 Mission::do_need_move_to_takeoff()
 {
@@ -188,7 +173,7 @@ Mission::do_need_move_to_takeoff()
 void Mission::setActiveMissionItems()
 {
 	/* Get mission item that comes after current if available */
-	constexpr static size_t max_num_next_items{2u};
+	static constexpr size_t max_num_next_items{2u};
 	int32_t next_mission_items_index[max_num_next_items];
 	size_t num_found_items;
 
@@ -218,10 +203,6 @@ void Mission::setActiveMissionItems()
 	const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current;
 
 	if (item_contains_position(_mission_item)) {
-		/* force vtol land */
-		if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) {
-			_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
-		}
 
 		handleTakeoff(new_work_item_type, next_mission_items, num_found_items);
 
@@ -425,132 +406,6 @@ void Mission::handleTakeoff(WorkItemType &new_work_item_type, mission_item_s nex
 	}
 }
 
-void Mission::handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[],
-			    size_t &num_found_items)
-{
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	/* move to land wp as fixed wing */
-	if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-	    && (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT
-		|| _work_item_type == WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF)
-	    && !_land_detected_sub.get().landed) {
-
-		new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
-
-		/* use current mission item as next position item */
-		num_found_items = 1u;
-		next_mission_items[0u] = _mission_item;
-
-		float altitude = _global_pos_sub.get().alt;
-
-		if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
-			altitude = pos_sp_triplet->current.alt;
-		}
-
-		_mission_item.altitude = altitude;
-		_mission_item.altitude_is_relative = false;
-		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.autocontinue = true;
-		_mission_item.time_inside = 0.0f;
-		_mission_item.vtol_back_transition = true;
-	}
-
-	/* transition to MC */
-	if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
-	    && _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND
-	    && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
-	    && !_land_detected_sub.get().landed) {
-
-		set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
-		_mission_item.altitude = _global_pos_sub.get().alt;
-		_mission_item.altitude_is_relative = false;
-
-		new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
-
-		// make previous setpoint invalid, such that there will be no prev-current line following
-		// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
-		pos_sp_triplet->previous.valid = false;
-	}
-
-	/* move to landing waypoint before descent if necessary */
-	if (do_need_move_to_land() &&
-	    (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT ||
-	     _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
-
-		new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
-
-		/* use current mission item as next position item */
-		num_found_items = 1u;
-		next_mission_items[0u] = _mission_item;
-
-		/*
-			* Ignoring waypoint altitude:
-			* Set altitude to the same as we have now to prevent descending too fast into
-			* the ground. Actual landing will descend anyway until it touches down.
-			* XXX: We might want to change that at some point if it is clear to the user
-			* what the altitude means on this waypoint type.
-			*/
-		float altitude = _global_pos_sub.get().alt;
-
-		if (pos_sp_triplet->current.valid
-		    && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
-			altitude = pos_sp_triplet->current.alt;
-		}
-
-		_mission_item.altitude = altitude;
-		_mission_item.altitude_is_relative = false;
-		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.autocontinue = true;
-
-		// have to reset here because these field were used in set_vtol_transition_item
-		_mission_item.time_inside = 0.f;
-		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-
-		// make previous setpoint invalid, such that there will be no prev-current line following.
-		// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
-		pos_sp_triplet->previous.valid = false;
-
-	} else if (_mission_item.nav_cmd == NAV_CMD_LAND && _work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
-		if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
-			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND;
-
-			if (_mission_item.land_precision == 1) {
-				_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
-
-			} else { //_mission_item.land_precision == 2
-				_navigator->get_precland()->set_mode(PrecLandMode::Required);
-			}
-
-			_navigator->get_precland()->on_activation();
-		}
-	}
-
-	/* we just moved to the landing waypoint, now descend */
-	if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) {
-
-		if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
-			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND;
-
-			if (_mission_item.land_precision == 1) {
-				_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
-
-			} else { //_mission_item.land_precision == 2
-				_navigator->get_precland()->set_mode(PrecLandMode::Required);
-			}
-
-			_navigator->get_precland()->on_activation();
-		}
-	}
-
-	/* ignore yaw for landing items */
-	/* XXX: if specified heading for landing is desired we could add another step before the descent
-		* that aligns the vehicle first */
-	if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
-		_mission_item.yaw = NAN;
-	}
-}
-
 void Mission::handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[],
 				   size_t &num_found_items)
 {

src/modules/navigator/mission.h

@@ -73,11 +73,6 @@ private:
 
 	bool setNextMissionItem() override;
 
-	/**
-	 * Returns true if we need to move to waypoint location before starting descent
-	 */
-	bool do_need_move_to_land();
-
 	/**
 	 * Returns true if we need to move to waypoint location after vtol takeoff
 	 */
@@ -97,8 +92,6 @@ private:
 
 	void handleTakeoff(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items);
 
-	void handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[], size_t &num_found_items);
-
 	void handleVtolTransition(WorkItemType &new_work_item_type, mission_item_s next_mission_items[],
 				  size_t &num_found_items);
 

src/modules/navigator/mission_base.cpp

@@ -446,6 +446,11 @@ MissionBase::set_mission_items()
 		/* By default set the mission item to the current planned mission item. Depending on request, it can be altered. */
 		loadCurrentMissionItem();
 
+		/* force vtol land */
+		if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) {
+			_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
+		}
+
 		setActiveMissionItems();
 
 	} else {
@@ -517,6 +522,124 @@ MissionBase::set_mission_result()
 	_navigator->set_mission_result_updated();
 }
 
+bool MissionBase::do_need_move_to_item()
+{
+	float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
+			  _global_pos_sub.get().lat, _global_pos_sub.get().lon);
+
+	return d_current > _navigator->get_acceptance_radius();
+}
+
+void MissionBase::handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[],
+				size_t &num_found_items)
+{
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	bool needs_to_land = !_land_detected_sub.get().landed &&
+			     ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND)
+			      || (_mission_item.nav_cmd == NAV_CMD_LAND));
+
+	bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol &&
+				  (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
+				  (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND) &&
+				  !_land_detected_sub.get().landed;
+
+	/* move to land wp as fixed wing */
+	if (needs_vtol_landing) {
+		if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
+
+			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
+
+			/* use current mission item as next position item */
+			num_found_items = 1u;
+			next_mission_items[0u] = _mission_item;
+
+			float altitude = _global_pos_sub.get().alt;
+
+			if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+				altitude = pos_sp_triplet->current.alt;
+			}
+
+			_mission_item.altitude = altitude;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item.autocontinue = true;
+			_mission_item.time_inside = 0.0f;
+			_mission_item.vtol_back_transition = true;
+
+			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
+		}
+
+		/* transition to MC */
+		if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) {
+
+			set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
+
+			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
+
+			// make previous setpoint invalid, such that there will be no prev-current line following
+			// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
+			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
+		}
+
+	} else if (needs_to_land) {
+		/* move to landing waypoint before descent if necessary */
+		if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) &&
+		    do_need_move_to_item() &&
+		    (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT ||
+		     _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
+
+			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
+
+			/* use current mission item as next position item */
+			num_found_items = 1u;
+			next_mission_items[0u] = _mission_item;
+
+			/*
+				* Ignoring waypoint altitude:
+				* Set altitude to the same as we have now to prevent descending too fast into
+				* the ground. Actual landing will descend anyway until it touches down.
+				* XXX: We might want to change that at some point if it is clear to the user
+				* what the altitude means on this waypoint type.
+				*/
+			float altitude = _global_pos_sub.get().alt;
+
+			if (pos_sp_triplet->current.valid
+			    && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
+				altitude = pos_sp_triplet->current.alt;
+			}
+
+			_mission_item.altitude = altitude;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			_mission_item.autocontinue = true;
+
+			// have to reset here because these field were used in set_vtol_transition_item
+			_mission_item.time_inside = 0.f;
+			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+
+			// make previous setpoint invalid, such that there will be no prev-current line following.
+			// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
+			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
+
+		} else {
+
+			if (_mission_item.land_precision > 0 && _mission_item.land_precision < 3) {
+				new_work_item_type = WorkItemType::WORK_ITEM_TYPE_PRECISION_LAND;
+
+				startPrecLand(_mission_item.land_precision);
+			}
+		}
+	}
+
+	/* ignore yaw for landing items */
+	/* XXX: if specified heading for landing is desired we could add another step before the descent
+		* that aligns the vehicle first */
+	if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
+		_mission_item.yaw = NAN;
+	}
+}
+
 bool MissionBase::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const
 {
 	return ((p1->valid == p2->valid) &&

src/modules/navigator/mission_base.h

@@ -279,6 +279,24 @@ protected:
 	 *
 	 */
 	void publish_navigator_mission_item();
+
+	/**
+	 * @brief Do need move to item
+	 *
+	 * @return true if the item is horizontally further away than the mission item
+	 * @return false otherwise
+	 */
+	bool do_need_move_to_item();
+
+	/**
+	 * @brief Handle landing
+	 *
+	 * @param new_work_item_type new work item type state machine to be set
+	 * @param next_mission_items the next mission items after the current mission item
+	 * @param num_found_items number of found next mission items
+	 */
+	void handleLanding(WorkItemType &new_work_item_type, mission_item_s next_mission_items[],
+			   size_t &num_found_items);
 	/**
 	 * @brief I position setpoint equal
 	 *

src/modules/navigator/mission_block.cpp

@@ -1059,3 +1059,15 @@ float MissionBlock::setYawFromHeadingMode(const DestinationPosition &dest, Headi
 
 	return desired_yaw;
 }
+
+void MissionBlock::startPrecLand(uint16_t land_precision)
+{
+	if (_mission_item.land_precision == 1) {
+		_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
+		_navigator->get_precland()->on_activation();
+
+	} else { //_mission_item.land_precision == 2
+		_navigator->get_precland()->set_mode(PrecLandMode::Required);
+		_navigator->get_precland()->on_activation();
+	}
+}

src/modules/navigator/mission_block.h

@@ -223,6 +223,8 @@ protected:
 
 	float setYawFromHeadingMode(const DestinationPosition &dest, HeadingMode heading_mode) const;
 
+	void startPrecLand(uint16_t land_precision);
+
 	/**
 	 * @brief Issue a command for mission items with a nav_cmd that specifies an action
 	 *

src/modules/navigator/rtl_direct.cpp

@@ -322,14 +322,7 @@ void RtlDirect::set_rtl_item()
 
 			_mission_item.land_precision = _param_rtl_pld_md.get();
 
-			if (_mission_item.land_precision == 1) {
+			startPrecLand(_mission_item.land_precision);
-				_navigator->get_precland()->set_mode(PrecLandMode::Opportunistic);
-				_navigator->get_precland()->on_activation();
-
-			} else if (_mission_item.land_precision == 2) {
-				_navigator->get_precland()->set_mode(PrecLandMode::Required);
-				_navigator->get_precland()->on_activation();
-			}
 
 			_rtl_state = RTLState::IDLE;
 

src/modules/navigator/rtl_direct_mission_land.cpp

@@ -140,9 +140,32 @@ void RtlDirectMissionLand::setActiveMissionItems()
 		new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF;
 
 	} else if (item_contains_position(_mission_item)) {
+
+		static constexpr size_t max_num_next_items{1u};
+		int32_t next_mission_items_index[max_num_next_items];
+		size_t num_found_items = 0;
+		getNextPositionItems(_mission.current_seq + 1, next_mission_items_index, num_found_items, max_num_next_items);
+
+		mission_item_s next_mission_items[max_num_next_items];
+		const dm_item_t dataman_id = static_cast<dm_item_t>(_mission.dataman_id);
+
+		for (size_t i = 0U; i < num_found_items; i++) {
+			mission_item_s next_mission_item;
+			bool success = _dataman_cache.loadWait(dataman_id, next_mission_items_index[i],
+							       reinterpret_cast<uint8_t *>(&next_mission_item), sizeof(next_mission_item), MAX_DATAMAN_LOAD_WAIT);
+
+			if (success) {
+				next_mission_items[i] = next_mission_item;
+
+			} else {
+				num_found_items = i;
+				break;
+			}
+		}
+
 		if (_mission_item.nav_cmd == NAV_CMD_LAND ||
 		    _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
-			handleLanding(new_work_item_type);
+			handleLanding(new_work_item_type, next_mission_items, num_found_items);
 
 		} else {
 			// convert mission item to a simple waypoint, keep loiter to alt
@@ -156,21 +179,9 @@ void RtlDirectMissionLand::setActiveMissionItems()
 			pos_sp_triplet->previous = pos_sp_triplet->current;
 		}
 
-		int32_t next_mission_item_index;
-		size_t num_found_items = 0;
-		getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1u);
-
 		if (num_found_items > 0) {
-
+			mission_apply_limitation(next_mission_items[0u]);
-			const dm_item_t dataman_id = static_cast<dm_item_t>(_mission.dataman_id);
+			mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next);
-			mission_item_s next_mission_item;
-			bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index,
-							       reinterpret_cast<uint8_t *>(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT);
-
-			if (success) {
-				mission_apply_limitation(next_mission_item);
-				mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next);
-			}
 		}
 
 		mission_apply_limitation(_mission_item);
@@ -192,85 +203,6 @@ void RtlDirectMissionLand::setActiveMissionItems()
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
-void RtlDirectMissionLand::handleLanding(WorkItemType &new_work_item_type)
-{
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	bool needs_to_land = !_land_detected_sub.get().landed &&
-			     ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND)
-			      || (_mission_item.nav_cmd == NAV_CMD_LAND));
-	bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol &&
-				  (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
-				  needs_to_land;
-
-	if (needs_vtol_landing) {
-		if (_work_item_type != WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) {
-			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
-
-			float altitude = _global_pos_sub.get().alt;
-
-			if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
-				altitude = pos_sp_triplet->current.alt;
-			}
-
-			_mission_item.altitude = altitude;
-			_mission_item.altitude_is_relative = false;
-			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-			_mission_item.autocontinue = true;
-			_mission_item.time_inside = 0.0f;
-			_mission_item.vtol_back_transition = true;
-
-			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
-
-		}
-
-		/* transition to MC */
-		if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) {
-
-			set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
-			_mission_item.altitude = _global_pos_sub.get().alt;
-			_mission_item.altitude_is_relative = false;
-			_mission_item.yaw = NAN;
-
-			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
-
-			// make previous setpoint invalid, such that there will be no prev-current line following
-			// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
-			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
-		}
-
-	} else if (needs_to_land) {
-		/* move to landing waypoint before descent if necessary */
-		if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) &&
-		    do_need_move_to_land() &&
-		    (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT ||
-		     _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
-
-			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
-
-			_mission_item.altitude = _global_pos_sub.get().alt;
-			_mission_item.altitude_is_relative = false;
-			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-			_mission_item.autocontinue = true;
-			_mission_item.time_inside = 0.0f;
-
-			// make previous setpoint invalid, such that there will be no prev-current line following.
-			// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
-			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
-
-		}
-	}
-}
-
-bool RtlDirectMissionLand::do_need_move_to_land()
-{
-	float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
-			  _global_pos_sub.get().lat, _global_pos_sub.get().lon);
-
-	return d_current > _navigator->get_acceptance_radius();
-
-}
-
 rtl_time_estimate_s RtlDirectMissionLand::calc_rtl_time_estimate()
 {
 	rtl_time_estimate_s time_estimate;

src/modules/navigator/rtl_direct_mission_land.h

@@ -65,8 +65,6 @@ public:
 private:
 	bool setNextMissionItem() override;
 	void setActiveMissionItems() override;
-	void handleLanding(WorkItemType &new_work_item_type);
-	bool do_need_move_to_land();
 
 	bool _needs_climbing{false}; 	//< Flag if climbing is required at the start
 	bool _enforce_rtl_alt{false};

src/modules/navigator/rtl_mission_fast.cpp

@@ -54,6 +54,8 @@ RtlMissionFast::RtlMissionFast(Navigator *navigator) :
 
 void RtlMissionFast::on_activation()
 {
+	_home_pos_sub.update();
+
 	_is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
 			_global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK;
 
@@ -65,18 +67,6 @@ void RtlMissionFast::on_activation()
 	MissionBase::on_activation();
 }
 
-void RtlMissionFast::on_active()
-{
-	_home_pos_sub.update();
-	MissionBase::on_active();
-}
-
-void RtlMissionFast::on_inactive()
-{
-	_home_pos_sub.update();
-	MissionBase::on_inactive();
-}
-
 bool RtlMissionFast::setNextMissionItem()
 {
 	return (goToNextPositionItem(true) == PX4_OK);
@@ -100,9 +90,32 @@ void RtlMissionFast::setActiveMissionItems()
 		new_work_item_type = WorkItemType::WORK_ITEM_TYPE_TRANSITION_AFTER_TAKEOFF;
 
 	} else if (item_contains_position(_mission_item)) {
+
+		static constexpr size_t max_num_next_items{1u};
+		int32_t next_mission_items_index[max_num_next_items];
+		size_t num_found_items = 0;
+		getNextPositionItems(_mission.current_seq + 1, next_mission_items_index, num_found_items, max_num_next_items);
+
+		mission_item_s next_mission_items[max_num_next_items];
+		const dm_item_t dataman_id = static_cast<dm_item_t>(_mission.dataman_id);
+
+		for (size_t i = 0U; i < num_found_items; i++) {
+			mission_item_s next_mission_item;
+			bool success = _dataman_cache.loadWait(dataman_id, next_mission_items_index[i],
+							       reinterpret_cast<uint8_t *>(&next_mission_item), sizeof(next_mission_item), MAX_DATAMAN_LOAD_WAIT);
+
+			if (success) {
+				next_mission_items[i] = next_mission_item;
+
+			} else {
+				num_found_items = i;
+				break;
+			}
+		}
+
 		if (_mission_item.nav_cmd == NAV_CMD_LAND ||
 		    _mission_item.nav_cmd == NAV_CMD_VTOL_LAND) {
-			handleLanding(new_work_item_type);
+			handleLanding(new_work_item_type, next_mission_items, num_found_items);
 
 		} else {
 			// convert mission item to a simple waypoint, keep loiter to alt
@@ -116,21 +129,11 @@ void RtlMissionFast::setActiveMissionItems()
 			pos_sp_triplet->previous = pos_sp_triplet->current;
 		}
 
-		int32_t next_mission_item_index;
+
-		size_t num_found_items = 0;
-		getNextPositionItems(_mission.current_seq + 1, &next_mission_item_index, num_found_items, 1u);
 
 		if (num_found_items > 0) {
-
+			mission_apply_limitation(next_mission_items[0u]);
-			const dm_item_t dataman_id = static_cast<dm_item_t>(_mission.dataman_id);
+			mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next);
-			mission_item_s next_mission_item;
-			bool success = _dataman_cache.loadWait(dataman_id, next_mission_item_index,
-							       reinterpret_cast<uint8_t *>(&next_mission_item), sizeof(mission_item_s), MAX_DATAMAN_LOAD_WAIT);
-
-			if (success) {
-				mission_apply_limitation(next_mission_item);
-				mission_item_to_position_setpoint(next_mission_item, &pos_sp_triplet->next);
-			}
 		}
 
 		mission_apply_limitation(_mission_item);
@@ -152,85 +155,6 @@ void RtlMissionFast::setActiveMissionItems()
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
-void RtlMissionFast::handleLanding(WorkItemType &new_work_item_type)
-{
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	bool needs_to_land = !_land_detected_sub.get().landed &&
-			     ((_mission_item.nav_cmd == NAV_CMD_VTOL_LAND)
-			      || (_mission_item.nav_cmd == NAV_CMD_LAND));
-	bool needs_vtol_landing = _vehicle_status_sub.get().is_vtol &&
-				  (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
-				  needs_to_land;
-
-	if (needs_vtol_landing) {
-		if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) {
-			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
-
-			float altitude = _global_pos_sub.get().alt;
-
-			if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
-				altitude = pos_sp_triplet->current.alt;
-			}
-
-			_mission_item.altitude = altitude;
-			_mission_item.altitude_is_relative = false;
-			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-			_mission_item.autocontinue = true;
-			_mission_item.time_inside = 0.0f;
-			_mission_item.vtol_back_transition = true;
-
-			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
-
-		}
-
-		/* transition to MC */
-		if (_work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND) {
-
-			set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
-			_mission_item.altitude = _global_pos_sub.get().alt;
-			_mission_item.altitude_is_relative = false;
-			_mission_item.yaw = NAN;
-
-			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
-
-			// make previous setpoint invalid, such that there will be no prev-current line following
-			// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
-			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
-		}
-
-	} else if (needs_to_land) {
-		/* move to landing waypoint before descent if necessary */
-		if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) &&
-		    do_need_move_to_land() &&
-		    (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT ||
-		     _work_item_type == WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION)) {
-
-			new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
-
-			_mission_item.altitude = _global_pos_sub.get().alt;
-			_mission_item.altitude_is_relative = false;
-			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-			_mission_item.autocontinue = true;
-			_mission_item.time_inside = 0.0f;
-
-			// make previous setpoint invalid, such that there will be no prev-current line following.
-			// if the vehicle drifted off the path during back-transition it should just go straight to the landing point
-			_navigator->reset_position_setpoint(pos_sp_triplet->previous);
-
-		}
-	}
-}
-
-bool RtlMissionFast::do_need_move_to_land()
-{
-	float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
-			  _global_pos_sub.get().lat, _global_pos_sub.get().lon);
-
-	return d_current > _navigator->get_acceptance_radius();
-
-}
-
 rtl_time_estimate_s RtlMissionFast::calc_rtl_time_estimate()
 {
 	rtl_time_estimate_s time_estimate;

src/modules/navigator/rtl_mission_fast.h

@@ -56,16 +56,12 @@ public:
 	~RtlMissionFast() = default;
 
 	void on_activation() override;
-	void on_active() override;
-	void on_inactive() override;
 
 	rtl_time_estimate_s calc_rtl_time_estimate() override;
 
 private:
 	bool setNextMissionItem() override;
 	void setActiveMissionItems() override;
-	void handleLanding(WorkItemType &new_work_item_type);
-	bool do_need_move_to_land();
 
 	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
 };

src/modules/navigator/rtl_mission_fast_reverse.cpp

@@ -54,6 +54,8 @@ RtlMissionFastReverse::RtlMissionFastReverse(Navigator *navigator) :
 
 void RtlMissionFastReverse::on_activation()
 {
+	_home_pos_sub.update();
+
 	_is_current_planned_mission_item_valid = setMissionToClosestItem(_global_pos_sub.get().lat, _global_pos_sub.get().lon,
 			_global_pos_sub.get().alt, _home_pos_sub.get().alt, _vehicle_status_sub.get()) == PX4_OK;
 
@@ -71,12 +73,6 @@ void RtlMissionFastReverse::on_active()
 	MissionBase::on_active();
 }
 
-void RtlMissionFastReverse::on_inactive()
-{
-	_home_pos_sub.update();
-	MissionBase::on_inactive();
-}
-
 bool RtlMissionFastReverse::setNextMissionItem()
 {
 	return (goToPreviousPositionItem(true) == PX4_OK);
@@ -227,7 +223,7 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type)
 			_mission_item.yaw = NAN;
 
 			if ((_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) &&
-			    do_need_move_to_land()) {
+			    do_need_move_to_item()) {
 				new_work_item_type = WorkItemType::WORK_ITEM_TYPE_MOVE_TO_LAND;
 
 				_mission_item.altitude = _global_pos_sub.get().alt;
@@ -249,15 +245,6 @@ void RtlMissionFastReverse::handleLanding(WorkItemType &new_work_item_type)
 	}
 }
 
-bool RtlMissionFastReverse::do_need_move_to_land()
-{
-	float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
-			  _global_pos_sub.get().lat, _global_pos_sub.get().lon);
-
-	return d_current > _navigator->get_acceptance_radius();
-
-}
-
 rtl_time_estimate_s RtlMissionFastReverse::calc_rtl_time_estimate()
 {
 	rtl_time_estimate_s time_estimate;

src/modules/navigator/rtl_mission_fast_reverse.h

@@ -57,7 +57,6 @@ public:
 
 	void on_activation() override;
 	void on_active() override;
-	void on_inactive() override;
 
 	rtl_time_estimate_s calc_rtl_time_estimate() override;
 
@@ -65,7 +64,6 @@ private:
 	bool setNextMissionItem() override;
 	void setActiveMissionItems() override;
 	void handleLanding(WorkItemType &new_work_item_type);
-	bool do_need_move_to_land();
 
 	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
 };