[Mission]: Update the mission mode to use the planned mission interface library

src/lib/CMakeLists.txt

@@ -53,6 +53,7 @@ add_subdirectory(l1 EXCLUDE_FROM_ALL)
 add_subdirectory(led EXCLUDE_FROM_ALL)
 add_subdirectory(matrix EXCLUDE_FROM_ALL)
 add_subdirectory(mathlib EXCLUDE_FROM_ALL)
+add_subdirectory(mission EXCLUDE_FROM_ALL)
 add_subdirectory(mixer_module EXCLUDE_FROM_ALL)
 add_subdirectory(motion_planning EXCLUDE_FROM_ALL)
 add_subdirectory(npfg EXCLUDE_FROM_ALL)

src/lib/mission/CMakeLists.txt

@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+px4_add_library(planned_mission_interface
+	planned_mission_interface.cpp
+)

src/lib/mission/planned_mission_interface.cpp

@@ -0,0 +1,595 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file planned_mission_interface.cpp
+ *
+ */
+
+#include "planned_mission_interface.h"
+
+#include <stdlib.h>
+
+#include "dataman/dataman.h"
+#include "lib/geo/geo.h"
+#include "modules/navigator/mission_block.h"
+
+void PlannedMissionInterface::update()
+{
+	_home_pos_sub.update();
+
+	if (!_is_land_start_item_searched && _home_pos_sub.get().valid_alt) {
+		findLandStartItem();
+		_is_land_start_item_searched = true;
+	}
+
+	if (_mission_sub.updated()) {
+		mission_s new_mission;
+		_mission_sub.update(&new_mission);
+
+		if (isMissionValid(new_mission)) {
+			/* Check if it was updated externally*/
+			if (new_mission.timestamp > _mission.timestamp) {
+				bool mission_waypoints_changed{checkMissionWaypointsChanged(_mission, new_mission)};
+				_mission = new_mission;
+
+				if (goToItem(new_mission.current_seq, true) == EXIT_SUCCESS) {
+					if (_home_pos_sub.get().valid_alt) {
+						findLandStartItem();
+
+					} else {
+						_is_land_start_item_searched = false;
+					}
+
+					onMissionUpdate(mission_waypoints_changed);
+				}
+			}
+		}
+	}
+}
+
+void PlannedMissionInterface::getPreviousPositionItems(int32_t start_index, struct mission_item_s items[],
+		size_t &num_found_items, uint8_t max_num_items) const
+{
+	num_found_items = 0u;
+
+	int32_t next_mission_index{start_index};
+
+	for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) {
+		if (next_mission_index < 0) {
+			break;
+		}
+
+		mission_item_s next_mission_item;
+		bool found_next_item{false};
+
+		do {
+			found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, true) == EXIT_SUCCESS;
+			next_mission_index--;
+		} while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item);
+
+		if (found_next_item) {
+			items[item_idx] = next_mission_item;
+			num_found_items = item_idx + 1;
+
+		} else {
+			break;
+		}
+	}
+}
+
+void PlannedMissionInterface::getNextPositionItems(int32_t start_index, struct mission_item_s items[],
+		size_t &num_found_items, uint8_t max_num_items) const
+{
+	// Make sure vector does not contain any preexisting elements.
+	num_found_items = 0u;
+
+	int32_t next_mission_index{start_index};
+
+	for (size_t item_idx = 0u; item_idx < max_num_items; item_idx++) {
+		if (next_mission_index >= _mission.count) {
+			break;
+		}
+
+		mission_item_s next_mission_item;
+		bool found_next_item{false};
+
+		do {
+			found_next_item = getNonJumpItem(next_mission_index, next_mission_item, true, false, false) == EXIT_SUCCESS;
+			next_mission_index++;
+		} while (!MissionBlock::item_contains_position(next_mission_item) && found_next_item);
+
+		if (found_next_item) {
+			items[item_idx] = next_mission_item;
+			num_found_items = item_idx + 1;
+
+		} else {
+			break;
+		}
+	}
+}
+
+int PlannedMissionInterface::getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump,
+		bool write_jumps, bool mission_direction_backward) const
+{
+	if (mission_index >= _mission.count || mission_index < 0) {
+		return EXIT_FAILURE;
+	}
+
+	int32_t new_mission_index{mission_index};
+	mission_item_s new_mission;
+
+	for (uint16_t jump_count = 0u; jump_count < _max_jump_iteraion; jump_count++) {
+		if (readMissionItem(new_mission, new_mission_index) != EXIT_SUCCESS) {
+			PX4_ERR("Could not read next item.");
+			return EXIT_FAILURE;
+		}
+
+		if (new_mission.nav_cmd == NAV_CMD_DO_JUMP) {
+			if (new_mission.do_jump_mission_index >= _mission.count || new_mission.do_jump_mission_index < 0) {
+				PX4_ERR("Do Jump mission index is out of bounds.");
+				return EXIT_FAILURE;
+			}
+
+			if ((new_mission.do_jump_current_count < new_mission.do_jump_repeat_count) && execute_jump) {
+				if (write_jumps) {
+					new_mission.do_jump_current_count++;
+
+					if (writeMissionItem(new_mission, new_mission_index) != EXIT_SUCCESS) {
+						PX4_ERR("Could not update jump count on mission item.");
+						// Still continue searching for next non jump item.
+					}
+				}
+
+				new_mission_index = new_mission.do_jump_mission_index;
+
+			} else {
+				if (mission_direction_backward) {
+					new_mission_index--;
+
+				} else {
+					new_mission_index++;
+				}
+			}
+
+		} else {
+			break;
+		}
+	}
+
+	mission_index = new_mission_index;
+	mission = new_mission;
+
+	return EXIT_SUCCESS;
+}
+
+bool PlannedMissionInterface::hasMissionLandStart() const
+{
+	return (_land_start_index != _invalid_index) && (_land_start_index < _mission.count);
+}
+
+int PlannedMissionInterface::goToNextItem(bool execute_jump)
+{
+	if (_mission.current_seq + 1 >= (_mission.count)) {
+		return EXIT_FAILURE;
+	}
+
+	return goToItem(_mission.current_seq + 1, execute_jump);
+}
+
+int PlannedMissionInterface::goToPreviousItem(bool execute_jump)
+{
+	if (_mission.current_seq <= 0) {
+		return EXIT_FAILURE;
+	}
+
+	return goToItem(_mission.current_seq - 1, execute_jump, true);
+}
+
+int PlannedMissionInterface::goToItem(int32_t index, bool execute_jump, bool mission_direction_backward)
+{
+	mission_item_s mission_item;
+
+	if (getNonJumpItem(index, mission_item, execute_jump, true, mission_direction_backward) == EXIT_SUCCESS) {
+		if (setMissionIndex(index) == EXIT_SUCCESS) {
+			_current_planned_mission_item = mission_item;
+
+		} else {
+			return EXIT_FAILURE;
+		}
+
+	} else {
+		return EXIT_FAILURE;
+	}
+
+	return EXIT_SUCCESS;
+}
+
+int PlannedMissionInterface::goToPreviousPositionItem(bool execute_jump)
+{
+	do {
+		if (goToPreviousItem(execute_jump) != EXIT_SUCCESS) {
+			return EXIT_FAILURE;
+		}
+	} while (!MissionBlock::item_contains_position(_current_planned_mission_item));
+
+	return EXIT_SUCCESS;
+}
+
+int PlannedMissionInterface::goToNextPositionItem(bool execute_jump)
+{
+	do {
+		if (goToNextItem(execute_jump) != EXIT_SUCCESS) {
+			return EXIT_FAILURE;
+		}
+	} while (!MissionBlock::item_contains_position(_current_planned_mission_item));
+
+	return EXIT_SUCCESS;
+}
+
+int PlannedMissionInterface::setMissionIndex(int32_t index)
+{
+	// Nothing to do if it is already at the current index.
+	if (index == _mission.current_seq) {
+		return EXIT_SUCCESS;
+	}
+
+	mission_s mission{_mission};
+	mission.current_seq = index;
+	mission.timestamp = hrt_absolute_time();
+
+	if (writeMission(mission) == EXIT_SUCCESS) {
+		_mission = mission;
+		return EXIT_SUCCESS;
+
+	} else {
+		return EXIT_FAILURE;
+	}
+}
+
+int PlannedMissionInterface::setMissionToClosestItem(double lat, double lon, float alt, float home_alt,
+		const vehicle_status_s &vehicle_status)
+{
+	int32_t min_dist_index(-1);
+	float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX);
+
+	for (int32_t mission_item_index = 0; mission_item_index < _mission.count; mission_item_index++) {
+		mission_item_s mission;
+
+		if (readMissionItem(mission, mission_item_index) != EXIT_SUCCESS) {
+			PX4_ERR("Could not set mission closest to position.");
+			return EXIT_FAILURE;
+		}
+
+		if (MissionBlock::item_contains_position(mission)) {
+			// do not consider land waypoints for a fw
+			if (!((mission.nav_cmd == NAV_CMD_LAND) &&
+			      (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
+			      (!vehicle_status.is_vtol))) {
+				float dist = get_distance_to_point_global_wgs84(mission.lat, mission.lon,
+						MissionBlock::get_absolute_altitude_for_item(mission, home_alt),
+						lat,
+						lon,
+						alt,
+						&dist_xy, &dist_z);
+
+				if (dist < min_dist) {
+					min_dist = dist;
+					min_dist_index = mission_item_index;
+				}
+			}
+		}
+	}
+
+	return goToItem(min_dist_index, false);
+}
+
+int PlannedMissionInterface::goToMissionLandStart()
+{
+	if (!hasMissionLandStart()) {
+		return EXIT_FAILURE;
+	}
+
+	return goToItem(_land_start_index, false);
+}
+
+int PlannedMissionInterface::initMission()
+{
+	mission_s mission;
+	bool ret_val{EXIT_FAILURE};
+
+	if (readMission(mission) == EXIT_SUCCESS) {
+		_mission = mission;
+
+		if (goToItem(mission.current_seq, true) == EXIT_SUCCESS) {
+			findLandStartItem();
+			ret_val = EXIT_SUCCESS;
+		}
+
+	} else {
+		resetMission();
+	}
+
+	_mission_pub.advertise();
+
+	return ret_val;
+}
+
+void PlannedMissionInterface::resetMission()
+{
+	/* we do not need to reset mission if there is none.*/
+	if (_mission.count == 0u) {
+		return;
+	}
+
+	/* Set a new mission*/
+	mission_s new_mission{.timestamp = hrt_absolute_time(),
+			      .current_seq = 0,
+			      .count = 0u,
+			      .dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0};
+
+	if (writeMission(new_mission) == EXIT_SUCCESS) {
+		_mission = new_mission;
+
+	} else {
+		PX4_ERR("Mission Initialization failed.");
+	}
+}
+
+void PlannedMissionInterface::resetMissionJumpCounter()
+{
+	for (size_t mission_index = 0u; mission_index < _mission.count; mission_index++) {
+		mission_item_s mission_item;
+
+		if (readMissionItem(mission_item, mission_index) != EXIT_SUCCESS) {
+			PX4_ERR("Could not read mission item for jump count reset.");
+			break;
+		}
+
+		if (mission_item.nav_cmd == NAV_CMD_DO_JUMP) {
+			mission_item.do_jump_current_count = 0u;
+
+			if (writeMissionItem(mission_item, mission_index) != EXIT_SUCCESS) {
+				PX4_ERR("Could not write mission item for jump count reset.");
+				break;
+			}
+		}
+	}
+}
+
+int PlannedMissionInterface::writeMission(mission_s &mission)
+{
+	int ret_val{EXIT_SUCCESS};
+
+	if (!isMissionValid(mission)) {
+		return EXIT_FAILURE;
+	}
+
+	/* lock MISSION_STATE item */
+	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+
+	if (dm_lock_ret != 0) {
+		PX4_ERR("DM_KEY_MISSION_STATE lock failed");
+		return EXIT_FAILURE;
+	}
+
+	if (dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) != sizeof(mission_s)) {
+		PX4_ERR("Can't save mission state");
+		ret_val = EXIT_FAILURE;
+
+	} else {
+		_mission_pub.publish(mission);
+	}
+
+	dm_unlock(DM_KEY_MISSION_STATE);
+
+	return ret_val;
+}
+
+int PlannedMissionInterface::readMission(mission_s &read_mission) const
+{
+	int ret_val{EXIT_SUCCESS};
+
+	/* lock MISSION_STATE item */
+	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+
+	if (dm_lock_ret != 0) {
+		PX4_ERR("DM lock failed.");
+		return EXIT_FAILURE;
+	}
+
+	mission_s mission;
+
+	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) != sizeof(mission_s)) {
+		PX4_ERR("Can't read mission state.");
+		ret_val = EXIT_FAILURE;
+
+	} else {
+		if (isMissionValid(mission)) {
+			read_mission = mission;
+
+		} else {
+			ret_val = EXIT_FAILURE;
+		}
+	}
+
+	dm_unlock(DM_KEY_MISSION_STATE);
+
+	return ret_val;
+}
+
+int PlannedMissionInterface::readMissionItem(mission_item_s &read_mission_item, size_t index) const
+{
+	int ret_val{EXIT_SUCCESS};
+
+	if (index >= _mission.count) {
+		return EXIT_FAILURE;
+	}
+
+	dm_item_t current_dm_item{static_cast<dm_item_t>(_mission.dataman_id)};
+
+	/* lock current mission item */
+	/*int dm_lock_ret = dm_lock(current_dm_item);
+
+	if (dm_lock_ret != 0) {
+		PX4_ERR("DM lock failed.");
+		return EXIT_FAILURE;
+	}*/
+
+	mission_item_s mission_item;
+
+	if (dm_read(current_dm_item, index, &mission_item, sizeof(mission_item_s)) != sizeof(mission_item_s)) {
+		PX4_ERR("Can't read mission item from DM.");
+		ret_val = EXIT_FAILURE;
+
+	} else {
+		read_mission_item = mission_item;
+	}
+
+	//dm_unlock(current_dm_item);
+
+	return ret_val;
+}
+
+int PlannedMissionInterface::writeMissionItem(const mission_item_s &mission_item, size_t index) const
+{
+	int ret_val{EXIT_SUCCESS};
+
+	if (index >= _mission.count) {
+		return EXIT_FAILURE;
+	}
+
+	dm_item_t current_dm_item{static_cast<dm_item_t>(_mission.dataman_id)};
+
+	/* lock current mission item */
+	/*int dm_lock_ret = dm_lock(current_dm_item);
+
+	if (dm_lock_ret != 0) {
+		PX4_ERR("DM lock failed.");
+		return EXIT_FAILURE;
+	}*/
+
+	if (dm_write(current_dm_item, index, &mission_item, sizeof(mission_item_s)) != sizeof(mission_item_s)) {
+		PX4_ERR("Can't write mission item to DM.");
+		ret_val = EXIT_FAILURE;
+	}
+
+	//dm_unlock(current_dm_item);
+
+	return ret_val;
+}
+
+bool PlannedMissionInterface::isMissionValid(mission_s &mission) const
+{
+	bool ret_val{false};
+
+	if ((mission.current_seq < mission.count) &&
+	    (mission.current_seq >= 0) &&
+	    (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) &&
+	    (mission.timestamp != 0u)) {
+		ret_val = true;
+
+	}
+
+	return ret_val;
+}
+
+void PlannedMissionInterface::findLandStartItem()
+{
+	_land_start_index = _invalid_index;
+	_land_index = _invalid_index;
+
+	for (size_t mission_item_index = 1u; mission_item_index < _mission.count; mission_item_index++) {
+		mission_item_s mission;
+
+		if (readMissionItem(mission, mission_item_index) == EXIT_SUCCESS) {
+			if (mission.nav_cmd == NAV_CMD_DO_LAND_START) {
+				_land_start_index = mission_item_index;
+			}
+
+			if ((mission.nav_cmd == NAV_CMD_VTOL_LAND) || (mission.nav_cmd == NAV_CMD_LAND)) {
+				_land_index = mission_item_index;
+				break;
+			}
+
+		} else {
+			break;
+		}
+	}
+
+	if (_land_start_index != _invalid_index) {
+		mission_item_s mission;
+		size_t num_found_item{0u};
+		getNextPositionItems(_land_start_index, &mission, num_found_item, 1u);
+
+		if (num_found_item == 1u) {
+			_land_start_pos.lat = mission.lat;
+			_land_start_pos.lon = mission.lon;
+			_land_start_pos.alt = mission.altitude_is_relative ?	mission.altitude +
+					      _home_pos_sub.get().alt : mission.altitude;
+
+		} else {
+			PX4_ERR("Could not read land start coordinates.");
+			_land_start_pos.lat = 0.0;
+			_land_start_pos.lon = 0.0;
+		}
+
+		_land_pos.lat = _land_start_pos.lat;
+		_land_pos.lon = _land_start_pos.lon;
+		_land_pos.alt = _land_start_pos.alt;
+	}
+
+	if (_land_index != _invalid_index) {
+		mission_item_s mission;
+
+		if (readMissionItem(mission, _land_index) == EXIT_SUCCESS) {
+			_land_pos.lat = mission.lat;
+			_land_pos.lon = mission.lon;
+			_land_pos.alt = mission.altitude_is_relative ?	mission.altitude +
+					_home_pos_sub.get().alt : mission.altitude;
+		}
+
+		if (_land_start_index == _invalid_index) {
+			_land_start_index = _land_index;
+			_land_start_pos.lat = _land_pos.lat;
+			_land_start_pos.lon = _land_pos.lon;
+			_land_start_pos.alt = _land_pos.alt;
+		}
+	}
+}
+
+bool PlannedMissionInterface::checkMissionWaypointsChanged(const mission_s &old_mission,
+		const mission_s &new_mission) const
+{
+	return (new_mission.count != old_mission.count) || (new_mission.dataman_id != old_mission.dataman_id);
+}

src/lib/mission/planned_mission_interface.h

@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file planned_mission_interface.h
+ *
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/uORB.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/vehicle_status.h>
+
+#include "navigator/navigation.h"
+
+class PlannedMissionInterface
+{
+public:
+	void update();
+	void getPreviousPositionItems(int32_t start_index, struct mission_item_s items[], size_t &num_found_items,
+				      uint8_t max_num_items) const;
+	void getNextPositionItems(int32_t start_index, struct mission_item_s items[], size_t &num_found_items,
+				  uint8_t max_num_items) const;
+	bool hasMissionLandStart() const;
+	int goToNextItem(bool execute_jump);
+	int goToPreviousItem(bool execute_jump);
+	int goToItem(int32_t index, bool execute_jump, bool mission_direction_backward = false);
+	int goToPreviousPositionItem(bool execute_jump);
+	int goToNextPositionItem(bool execute_jump);
+	int goToMissionLandStart();
+	int setMissionToClosestItem(double lat, double lon, float alt, float home_alt, const vehicle_status_s &vehicle_status);
+	virtual void onMissionUpdate(bool has_mission_items_changed) = 0;
+
+	int initMission();
+	void resetMission();
+	void resetMissionJumpCounter();
+
+private:
+	int getNonJumpItem(int32_t &mission_index, mission_item_s &mission, bool execute_jump, bool write_jumps,
+			   bool mission_direction_backward = false) const;
+	int setMissionIndex(int32_t index);
+	int writeMission(mission_s &mission);
+	int readMission(mission_s &read_mission) const;
+	int readMissionItem(mission_item_s &read_mission_item, size_t index) const;
+	int writeMissionItem(const mission_item_s &mission_item, size_t index) const ;
+	bool isMissionValid(mission_s &mission) const;
+	void findLandStartItem();
+	bool checkMissionWaypointsChanged(const mission_s &old_mission, const mission_s &new_mission) const;
+public:
+	static const uint16_t _invalid_index{UINT16_MAX};
+private:
+	static const uint16_t _max_jump_iteraion{10u};
+protected:
+	mission_s _mission;
+	mission_item_s _current_planned_mission_item;
+	uint16_t _land_start_index;
+	uint16_t _land_index;
+	struct {
+		double lat;
+		double lon;
+		float alt;
+	} _land_start_pos{.lat = 0.0,
+				  .lon = 0.0,
+				  .alt = 0.0f},
+			       _land_pos{.lat = 0.0,
+					 .lon = 0.0,
+					 .alt = 0.0f};
+
+private:
+	bool _is_land_start_item_searched{false};
+
+	uORB::Subscription _mission_sub{ORB_ID(mission)};
+	uORB::SubscriptionData<home_position_s> _home_pos_sub{ORB_ID(home_position)};		/**< home position subscription */
+	uORB::Publication<mission_s> _mission_pub{ORB_ID(mission)};
+};

src/modules/navigator/CMakeLists.txt

@@ -53,4 +53,5 @@ px4_add_module(
 		geo
 		geofence_breach_avoidance
 		motion_planning
+		planned_mission_interface
 	)

src/modules/navigator/mission.cpp

@@ -67,28 +67,33 @@ Mission::Mission(Navigator *navigator) :
 	MissionBlock(navigator),
 	ModuleParams(navigator)
 {
-	mission_init();
+	_is_current_planned_mission_item_valid = initMission() == EXIT_SUCCESS;
 }
 
-void Mission::mission_init()
+void Mission::onMissionUpdate(bool has_mission_items_changed)
 {
-	// init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start
-	mission_s mission{};
+	if (has_mission_items_changed && !_navigator->get_land_detected()->landed) {
+		_mission_waypoints_changed = true;
+	}
 
-	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
-		if ((mission.timestamp != 0)
-		    && (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1)) {
-			if (mission.count > 0) {
-				PX4_INFO("Mission #%" PRIu8 " loaded, %" PRIu16 " WPs", mission.dataman_id, mission.count);
-			}
+	_is_current_planned_mission_item_valid = true;
+	update_mission();
 
-		} else {
-			PX4_ERR("reading mission state failed");
+	if (isActive()) {
+		_navigator->reset_triplets();
 
-			// initialize mission state in dataman
-			mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
-			mission.timestamp = hrt_absolute_time();
-			dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
+		if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) {
+			_is_current_planned_mission_item_valid = setMissionToClosestItem(_navigator->get_global_position()->lat,
+					_navigator->get_global_position()->lon, _navigator->get_global_position()->alt, _navigator->get_home_position()->alt,
+					*_navigator->get_vstatus()) == EXIT_SUCCESS;
+		}
+
+		_mission_waypoints_changed = false;
+		set_mission_items();
+
+	} else {
+		if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) {
+			_mission_type = MISSION_TYPE_MISSION;
 		}
 	}
 }
@@ -96,58 +101,25 @@ void Mission::mission_init()
 void
 Mission::on_inactive()
 {
-	// if we were executing an landing but have been inactive for 2 seconds, then make the landing invalid
+	PlannedMissionInterface::update();
+
+	// if we were executing a landing but have been inactive for 2 seconds, then make the landing invalid
 	// this prevents RTL to just continue at the current mission index
 	if (_navigator->getMissionLandingInProgress() && (hrt_absolute_time() - _time_mission_deactivated) > 2_s) {
 		_navigator->setMissionLandingInProgress(false);
 	}
 
-	/* Without home a mission can't be valid yet anyway, let's wait. */
-	if (!_navigator->home_global_position_valid()) {
-		return;
+	/* reset the current mission if needed */
+	if (need_to_reset_mission()) {
+		reset_mission(_mission);
+		update_mission();
+		_navigator->reset_cruising_speed();
 	}
 
-	if (_inited) {
-		if (_mission_sub.updated()) {
-			update_mission();
-
-			if (_mission_type == MISSION_TYPE_NONE && _mission.count > 0) {
-				_mission_type = MISSION_TYPE_MISSION;
-			}
-		}
-
-		/* reset the current mission if needed */
-		if (need_to_reset_mission()) {
-			reset_mission(_mission);
-			update_mission();
-			_navigator->reset_cruising_speed();
-		}
-
-	} else {
-
-		/* load missions from storage */
-		mission_s mission_state = {};
-
-		dm_lock(DM_KEY_MISSION_STATE);
-
-		/* read current state */
-		int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
-
-		dm_unlock(DM_KEY_MISSION_STATE);
-
-		if (read_res == sizeof(mission_s)) {
-			_mission.dataman_id = mission_state.dataman_id;
-			_mission.count = mission_state.count;
-			_current_mission_index = mission_state.current_seq;
-
-			// find and store landing start marker (if available)
-			find_mission_land_start();
-		}
-
-		/* On init let's check the mission, maybe there is already one available. */
-		check_mission_valid(false);
-
-		_inited = true;
+	/* Need to check the initialized mission once, have to do it here, since we need to wait for the home position. */
+	if (_navigator->home_global_position_valid() && !_initialized_mission_checked) {
+		check_mission_valid();
+		_initialized_mission_checked = true;
 	}
 
 	/* require takeoff after non-loiter or landing */
@@ -192,12 +164,16 @@ Mission::on_activation()
 	if (_mission_waypoints_changed) {
 		// do not set the closest mission item in the normal mission mode
 		if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) {
-			_current_mission_index = index_closest_mission_item();
+			_is_current_planned_mission_item_valid = setMissionToClosestItem(_navigator->get_global_position()->lat,
+					_navigator->get_global_position()->lon, _navigator->get_global_position()->alt, _navigator->get_home_position()->alt,
+					*_navigator->get_vstatus()) == EXIT_SUCCESS;
 		}
 
 		_mission_waypoints_changed = false;
 	}
 
+	check_mission_valid();
+
 	// we already reset the mission items
 	_execution_mode_changed = false;
 
@@ -215,15 +191,7 @@ Mission::on_activation()
 void
 Mission::on_active()
 {
-	check_mission_valid(false);
-
-	/* Check if stored mission plan has changed */
-	const bool mission_sub_updated = _mission_sub.updated();
-
-	if (mission_sub_updated) {
-		_navigator->reset_triplets();
-		update_mission();
-	}
+	PlannedMissionInterface::update();
 
 	/* mission is running (and we are armed), need reset after disarm */
 	_need_mission_reset = true;
@@ -231,16 +199,7 @@ Mission::on_active()
 	_mission_changed = false;
 
 	/* reset mission items if needed */
-	if (mission_sub_updated || _mission_waypoints_changed || _execution_mode_changed) {
-		if (_mission_waypoints_changed) {
-			// do not set the closest mission item in the normal mission mode
-			if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_NORMAL) {
-				_current_mission_index = index_closest_mission_item();
-			}
-
-			_mission_waypoints_changed = false;
-		}
-
+	if (_execution_mode_changed) {
 		_execution_mode_changed = false;
 		set_mission_items();
 	}
@@ -308,23 +267,21 @@ Mission::on_active()
 bool
 Mission::set_current_mission_index(uint16_t index)
 {
-	if (index == _current_mission_index) {
-		return true; // nothing to do, so return true
+	if (_navigator->get_mission_result()->valid && (index < _mission.count)) {
+		_is_current_planned_mission_item_valid = (goToItem(index, true) == EXIT_SUCCESS);
 
-	} else if (_navigator->get_mission_result()->valid && (index < _mission.count)) {
-
-		_current_mission_index = index;
+		if (!_is_current_planned_mission_item_valid) {
+			return false;
+		}
 
 		// a mission index is set manually which has the higher priority than the closest mission item
 		// as it is set by the user
 		_mission_waypoints_changed = false;
 
 		// update mission items if already in active mission
-		if (_navigator->is_planned_mission()) {
+		if (isActive()) {
 			// prevent following "previous - current" line
-			_navigator->get_position_setpoint_triplet()->previous.valid = false;
-			_navigator->get_position_setpoint_triplet()->current.valid = false;
-			_navigator->get_position_setpoint_triplet()->next.valid = false;
+			_navigator->reset_triplets();
 			set_mission_items();
 		}
 
@@ -337,7 +294,10 @@ Mission::set_current_mission_index(uint16_t index)
 void
 Mission::set_closest_item_as_current()
 {
-	_current_mission_index = index_closest_mission_item();
+	_is_current_planned_mission_item_valid = (setMissionToClosestItem(_navigator->get_global_position()->lat,
+			_navigator->get_global_position()->lon,
+			_navigator->get_global_position()->alt, _navigator->get_home_position()->alt,
+			*_navigator->get_vstatus()) == EXIT_SUCCESS);
 }
 
 void
@@ -372,12 +332,7 @@ Mission::set_execution_mode(const uint8_t mode)
 					_mission_type = MISSION_TYPE_MISSION;
 				}
 
-				if (_current_mission_index > _mission.count - 1) {
-					_current_mission_index = _mission.count - 1;
-
-				} else if (_current_mission_index > 0) {
-					--_current_mission_index;
-				}
+				_is_current_planned_mission_item_valid = goToPreviousItem(false) == EXIT_SUCCESS;
 
 				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 			}
@@ -388,12 +343,7 @@ Mission::set_execution_mode(const uint8_t mode)
 			if ((mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL) ||
 			    (mode == mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD)) {
 				// handle switch from reverse to forward mission
-				if (_current_mission_index < 0) {
-					_current_mission_index = 0;
-
-				} else if (_current_mission_index < _mission.count - 1) {
-					++_current_mission_index;
-				}
+				_is_current_planned_mission_item_valid = goToNextItem(true) == EXIT_SUCCESS;
 
 				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 			}
@@ -406,82 +356,20 @@ Mission::set_execution_mode(const uint8_t mode)
 	}
 }
 
-bool
-Mission::find_mission_land_start()
-{
-	/* return true if a MAV_CMD_DO_LAND_START, NAV_CMD_VTOL_LAND or NAV_CMD_LAND is found and internally save the index
-	 *  return false if not found
-	 */
-
-	const dm_item_t dm_current = (dm_item_t)_mission.dataman_id;
-	struct mission_item_s missionitem = {};
-	struct mission_item_s missionitem_prev = {}; //to store mission item before currently checked on, needed to get pos of wp before NAV_CMD_DO_LAND_START
-
-	_land_start_available = false;
-
-	bool found_land_start_marker = false;
-
-	for (size_t i = 1; i < _mission.count; i++) {
-		const ssize_t len = sizeof(missionitem);
-		missionitem_prev = missionitem; // store the last mission item before reading a new one
-
-		if (dm_read(dm_current, i, &missionitem, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			PX4_ERR("dataman read failure");
-			break;
-		}
-
-		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
-			found_land_start_marker = true;
-			_land_start_index = i;
-		}
-
-		if (found_land_start_marker && !_land_start_available && i > _land_start_index
-		    && item_contains_position(missionitem)) {
-			// use the position of any waypoint after the land start marker which specifies a position.
-			_landing_start_lat = missionitem.lat;
-			_landing_start_lon = missionitem.lon;
-			_landing_start_alt = missionitem.altitude_is_relative ?	missionitem.altitude +
-					     _navigator->get_home_position()->alt : missionitem.altitude;
-			_landing_loiter_radius = (PX4_ISFINITE(missionitem.loiter_radius)
-						  && fabsf(missionitem.loiter_radius) > FLT_EPSILON) ? fabsf(missionitem.loiter_radius) :
-						 _navigator->get_loiter_radius();
-			_land_start_available = true;
-		}
-
-		if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) ||
-		    (missionitem.nav_cmd == NAV_CMD_LAND)) {
-
-			_landing_lat = missionitem.lat;
-			_landing_lon = missionitem.lon;
-			_landing_alt = missionitem.altitude_is_relative ?	missionitem.altitude + _navigator->get_home_position()->alt :
-				       missionitem.altitude;
-
-			// don't have a valid land start yet, use the landing item itself then
-			if (!_land_start_available) {
-				_land_start_index = i;
-				_landing_start_lat = _landing_lat;
-				_landing_start_lon = _landing_lon;
-				_landing_start_alt = _landing_alt;
-				_land_start_available = true;
-			}
-
-		}
-	}
-
-	return _land_start_available;
-}
-
 bool
 Mission::land_start()
 {
 	// if not currently landing, jump to do_land_start
-	if (_land_start_available) {
+	if (hasMissionLandStart()) {
 		if (_navigator->getMissionLandingInProgress()) {
 			return true;
 
 		} else {
-			set_current_mission_index(get_land_start_index());
+			_is_current_planned_mission_item_valid = (goToMissionLandStart() == EXIT_SUCCESS);
+
+			if (!_is_current_planned_mission_item_valid) {
+				return false;
+			}
 
 			const bool can_land_now = landing();
 			_navigator->setMissionLandingInProgress(can_land_now);
@@ -499,7 +387,7 @@ Mission::landing()
 	//  mission valid, still flying, and in the landing portion of mission
 
 	const bool mission_valid = _navigator->get_mission_result()->valid;
-	const bool on_landing_stage = _land_start_available && (_current_mission_index >= get_land_start_index());
+	const bool on_landing_stage = hasMissionLandStart() && (_mission.current_seq >= _land_start_index);
 
 	return mission_valid && on_landing_stage;
 }
@@ -515,50 +403,21 @@ Mission::update_mission()
 	 * an existing ROI setting from previous missions */
 	_navigator->reset_vroi();
 
-	const mission_s old_mission = _mission;
+	check_mission_valid();
 
-	if (_mission_sub.copy(&_mission)) {
-		/* determine current index */
-		if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) {
-			_current_mission_index = _mission.current_seq;
+	failed = !_navigator->get_mission_result()->valid;
 
-		} else {
-			/* if less items available, reset to first item */
-			if (_current_mission_index >= (int)_mission.count) {
-				_current_mission_index = 0;
+	if (!failed) {
+		/* reset mission failure if we have an updated valid mission */
+		_navigator->get_mission_result()->failure = false;
 
-			} else if (_current_mission_index < 0) {
-				/* if not initialized, set it to 0 */
-				_current_mission_index = 0;
-			}
+		/* reset sequence info as well */
+		_navigator->get_mission_result()->seq_reached = -1;
+		_navigator->get_mission_result()->seq_total = _mission.count;
 
-			/* otherwise, just leave it */
-		}
-
-		check_mission_valid(true);
-
-		failed = !_navigator->get_mission_result()->valid;
-
-		if (!failed) {
-			/* reset mission failure if we have an updated valid mission */
-			_navigator->get_mission_result()->failure = false;
-
-			/* reset sequence info as well */
-			_navigator->get_mission_result()->seq_reached = -1;
-			_navigator->get_mission_result()->seq_total = _mission.count;
-
-			/* reset work item if new mission has been accepted */
-			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
-			_mission_changed = true;
-		}
-
-		/* check if the mission waypoints changed while the vehicle is in air
-		 * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */
-		if (((_mission.count != old_mission.count) ||
-		     (_mission.dataman_id != old_mission.dataman_id)) &&
-		    !_navigator->get_land_detected()->landed) {
-			_mission_waypoints_changed = true;
-		}
+		/* reset work item if new mission has been accepted */
+		_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+		_mission_changed = true;
 
 	} else {
 		PX4_ERR("mission update failed");
@@ -571,18 +430,13 @@ Mission::update_mission()
 		}
 
 		// reset the mission
-		_mission.count = 0;
-		_mission.current_seq = 0;
-		_current_mission_index = 0;
+		resetMission();
+		_is_current_planned_mission_item_valid = false;
 	}
 
-	// find and store landing start marker (if available)
-	find_mission_land_start();
-
 	set_current_mission_item();
 }
 
-
 void
 Mission::advance_mission()
 {
@@ -596,37 +450,18 @@ Mission::advance_mission()
 		switch (_mission_execution_mode) {
 		case mission_result_s::MISSION_EXECUTION_MODE_NORMAL:
 		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: {
-				_current_mission_index++;
+				_is_current_planned_mission_item_valid = goToNextItem(true) == EXIT_SUCCESS;
 				break;
 			}
 
 		case mission_result_s::MISSION_EXECUTION_MODE_REVERSE: {
 				// find next position item in reverse order
-				dm_item_t dm_current = (dm_item_t)(_mission.dataman_id);
-
-				for (int32_t i = _current_mission_index - 1; i >= 0; i--) {
-					struct mission_item_s missionitem = {};
-					const ssize_t len = sizeof(missionitem);
-
-					if (dm_read(dm_current, i, &missionitem, len) != len) {
-						/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-						PX4_ERR("dataman read failure");
-						break;
-					}
-
-					if (item_contains_position(missionitem)) {
-						_current_mission_index = i;
-						return;
-					}
-				}
-
-				// finished flying back the mission
-				_current_mission_index = -1;
+				_is_current_planned_mission_item_valid = goToPreviousPositionItem(true) == EXIT_SUCCESS;
 				break;
 			}
 
 		default:
-			_current_mission_index++;
+			_is_current_planned_mission_item_valid = goToNextItem(true) == EXIT_SUCCESS;
 		}
 
 		break;
@@ -644,15 +479,20 @@ Mission::set_mission_items()
 	bool user_feedback_done = false;
 
 	/* mission item that comes after current if available */
-	struct mission_item_s mission_item_next_position;
-	struct mission_item_s mission_item_after_next_position;
-	bool has_next_position_item = false;
-	bool has_after_next_position_item = false;
+	constexpr static size_t max_num_next_items{2u};
+	mission_item_s next_mission_items[max_num_next_items];
+	size_t num_found_items;
 
 	work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 
-	if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item,
-				  &mission_item_after_next_position, &has_after_next_position_item)) {
+	if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
+		getPreviousPositionItems(_mission.current_seq - 1, next_mission_items, num_found_items, max_num_next_items);
+
+	} else {
+		getNextPositionItems(_mission.current_seq + 1, next_mission_items, num_found_items, max_num_next_items);
+	}
+
+	if (_is_current_planned_mission_item_valid) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_MISSION) {
 			mavlink_log_info(_navigator->get_mavlink_log_pub(),
@@ -670,6 +510,8 @@ Mission::set_mission_items()
 		}
 
 		_mission_type = MISSION_TYPE_MISSION;
+		/* By default set the mission item to the current planned mission item. Depending on request, it can be altered. */
+		_mission_item = _current_planned_mission_item;
 
 	} else {
 		if (_mission_type != MISSION_TYPE_NONE) {
@@ -789,9 +631,10 @@ Mission::set_mission_items()
 					new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
 
 					/* use current mission item as next position item */
-					mission_item_next_position = _mission_item;
-					mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT;
-					has_next_position_item = true;
+					mission_item_s next_mission_item = _mission_item;
+					next_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+					next_mission_items[0u] = next_mission_item;
+					num_found_items = 1u;
 
 					float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
 
@@ -907,8 +750,8 @@ Mission::set_mission_items()
 					new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 
 					/* use current mission item as next position item */
-					mission_item_next_position = _mission_item;
-					has_next_position_item = true;
+					next_mission_items[0u] = _mission_item;
+					num_found_items = 1u;
 
 					float altitude = _navigator->get_global_position()->alt;
 
@@ -951,8 +794,8 @@ Mission::set_mission_items()
 					new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 
 					/* use current mission item as next position item */
-					mission_item_next_position = _mission_item;
-					has_next_position_item = true;
+					next_mission_items[0u] = _mission_item;
+					num_found_items = 1u;
 
 					/*
 					 * Ignoring waypoint altitude:
@@ -1066,18 +909,18 @@ Mission::set_mission_items()
 				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
 				    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 				    && !_navigator->get_land_detected()->landed
-				    && has_next_position_item) {
+				    && num_found_items > 0u) {
 
 					/* disable weathervane before front transition for allowing yaw to align */
 					pos_sp_triplet->current.disable_weather_vane = true;
 
 					new_work_item_type = WORK_ITEM_TYPE_ALIGN;
 
-					set_align_mission_item(&_mission_item, &mission_item_next_position);
+					set_align_mission_item(&_mission_item, &next_mission_items[0u]);
 
 					/* set position setpoint to target during the transition */
 					mission_apply_limitation(_mission_item);
-					mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current);
+					mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current);
 				}
 
 				/* yaw is aligned now */
@@ -1133,11 +976,10 @@ Mission::set_mission_items()
 		// The mission item is a gate, let's check if the next item in the list provides
 		// a position to go towards.
 
-		// TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320
-		if (has_next_position_item) {
+		if (num_found_items > 0u) {
 			// We have a position, convert it to the setpoint and update setpoint triplet
-			mission_apply_limitation(mission_item_next_position);
-			mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current);
+			mission_apply_limitation(next_mission_items[0u]);
+			mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->current);
 		}
 
 		// ELSE: The current position setpoint stays unchanged.
@@ -1183,10 +1025,10 @@ Mission::set_mission_items()
 	// If the mission item under evaluation contains a gate, we need to check if we have a next position item so
 	// the controller can fly the correct line between the current and next setpoint
 	if (item_contains_gate(_mission_item)) {
-		if (has_after_next_position_item) {
+		if (num_found_items > 1u) {
 			/* got next mission item, update setpoint triplet */
-			mission_apply_limitation(mission_item_next_position);
-			mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next);
+			mission_apply_limitation(next_mission_items[1u]);
+			mission_item_to_position_setpoint(next_mission_items[1u], &pos_sp_triplet->next);
 
 		} else {
 			pos_sp_triplet->next.valid = false;
@@ -1203,9 +1045,9 @@ Mission::set_mission_items()
 
 		if (_mission_item.autocontinue && !brake_for_hold) {
 			/* try to process next mission item */
-			if (has_next_position_item) {
+			if (num_found_items > 0u) {
 				/* got next mission item, update setpoint triplet */
-				mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next);
+				mission_item_to_position_setpoint(next_mission_items[0u], &pos_sp_triplet->next);
 
 			} else {
 				/* next mission item is not available */
@@ -1474,12 +1316,12 @@ Mission::do_abort_landing()
 			    "Holding at {1:.0m_v} above landing waypoint", alt_sp - alt_landing);
 
 	// reset mission index to start of landing
-	if (_land_start_available) {
-		_current_mission_index = get_land_start_index();
+	if (hasMissionLandStart()) {
+		_is_current_planned_mission_item_valid = goToMissionLandStart() == EXIT_SUCCESS;
 
 	} else {
 		// move mission index back (landing approach point)
-		_current_mission_index -= 1;
+		_is_current_planned_mission_item_valid = goToItem(_mission.current_seq - 1u, false, true);
 	}
 
 	// send reposition cmd to get out of mission
@@ -1495,213 +1337,6 @@ Mission::do_abort_landing()
 	_navigator->publish_vehicle_cmd(&vcmd);
 }
 
-bool
-Mission::prepare_mission_items(struct mission_item_s *mission_item,
-			       struct mission_item_s *next_position_mission_item, bool *has_next_position_item,
-			       struct mission_item_s *after_next_position_mission_item, bool *has_after_next_position_item)
-{
-	*has_next_position_item = false;
-	bool first_res = false;
-	int offset = 1;
-
-	if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
-		offset = -1;
-	}
-
-	if (read_mission_item(0, mission_item)) {
-
-		first_res = true;
-
-		/* trying to find next position mission item */
-		while (read_mission_item(offset, next_position_mission_item)) {
-			if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
-				offset--;
-
-			} else {
-				offset++;
-			}
-
-			if (item_contains_position(*next_position_mission_item)) {
-				*has_next_position_item = true;
-				break;
-			}
-		}
-
-		if (_mission_execution_mode != mission_result_s::MISSION_EXECUTION_MODE_REVERSE &&
-		    after_next_position_mission_item && has_after_next_position_item) {
-			/* trying to find next next position mission item */
-			while (read_mission_item(offset, after_next_position_mission_item)) {
-				offset++;
-
-				if (item_contains_position(*after_next_position_mission_item)) {
-					*has_after_next_position_item = true;
-					break;
-				}
-			}
-		}
-	}
-
-	return first_res;
-}
-
-bool
-Mission::read_mission_item(int offset, struct mission_item_s *mission_item)
-{
-	/* select mission */
-	const int current_index = _current_mission_index;
-	int index_to_read = current_index + offset;
-
-	int *mission_index_ptr = (offset == 0) ? (int *) &_current_mission_index : &index_to_read;
-	const dm_item_t dm_item = (dm_item_t)_mission.dataman_id;
-
-	/* do not work on empty missions */
-	if (_mission.count == 0) {
-		return false;
-	}
-
-	/* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
-	 * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
-	for (int i = 0; i < 10; i++) {
-		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)_mission.count) {
-			/* mission item index out of bounds - if they are equal, we just reached the end */
-			if ((*mission_index_ptr != (int)_mission.count) && (*mission_index_ptr != -1)) {
-				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
-						     "Mission item index out of bound, index: %d, max: %" PRIu16 ".\t",
-						     *mission_index_ptr, _mission.count);
-				events::send<uint16_t, uint16_t>(events::ID("mission_index_out_of_bound"), events::Log::Error,
-								 "Mission item index out of bound, index: {1}, max: {2}", *mission_index_ptr, _mission.count);
-			}
-
-			return false;
-		}
-
-		const ssize_t len = sizeof(struct mission_item_s);
-
-		/* read mission item to temp storage first to not overwrite current mission item if data damaged */
-		struct mission_item_s mission_item_tmp;
-
-		/* read mission item from datamanager */
-		if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Waypoint could not be read.\t");
-			events::send<uint16_t>(events::ID("mission_failed_to_read_wp"), events::Log::Error,
-					       "Waypoint {1} could not be read from storage", *mission_index_ptr);
-			return false;
-		}
-
-		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
-		if (mission_item_tmp.nav_cmd == NAV_CMD_DO_JUMP) {
-			const bool execute_jumps = _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_NORMAL;
-
-			/* do DO_JUMP as many times as requested if not in reverse mode */
-			if ((mission_item_tmp.do_jump_current_count < mission_item_tmp.do_jump_repeat_count) && execute_jumps) {
-
-				/* only raise the repeat count if this is for the current mission item
-				 * but not for the read ahead mission item */
-				if (offset == 0) {
-					(mission_item_tmp.do_jump_current_count)++;
-
-					/* save repeat count */
-					if (dm_write(dm_item, *mission_index_ptr, &mission_item_tmp, len) != len) {
-						/* not supposed to happen unless the datamanager can't access the dataman */
-						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP waypoint could not be written.\t");
-						events::send(events::ID("mission_failed_to_write_do_jump"), events::Log::Error,
-							     "DO JUMP waypoint could not be written");
-						return false;
-					}
-
-					report_do_jump_mission_changed(*mission_index_ptr, mission_item_tmp.do_jump_repeat_count);
-				}
-
-				/* set new mission item index and repeat
-				 * we don't have to validate here, if it's invalid, we should realize this later .*/
-				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
-
-			} else {
-				if (offset == 0 && execute_jumps) {
-					mavlink_log_info(_navigator->get_mavlink_log_pub(), "DO JUMP repetitions completed.\t");
-					events::send(events::ID("mission_do_jump_rep_completed"), events::Log::Info,
-						     "DO JUMP repetitions completed");
-				}
-
-				/* no more DO_JUMPS, therefore just try to continue with next mission item */
-				if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
-					(*mission_index_ptr)--;
-
-				} else {
-					(*mission_index_ptr)++;
-				}
-			}
-
-		} else {
-			/* if it's not a DO_JUMP, then we were successful */
-			memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s));
-			return true;
-		}
-	}
-
-	/* we have given up, we don't want to cycle forever */
-	mavlink_log_critical(_navigator->get_mavlink_log_pub(), "DO JUMP is cycling, giving up.\t");
-	events::send(events::ID("mission_do_jump_cycle"), events::Log::Error, "DO JUMP is cycling, giving up");
-	return false;
-}
-
-void
-Mission::save_mission_state()
-{
-	mission_s mission_state = {};
-
-	/* lock MISSION_STATE item */
-	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
-
-	if (dm_lock_ret != 0) {
-		PX4_ERR("DM_KEY_MISSION_STATE lock failed");
-	}
-
-	/* read current state */
-	int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
-
-	if (read_res == sizeof(mission_s)) {
-		/* data read successfully, check dataman ID and items count */
-		if (mission_state.dataman_id == _mission.dataman_id && mission_state.count == _mission.count) {
-			/* navigator may modify only sequence, write modified state only if it changed */
-			if (mission_state.current_seq != _current_mission_index) {
-				mission_state.current_seq = _current_mission_index;
-				mission_state.timestamp = hrt_absolute_time();
-
-				if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
-
-					PX4_ERR("Can't save mission state");
-				}
-			}
-		}
-
-	} else {
-		/* invalid data, this must not happen and indicates error in mission publisher */
-		mission_state.timestamp = hrt_absolute_time();
-		mission_state.dataman_id = _mission.dataman_id;
-		mission_state.count = _mission.count;
-		mission_state.current_seq = _current_mission_index;
-
-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Invalid mission state.\t");
-		/* EVENT
-		 * @description No mission or storage failure
-		 */
-		events::send(events::ID("mission_invalid_mission_state"), events::Log::Error, "Invalid mission state");
-
-		/* write modified state only if changed */
-		if (dm_write(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
-
-			PX4_ERR("Can't save mission state");
-		}
-	}
-
-	/* unlock MISSION_STATE item */
-	if (dm_lock_ret == 0) {
-		dm_unlock(DM_KEY_MISSION_STATE);
-	}
-}
-
 void
 Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
 {
@@ -1716,7 +1351,7 @@ Mission::report_do_jump_mission_changed(int index, int do_jumps_remaining)
 void
 Mission::set_mission_item_reached()
 {
-	_navigator->get_mission_result()->seq_reached = _current_mission_index;
+	_navigator->get_mission_result()->seq_reached = _mission.current_seq;
 	_navigator->set_mission_result_updated();
 
 	// let the navigator know that we are currently executing the mission landing.
@@ -1731,84 +1366,40 @@ void
 Mission::set_current_mission_item()
 {
 	_navigator->get_mission_result()->finished = false;
-	_navigator->get_mission_result()->seq_current = _current_mission_index;
+	_navigator->get_mission_result()->seq_current = _mission.current_seq;
 
 	_navigator->set_mission_result_updated();
-
-	save_mission_state();
 }
 
 void
-Mission::check_mission_valid(bool force)
+Mission::check_mission_valid()
 {
-	if ((!_home_inited && _navigator->home_global_position_valid()) || force) {
 
-		MissionFeasibilityChecker _missionFeasibilityChecker(_navigator);
+	MissionFeasibilityChecker _missionFeasibilityChecker(_navigator);
 
-		_navigator->get_mission_result()->valid =
-			_missionFeasibilityChecker.checkMissionFeasible(_mission,
-					_param_mis_dist_1wp.get(),
-					_param_mis_dist_wps.get());
+	_navigator->get_mission_result()->valid =
+		_missionFeasibilityChecker.checkMissionFeasible(_mission,
+				_param_mis_dist_1wp.get(),
+				_param_mis_dist_wps.get());
 
-		_navigator->get_mission_result()->seq_total = _mission.count;
-		_navigator->increment_mission_instance_count();
-		_navigator->set_mission_result_updated();
-		_home_inited = _navigator->home_global_position_valid();
-
-		// find and store landing start marker (if available)
-		find_mission_land_start();
-	}
+	_navigator->get_mission_result()->seq_total = _mission.count;
+	_navigator->increment_mission_instance_count();
+	_navigator->set_mission_result_updated();
 }
 
 void
 Mission::reset_mission(struct mission_s &mission)
 {
-	dm_lock(DM_KEY_MISSION_STATE);
+	if (goToItem(0u, true) == EXIT_SUCCESS) {
+		resetMissionJumpCounter();
 
-	if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) {
-		if (mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 || mission.dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_1) {
-			/* set current item to 0 */
-			mission.current_seq = 0;
-
-			/* reset jump counters */
-			if (mission.count > 0) {
-				const dm_item_t dm_current = (dm_item_t)mission.dataman_id;
-
-				for (unsigned index = 0; index < mission.count; index++) {
-					struct mission_item_s item;
-					const ssize_t len = sizeof(struct mission_item_s);
-
-					if (dm_read(dm_current, index, &item, len) != len) {
-						PX4_WARN("could not read mission item during reset");
-						break;
-					}
-
-					if (item.nav_cmd == NAV_CMD_DO_JUMP) {
-						item.do_jump_current_count = 0;
-
-						if (dm_write(dm_current, index, &item, len) != len) {
-							PX4_WARN("could not save mission item during reset");
-							break;
-						}
-					}
-				}
-			}
-
-		} else {
-			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not read mission.\t");
-			events::send(events::ID("mission_cannot_read_mission"), events::Log::Error, "Could not read mission");
-
-			/* initialize mission state in dataman */
-			mission.timestamp = hrt_absolute_time();
-			mission.dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
-			mission.count = 0;
-			mission.current_seq = 0;
-		}
-
-		dm_write(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s));
+	} else {
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Could not read mission.\t");
+		events::send(events::ID("mission_cannot_read_mission"), events::Log::Error, "Could not read mission");
+		initMission();
 	}
 
-	dm_unlock(DM_KEY_MISSION_STATE);
+	check_mission_valid();
 }
 
 bool
@@ -1823,64 +1414,6 @@ Mission::need_to_reset_mission()
 	return false;
 }
 
-int32_t
-Mission::index_closest_mission_item() const
-{
-	int32_t min_dist_index(0);
-	float min_dist(FLT_MAX), dist_xy(FLT_MAX), dist_z(FLT_MAX);
-
-	dm_item_t dm_current = (dm_item_t)(_mission.dataman_id);
-
-	for (size_t i = 0; i < _mission.count; i++) {
-		struct mission_item_s missionitem = {};
-		const ssize_t len = sizeof(missionitem);
-
-		if (dm_read(dm_current, i, &missionitem, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			PX4_ERR("dataman read failure");
-			break;
-		}
-
-		if (item_contains_position(missionitem)) {
-			// do not consider land waypoints for a fw
-			if (!((missionitem.nav_cmd == NAV_CMD_LAND) &&
-			      (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
-			      (!_navigator->get_vstatus()->is_vtol))) {
-				float dist = get_distance_to_point_global_wgs84(missionitem.lat, missionitem.lon,
-						get_absolute_altitude_for_item(missionitem),
-						_navigator->get_global_position()->lat,
-						_navigator->get_global_position()->lon,
-						_navigator->get_global_position()->alt,
-						&dist_xy, &dist_z);
-
-				if (dist < min_dist) {
-					min_dist = dist;
-					min_dist_index = i;
-				}
-			}
-		}
-	}
-
-	// for mission reverse also consider the home position
-	if (_mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
-		float dist = get_distance_to_point_global_wgs84(
-				     _navigator->get_home_position()->lat,
-				     _navigator->get_home_position()->lon,
-				     _navigator->get_home_position()->alt,
-				     _navigator->get_global_position()->lat,
-				     _navigator->get_global_position()->lon,
-				     _navigator->get_global_position()->alt,
-				     &dist_xy, &dist_z);
-
-		if (dist < min_dist) {
-			min_dist = dist;
-			min_dist_index = -1;
-		}
-	}
-
-	return min_dist_index;
-}
-
 bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const
 {
 	return ((p1->valid == p2->valid) &&
@@ -1909,7 +1442,7 @@ void Mission::publish_navigator_mission_item()
 	navigator_mission_item_s navigator_mission_item{};
 
 	navigator_mission_item.instance_count = _navigator->mission_instance_count();
-	navigator_mission_item.sequence_current = _current_mission_index;
+	navigator_mission_item.sequence_current = _mission.current_seq;
 	navigator_mission_item.nav_cmd = _mission_item.nav_cmd;
 	navigator_mission_item.latitude = _mission_item.lat;
 	navigator_mission_item.longitude = _mission_item.lon;

src/modules/navigator/mission.h

@@ -52,6 +52,8 @@
 
 #include <float.h>
 
+#include "lib/mission/planned_mission_interface.h"
+
 #include <dataman/dataman.h>
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
@@ -68,12 +70,14 @@
 
 class Navigator;
 
-class Mission : public MissionBlock, public ModuleParams
+class Mission : public MissionBlock, public ModuleParams, protected PlannedMissionInterface
 {
 public:
 	Mission(Navigator *navigator);
 	~Mission() override = default;
 
+	void onMissionUpdate(bool has_mission_items_changed) override;
+
 	void on_inactive() override;
 	void on_inactivation() override;
 	void on_activation() override;
@@ -85,17 +89,17 @@ public:
 	bool landing();
 
 	uint16_t get_land_start_index() const { return _land_start_index; }
-	bool get_land_start_available() const { return _land_start_available; }
+	bool get_land_start_available() const { return _land_start_index != _invalid_index; }
 	bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; }
 	bool get_mission_changed() const { return _mission_changed ; }
 	bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; }
-	double get_landing_start_lat() { return _landing_start_lat; }
-	double get_landing_start_lon() { return _landing_start_lon; }
-	float get_landing_start_alt() { return _landing_start_alt; }
+	double get_landing_start_lat() { return _land_start_pos.lat; }
+	double get_landing_start_lon() { return _land_start_pos.lon; }
+	float get_landing_start_alt() { return _land_start_pos.alt; }
 
-	double get_landing_lat() { return _landing_lat; }
-	double get_landing_lon() { return _landing_lon; }
-	float get_landing_alt() { return _landing_alt; }
+	double get_landing_lat() { return _land_pos.lat; }
+	double get_landing_lon() { return _land_pos.lon; }
+	float get_landing_alt() { return _land_pos.alt; }
 	float get_landing_loiter_rad() { return _landing_loiter_radius; }
 
 	void set_closest_item_as_current();
@@ -107,9 +111,6 @@ public:
 	 */
 	void set_execution_mode(const uint8_t mode);
 private:
-
-	void mission_init();
-
 	/**
 	 * Update mission topic
 	 */
@@ -173,29 +174,6 @@ private:
 	 */
 	void do_abort_landing();
 
-	/**
-	 * Read the current and the next mission item. The next mission item read is the
-	 * next mission item that contains a position.
-	 *
-	 * @return true if current mission item available
-	 */
-	bool prepare_mission_items(mission_item_s *mission_item,
-				   mission_item_s *next_position_mission_item, bool *has_next_position_item,
-				   mission_item_s *next_next_position_mission_item = nullptr, bool *has_next_next_position_item = nullptr);
-
-	/**
-	 * Read current (offset == 0) or a specific (offset > 0) mission item
-	 * from the dataman and watch out for DO_JUMPS
-	 *
-	 * @return true if successful
-	 */
-	bool read_mission_item(int offset, struct mission_item_s *mission_item);
-
-	/**
-	 * Save current mission state to dataman
-	 */
-	void save_mission_state();
-
 	/**
 	 * Inform about a changed mission item after a DO_JUMP
 	 */
@@ -214,7 +192,7 @@ private:
 	/**
 	 * Check whether a mission is ready to go
 	 */
-	void check_mission_valid(bool force);
+	void check_mission_valid();
 
 	/**
 	 * Reset mission
@@ -226,16 +204,6 @@ private:
 	 */
 	bool need_to_reset_mission();
 
-	/**
-	 * Find and store the index of the landing sequence (DO_LAND_START)
-	 */
-	bool find_mission_land_start();
-
-	/**
-	 * Return the index of the closest mission item to the current global position.
-	 */
-	int32_t index_closest_mission_item() const;
-
 	bool position_setpoint_equal(const position_setpoint_s *p1, const position_setpoint_s *p2) const;
 
 	void publish_navigator_mission_item();
@@ -248,24 +216,12 @@ private:
 
 	uORB::Publication<navigator_mission_item_s> _navigator_mission_item_pub{ORB_ID::navigator_mission_item};
 
-	uORB::Subscription	_mission_sub{ORB_ID(mission)};		/**< mission subscription */
-	mission_s		_mission {};
-
-	int32_t _current_mission_index{-1};
-
-	// track location of planned mission landing
-	bool	_land_start_available{false};
-	uint16_t _land_start_index{UINT16_MAX};		/**< index of DO_LAND_START, INVALID_DO_LAND_START if no planned landing */
-	double _landing_start_lat{0.0};
-	double _landing_start_lon{0.0};
-	float _landing_start_alt{0.0f};
-
-	double _landing_lat{0.0};
-	double _landing_lon{0.0};
-	float _landing_alt{0.0f};
-
 	float _landing_loiter_radius{0.f};
 
+	bool _is_current_planned_mission_item_valid{false};
+
+	bool _initialized_mission_checked{false};
+
 	bool _need_takeoff{true};					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
 
 	hrt_abstime _time_mission_deactivated{0};
@@ -275,8 +231,6 @@ private:
 		MISSION_TYPE_MISSION
 	} _mission_type{MISSION_TYPE_NONE};
 
-	bool _inited{false};
-	bool _home_inited{false};
 	bool _need_mission_reset{false};
 	bool _mission_waypoints_changed{false};
 	bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */

src/modules/navigator/mission_block.cpp

@@ -924,9 +924,14 @@ MissionBlock::mission_apply_limitation(mission_item_s &item)
 
 float
 MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const
+{
+	return get_absolute_altitude_for_item(mission_item, _navigator->get_home_position()->alt);
+}
+
+float MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item, float home_alt)
 {
 	if (mission_item.altitude_is_relative) {
-		return mission_item.altitude + _navigator->get_home_position()->alt;
+		return mission_item.altitude + home_alt;
 
 	} else {
 		return mission_item.altitude;

src/modules/navigator/mission_block.h

@@ -99,6 +99,8 @@ public:
 	 */
 	static bool item_contains_marker(const mission_item_s &item);
 
+	static float get_absolute_altitude_for_item(const  mission_item_s &mission_item, float home_alt);
+
 	/**
 	 * @brief Set the payload deployment successful flag object
 	 *

src/modules/navigator/navigator_mode.h

@@ -54,6 +54,8 @@ public:
 
 	void run(bool active);
 
+	bool isActive() {return _active;};
+
 	/**
 	 * This function is called while the mode is inactive
 	 */