navigator: wrote unit tests for Takeoff navigation mode

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/navigator/CMakeLists.txt

@@ -51,6 +51,7 @@ px4_add_module(
 		enginefailure.cpp
 		gpsfailure.cpp
 		follow_target.cpp
+		NavigatorCore.cpp
 	DEPENDS
 		git_ecl
 		ecl_geo
@@ -59,4 +60,5 @@ px4_add_module(
 		motion_planning
 	)
 
-px4_add_functional_gtest(SRC RangeRTLTest.cpp LINKLIBS modules__navigator modules__dataman)
+px4_add_functional_gtest(SRC RangeRTLTest.cpp LINKLIBS modules__navigator modules__dataman)
+px4_add_functional_gtest(SRC NavigatorModeTakeoffTest.cpp LINKLIBS modules__navigator modules__dataman)

src/modules/navigator/FakeNavigator.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#define MODULE_NAME "navigator"
+
+#include <navigator/navigator.h>
+
+
+class FakeNavigator : public Navigator
+{
+public:
+	FakeNavigator() :
+		Navigator() {};
+
+	virtual ~FakeNavigator() {};
+
+private:
+};

src/modules/navigator/GeofenceBreachAvoidance/GeofenceBreachAvoidanceTest.cpp

@@ -34,7 +34,7 @@
 #include <gtest/gtest.h>
 #include "geofence_breach_avoidance.h"
 #include "fake_geofence.hpp"
-#include "dataman_mocks.hpp"
+#include <dataman/dataman_mocks.hpp>
 #include <parameters/param.h>
 
 using namespace matrix;

src/modules/navigator/NavigatorCore.cpp

@@ -0,0 +1,131 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2021 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 NavigatorCore.cpp
+ *
+ */
+
+#include "NavigatorCore.h"
+#include <px4_platform_common/defines.h>
+
+
+NavigatorCore::NavigatorCore() :
+	ModuleParams(nullptr)
+{
+
+}
+
+NavigatorCore::~NavigatorCore()
+{
+
+}
+
+bool NavigatorCore::forceVTOL()
+{
+
+	return _status.is_vtol && (isRotaryWing() || _status.in_transition_to_fw) && _param_nav_force_vt.get();
+}
+
+float NavigatorCore::getHorAcceptanceRadiusMeter()
+{
+	if (isRotaryWing()) {
+		return getDefaultHorAcceptanceRadiusMeter();
+
+	} else {
+		return math::max(_pos_ctrl_status.acceptance_radius, getDefaultHorAcceptanceRadiusMeter());
+	}
+}
+
+float NavigatorCore::getAltAcceptanceRadMeter()
+{
+	if (isFixedWing()) {
+		const position_setpoint_s &next_sp =  getCurrentTriplet().next;
+
+		if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) {
+			// Use separate (tighter) altitude acceptance for clean altitude starting point before landing
+			return getFixedWingLandingAltAcceptanceRadius();
+		}
+	}
+
+	return getDefaultAltAcceptanceRadiusMeter();
+}
+
+float NavigatorCore::getDefaultAltAcceptanceRadiusMeter()
+{
+
+	if (isFixedWing()) {
+		return getFixedWingAltAcceptanceRadiusMeter();
+
+	} else if (isRover()) {
+		return INFINITY;
+
+	} else {
+		float alt_acceptance_radius = getMulticopterAltAcceptanceRadiusMeter();
+
+		if ((_pos_ctrl_status.timestamp > getCurrentTriplet().timestamp)
+		    && _pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) {
+			alt_acceptance_radius = _pos_ctrl_status.altitude_acceptance;
+		}
+
+		return alt_acceptance_radius;
+	}
+}
+
+float NavigatorCore::getAcceptanceRadiusMeter()
+{
+
+	float acceptance_radius = _param_nav_acc_rad.get();
+
+	// for fixed-wing and rover, return the max of NAV_ACC_RAD and the controller acceptance radius (e.g. L1 distance)
+	if (!isRotaryWing() && PX4_ISFINITE(_pos_ctrl_status.acceptance_radius) && _pos_ctrl_status.timestamp != 0) {
+
+		acceptance_radius = math::max(acceptance_radius, _pos_ctrl_status.acceptance_radius);
+	}
+
+	return acceptance_radius;
+}
+
+float NavigatorCore::getAltAcceptanceRadiusMeter()
+{
+
+	if (isFixedWing()) {
+		const position_setpoint_s &next_sp = _triplet.next;
+
+		if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) {
+			// Use separate (tighter) altitude acceptance for clean altitude starting point before landing
+			return _param_nav_fw_altl_rad.get();
+		}
+	}
+
+	return getDefaultAltAcceptanceRadiusMeter();
+}

src/modules/navigator/NavigatorCore.h

@@ -0,0 +1,157 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+#include <px4_platform_common/module_params.h>
+
+#include <lib/mathlib/mathlib.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/position_controller_status.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+
+
+class NavigatorCore : public ModuleParams
+{
+
+public:
+	NavigatorCore();
+
+	~NavigatorCore();
+
+	typedef matrix::Vector3<float> Vector3f;
+	typedef matrix::Vector2<double> Vector2d;
+
+	bool getLanded() { return _landed_state.landed; }
+	bool getMaybeLanded() { return _landed_state.maybe_landed; }
+	float getLandDetectedAltMaxMeter() { return _landed_state.alt_max; }
+	double getLatRad() { return _global_pos.lat; }
+	double getLonRad() { return _global_pos.lon;}
+	float getAltitudeAMSLMeters() { return _global_pos.alt; }
+	float getTrueHeadingRad() { return _local_pos.heading; }
+	double getHomeLatRad() { return _home.lat; }
+	double getHomeLonRad() { return _home.lon; }
+	float getHomeAltAMSLMeter() { return _home.alt; }
+	float getHomeTrueHeadingRad() { return _home.yaw; }
+	home_position_s &getHomePosition() { return _home; }
+
+	bool isNotArmed() { return _status.arming_state != vehicle_status_s::ARMING_STATE_ARMED; }
+	uint8_t getArmingState() { return _status.arming_state; }
+
+
+	float getPosNorthMeter() { return _local_pos.x; }
+	float getPosEastMeter() { return _local_pos.y; }
+	float getPosDownMeter() { return _local_pos.z; }
+
+	float getVelNorthMPS() { return _local_pos.vx; }
+	float getVelEastMPS() { return _local_pos.vy; }
+	float getVelDownMPS() { return _local_pos.vz; }
+
+	float getLoiterRadiusMeter() { return _param_nav_loiter_rad.get(); }
+
+	bool getIsInTransitionMode() { return _status.in_transition_mode; }
+
+	position_setpoint_triplet_s getCurrentTriplet() { return _triplet; }
+
+	bool isHomeValid() { return (_home.timestamp > 0 && _home.valid_alt && _home.valid_hpos && _home.valid_lpos); }
+	bool isHomeAltValid() { return (_home.timestamp > 0 && _home.valid_alt);  }
+
+	bool isRotaryWing() { return _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; }
+	bool isFixedWing() { return _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING; }
+	bool isRover() { return _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER; }
+	bool isVTOL() { return _status.is_vtol; }
+	uint8_t getVehicleType() { return _status.vehicle_type; }
+
+	float getDefaultAltAcceptanceRadiusMeter();
+	float getAltAcceptanceRadMeter();
+	float getFixedWingLandingAltAcceptanceRadius() { return _param_nav_fw_altl_rad.get(); }
+	float getDefaultHorAcceptanceRadiusMeter() { return _param_nav_acc_rad.get(); }
+	float getHorAcceptanceRadiusMeter();
+	float getMulticopterAltAcceptanceRadiusMeter() { return _param_nav_mc_alt_rad.get(); }
+	float getFixedWingAltAcceptanceRadiusMeter() { return _param_nav_fw_alt_rad.get(); }
+
+	float getAcceptanceRadiusMeter();
+	float getAltAcceptanceRadiusMeter();
+
+	float getRelativeTakeoffMinAltitudeMeter() { return _param_mis_takeoff_alt.get(); }
+	float getRelativeLoiterMinAltitudeMeter() { return _param_mis_ltrmin_alt.get(); }
+	bool isTakeoffRequired() { return _param_mis_takeoff_req.get(); }
+	float getWaypointHeadingTimeoutSeconds() { return _param_mis_yaw_tmt.get(); }
+	float getWaypointHeadingAcceptanceRad() { return _param_mis_yaw_err.get(); }
+
+
+	bool forceVTOL();
+
+	void updateLocalPosition(const vehicle_local_position_s &local_pos) { _local_pos = local_pos; };
+	void updateVehicleStatus(const vehicle_status_s &status) { _status = status; }
+	void updateGlobalPosition(const vehicle_global_position_s &global_pos) { _global_pos = global_pos; }
+	void updateLandedState(const vehicle_land_detected_s &landed_state) { _landed_state = landed_state; }
+	void updateHomePosition(const home_position_s &home) { _home = home; }
+	void updatePositionControllerStatus(const position_controller_status_s &pos_ctrl_status) { _pos_ctrl_status = pos_ctrl_status; }
+
+private:
+
+	vehicle_local_position_s _local_pos{};
+	vehicle_status_s _status{};
+	vehicle_global_position_s _global_pos{};
+	vehicle_land_detected_s _landed_state{};
+	home_position_s _home{};
+	position_controller_status_s _pos_ctrl_status{};
+
+	position_setpoint_triplet_s _triplet{};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,	/**< loiter radius for fixedwing */
+		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,	/**< acceptance for takeoff */
+		(ParamFloat<px4::params::NAV_FW_ALT_RAD>)
+		_param_nav_fw_alt_rad,	/**< acceptance radius for fixedwing altitude */
+		(ParamFloat<px4::params::NAV_FW_ALTL_RAD>)
+		_param_nav_fw_altl_rad,	/**< acceptance radius for fixedwing altitude before landing*/
+		(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
+		_param_nav_mc_alt_rad,	/**< acceptance radius for multicopter altitude */
+		(ParamInt<px4::params::NAV_FORCE_VT>) _param_nav_force_vt,	/**< acceptance radius for multicopter altitude */
+		(ParamInt<px4::params::NAV_TRAFF_AVOID>) _param_nav_traff_avoid,	/**< avoiding other aircraft is enabled */
+		(ParamFloat<px4::params::NAV_TRAFF_A_RADU>) _param_nav_traff_a_radu,	/**< avoidance Distance Unmanned*/
+		(ParamFloat<px4::params::NAV_TRAFF_A_RADM>) _param_nav_traff_a_radm,	/**< avoidance Distance Manned*/
+
+		// non-navigator parameters
+		// Mission (MIS_*)
+		(ParamFloat<px4::params::MIS_LTRMIN_ALT>) _param_mis_ltrmin_alt,
+		(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
+		(ParamBool<px4::params::MIS_TAKEOFF_REQ>) _param_mis_takeoff_req,
+		(ParamFloat<px4::params::MIS_YAW_TMT>) _param_mis_yaw_tmt,
+		(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err
+	)
+};

src/modules/navigator/NavigatorModeTakeoffTest.cpp

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+#include <gtest/gtest.h>
+#include "FakeNavigator.h"
+#include "takeoff.h"
+#include <dataman/dataman_mocks.hpp>
+
+
+TEST(NavigatorModeTakeoffTest, TestTakeoff)
+{
+	FakeNavigator fake_navigator;
+
+	NavigatorCore &navigator_core = fake_navigator.getCore();
+	Takeoff takeoff(&fake_navigator, navigator_core);
+	fake_navigator.params_update();
+
+	vehicle_status_s status = {};
+	status.timestamp = 1e6;
+	status.is_vtol = true;
+	status.arming_state = vehicle_status_s::ARMING_STATE_ARMED;
+	status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+
+	vehicle_global_position_s global_pos = {};
+	global_pos.timestamp = 1e6;
+	global_pos.lat = 40;
+	global_pos.lon = 3;
+	global_pos.alt = 300;
+
+	vehicle_land_detected_s landed = {};
+	landed.landed = true;
+	landed.timestamp = 1e6;
+
+	home_position_s home = {};
+	home.timestamp = 1e6;
+	home.lat = global_pos.lat;
+	home.lon = global_pos.lon;
+	home.alt = global_pos.alt;
+	home.valid_alt = true;
+	home.valid_hpos = true;
+	home.valid_lpos = true;
+
+	vehicle_local_position_s local_pos = {};
+	local_pos.timestamp = 1e6;
+	local_pos.x = -0.2f;
+	local_pos.y = 0.1f;
+	local_pos.z = 0.1f;
+
+	local_pos.vx = local_pos.vy = local_pos.vz = 0;
+	local_pos.heading = -0.54f;
+
+	navigator_core.updateGlobalPosition(global_pos);
+	navigator_core.updateHomePosition(home);
+	navigator_core.updateLandedState(landed);
+	navigator_core.updateLocalPosition(local_pos);
+	navigator_core.updateVehicleStatus(status);
+
+	takeoff.on_activation();
+
+	position_setpoint_s current = fake_navigator.get_position_setpoint_triplet()->current;
+
+	ASSERT_EQ(current.lat, navigator_core.getLatRad());
+	ASSERT_EQ(current.lon, navigator_core.getLonRad());
+	ASSERT_EQ(current.type, 3); // position_setpoint_s::TYPE_TAKEOFF
+	ASSERT_EQ(current.valid, true);
+	ASSERT_EQ(current.alt_valid, true);
+	ASSERT_EQ(current.alt, global_pos.alt + navigator_core.getRelativeTakeoffMinAltitudeMeter());
+	ASSERT_EQ(current.yaw, navigator_core.getTrueHeadingRad());
+	ASSERT_EQ(current.yaw_valid, true);
+	ASSERT_EQ(current.yawspeed_valid, false);
+	ASSERT_EQ(current.loiter_radius, navigator_core.getLoiterRadiusMeter());
+	ASSERT_EQ(current.loiter_direction, 1);
+	ASSERT_EQ(current.acceptance_radius, navigator_core.getHorAcceptanceRadiusMeter());
+	ASSERT_EQ(current.cruising_speed, -1.0f);
+
+	uint64_t t_first = current.timestamp;
+
+	landed.landed = false;
+	navigator_core.updateLandedState(landed);
+	global_pos.alt += navigator_core.getRelativeTakeoffMinAltitudeMeter();
+	navigator_core.updateGlobalPosition(global_pos);
+
+	takeoff.on_active();
+	current = fake_navigator.get_position_setpoint_triplet()->current;
+
+	ASSERT_NE(t_first, current.timestamp);
+	ASSERT_EQ(current.type, 2);
+
+
+
+
+}

src/modules/navigator/RangeRTLTest.cpp

@@ -11,7 +11,8 @@
 TEST(Navigator_and_RTL, interact_correctly)
 {
 	Navigator n;
-	RTL rtl(&n);
+	NavigatorCore navigator_core;
+	RTL rtl(&n, navigator_core);
 
 
 	home_position_s home_pos{};

src/modules/navigator/enginefailure.cpp

@@ -52,8 +52,8 @@
 #include "navigator.h"
 #include "enginefailure.h"
 
-EngineFailure::EngineFailure(Navigator *navigator) :
-	MissionBlock(navigator),
+EngineFailure::EngineFailure(Navigator *navigator, NavigatorCore &navigator_core) :
+	MissionBlock(navigator, navigator_core),
 	_ef_state(EF_STATE_NONE)
 {
 }
@@ -93,15 +93,15 @@ EngineFailure::set_ef_item()
 	case EF_STATE_LOITERDOWN: {
 			//XXX create mission item at ground (below?) here
 
-			_mission_item.lat = _navigator->get_global_position()->lat;
-			_mission_item.lon = _navigator->get_global_position()->lon;
+			_mission_item.lat = _navigator_core.getLatRad();
+			_mission_item.lon = _navigator_core.getLonRad();
 			_mission_item.altitude_is_relative = false;
 			//XXX setting altitude to a very low value, evaluate other options
-			_mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f;
+			_mission_item.altitude = _navigator_core.getHomeAltAMSLMeter() - 1000.0f;
 			_mission_item.yaw = NAN;
-			_mission_item.loiter_radius = _navigator->get_loiter_radius();
+			_mission_item.loiter_radius = _navigator_core.getLoiterRadiusMeter();
 			_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+			_mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter();
 			_mission_item.autocontinue = true;
 			_mission_item.origin = ORIGIN_ONBOARD;
 

src/modules/navigator/enginefailure.h

@@ -47,7 +47,7 @@ class Navigator;
 class EngineFailure : public MissionBlock
 {
 public:
-	EngineFailure(Navigator *navigator);
+	EngineFailure(Navigator *navigator, NavigatorCore &navigator_core);
 	~EngineFailure() = default;
 
 	void on_inactive() override;

src/modules/navigator/follow_target.cpp

@@ -60,8 +60,8 @@ using namespace matrix;
 
 constexpr float FollowTarget::_follow_position_matricies[4][9];
 
-FollowTarget::FollowTarget(Navigator *navigator) :
-	MissionBlock(navigator),
+FollowTarget::FollowTarget(Navigator *navigator, NavigatorCore &navigator_core) :
+	MissionBlock(navigator, navigator_core),
 	ModuleParams(navigator)
 {
 	_current_vel.zero();
@@ -134,7 +134,7 @@ void FollowTarget::on_active()
 
 		// get distance to target
 
-		map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+		map_projection_init(&target_ref, _navigator_core.getLatRad(), _navigator_core.getLonRad());
 		map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0),
 				       &_target_distance(1));
 
@@ -200,12 +200,12 @@ void FollowTarget::on_active()
 				// this really needs to control the yaw rate directly in the attitude pid controller
 				// but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode
 
-				_yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
-						_navigator->get_global_position()->lon,
+				_yaw_angle = get_bearing_to_next_waypoint(_navigator_core.getLatRad(),
+						_navigator_core.getLonRad(),
 						_current_target_motion.lat,
 						_current_target_motion.lon);
 
-				_yaw_rate = wrap_pi((_yaw_angle - _navigator->get_local_position()->heading) / (dt_ms / 1000.0f));
+				_yaw_rate = wrap_pi((_yaw_angle - _navigator_core.getTrueHeadingRad()) / (dt_ms / 1000.0f));
 
 			} else {
 				_yaw_angle = _yaw_rate = NAN;
@@ -238,7 +238,7 @@ void FollowTarget::on_active()
 	// 3 degrees of facing target
 
 	if (PX4_ISFINITE(_yaw_rate)) {
-		if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_local_position()->heading)) < math::radians(3.0F)) {
+		if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator_core.getTrueHeadingRad())) < math::radians(3.0F)) {
 			_yaw_rate = NAN;
 		}
 	}
@@ -298,8 +298,8 @@ void FollowTarget::on_active()
 
 			// for now set the target at the minimum height above the uav
 
-			target.lat = _navigator->get_global_position()->lat;
-			target.lon = _navigator->get_global_position()->lon;
+			target.lat = _navigator_core.getLatRad();
+			target.lon = _navigator_core.getLonRad();
 			target.alt = 0.0F;
 
 			set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target, _yaw_angle);
@@ -376,7 +376,7 @@ void
 FollowTarget::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target,
 				     float yaw)
 {
-	if (_navigator->get_land_detected()->landed) {
+	if (_navigator_core.getLanded()) {
 		/* landed, don't takeoff, but switch to IDLE mode */
 		item->nav_cmd = NAV_CMD_IDLE;
 
@@ -387,7 +387,8 @@ FollowTarget::set_follow_target_item(struct mission_item_s *item, float min_clea
 		/* use current target position */
 		item->lat = target.lat;
 		item->lon = target.lon;
-		item->altitude = _navigator->get_home_position()->alt;
+		item->altitude = _navigator_core.getHomeAltAMSLMeter();
+
 
 		if (min_clearance > 8.0f) {
 			item->altitude += min_clearance;
@@ -399,8 +400,8 @@ FollowTarget::set_follow_target_item(struct mission_item_s *item, float min_clea
 
 	item->altitude_is_relative = false;
 	item->yaw = yaw;
-	item->loiter_radius = _navigator->get_loiter_radius();
-	item->acceptance_radius = _navigator->get_acceptance_radius();
+	item->loiter_radius = _navigator_core.getLoiterRadiusMeter();
+	item->acceptance_radius = _navigator_core.getAcceptanceRadiusMeter();
 	item->time_inside = 0.0f;
 	item->autocontinue = false;
 	item->origin = ORIGIN_ONBOARD;

src/modules/navigator/follow_target.h

@@ -54,7 +54,7 @@ class FollowTarget : public MissionBlock, public ModuleParams
 {
 
 public:
-	FollowTarget(Navigator *navigator);
+	FollowTarget(Navigator *navigator, NavigatorCore &navigator_core);
 	~FollowTarget() = default;
 
 	void on_inactive() override;

src/modules/navigator/geofence.cpp

@@ -224,13 +224,13 @@ bool Geofence::isCloserThanMaxDistToHome(double lat, double lon, float altitude)
 {
 	bool inside_fence = true;
 
-	if (isHomeRequired() && _navigator->home_position_valid()) {
+	if (isHomeRequired() && _navigator->getCore().isHomeValid()) {
 
 		const float max_horizontal_distance = _param_gf_max_hor_dist.get();
 
-		const double home_lat = _navigator->get_home_position()->lat;
-		const double home_lon = _navigator->get_home_position()->lon;
-		const float home_alt = _navigator->get_home_position()->alt;
+		const double home_lat = _navigator->getCore().getHomeLatRad();
+		const double home_lon = _navigator->getCore().getHomeLonRad();
+		const float home_alt = _navigator->getCore().getHomeAltAMSLMeter();
 
 		float dist_xy = -1.0f;
 		float dist_z = -1.0f;
@@ -255,10 +255,10 @@ bool Geofence::isBelowMaxAltitude(float altitude)
 {
 	bool inside_fence = true;
 
-	if (isHomeRequired() && _navigator->home_position_valid()) {
+	if (isHomeRequired() && _navigator->getCore().isHomeValid()) {
 
 		const float max_vertical_distance = _param_gf_max_ver_dist.get();
-		const float home_alt = _navigator->get_home_position()->alt;
+		const float home_alt = _navigator->getCore().getHomeAltAMSLMeter();
 
 		float dist_z = altitude - home_alt;
 

src/modules/navigator/gpsfailure.cpp

@@ -52,8 +52,8 @@
 using matrix::Eulerf;
 using matrix::Quatf;
 
-GpsFailure::GpsFailure(Navigator *navigator) :
-	MissionBlock(navigator),
+GpsFailure::GpsFailure(Navigator *navigator, NavigatorCore &navigator_core) :
+	MissionBlock(navigator, navigator_core),
 	ModuleParams(navigator)
 {
 }
@@ -92,7 +92,7 @@ GpsFailure::on_active()
 			Quatf q(Eulerf(att_sp.roll_body, att_sp.pitch_body, 0.0f));
 			q.copyTo(att_sp.q_d);
 
-			if (_navigator->get_vstatus()->is_vtol) {
+			if (_navigator_core.isVTOL()) {
 				_fw_virtual_att_sp_pub.publish(att_sp);
 
 			} else {

src/modules/navigator/gpsfailure.h

@@ -51,7 +51,7 @@ class Navigator;
 class GpsFailure : public MissionBlock, public ModuleParams
 {
 public:
-	GpsFailure(Navigator *navigator);
+	GpsFailure(Navigator *navigator, NavigatorCore &navigator_core);
 	~GpsFailure() = default;
 
 	void on_inactive() override;

src/modules/navigator/land.cpp

@@ -41,8 +41,8 @@
 #include "land.h"
 #include "navigator.h"
 
-Land::Land(Navigator *navigator) :
-	MissionBlock(navigator)
+Land::Land(Navigator *navigator, NavigatorCore &navigator_core) :
+	MissionBlock(navigator, navigator_core)
 {
 }
 
@@ -71,16 +71,16 @@ void
 Land::on_active()
 {
 	/* for VTOL update landing location during back transition */
-	if (_navigator->get_vstatus()->is_vtol &&
-	    _navigator->get_vstatus()->in_transition_mode) {
+	if (_navigator_core.isVTOL() &&
+	    _navigator_core.getIsInTransitionMode()) {
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
+		pos_sp_triplet->current.lat = _navigator_core.getLatRad();
+		pos_sp_triplet->current.lon = _navigator_core.getLonRad();
 		_navigator->set_position_setpoint_triplet_updated();
 	}
 
 
-	if (_navigator->get_land_detected()->landed) {
+	if (_navigator_core.getLanded()) {
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
 		set_idle_item(&_mission_item);

src/modules/navigator/land.h

@@ -46,7 +46,7 @@
 class Land : public MissionBlock
 {
 public:
-	Land(Navigator *navigator);
+	Land(Navigator *navigator, NavigatorCore &navigator_core);
 	~Land() = default;
 
 	void on_activation() override;

src/modules/navigator/loiter.cpp

@@ -42,8 +42,8 @@
 #include "loiter.h"
 #include "navigator.h"
 
-Loiter::Loiter(Navigator *navigator) :
-	MissionBlock(navigator),
+Loiter::Loiter(Navigator *navigator, NavigatorCore &navigator_core) :
+	MissionBlock(navigator, navigator_core),
 	ModuleParams(navigator)
 {
 }
@@ -73,7 +73,7 @@ Loiter::on_active()
 	}
 
 	// reset the loiter position if we get disarmed
-	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+	if (_navigator_core.isNotArmed()) {
 		_loiter_pos_set = false;
 	}
 }
@@ -81,8 +81,8 @@ Loiter::on_active()
 void
 Loiter::set_loiter_position()
 {
-	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED &&
-	    _navigator->get_land_detected()->landed) {
+	if (_navigator_core.isNotArmed() &&
+	    _navigator_core.getLanded()) {
 
 		// Not setting loiter position if disarmed and landed, instead mark the current
 		// setpoint as invalid and idle (both, just to be sure).
@@ -101,7 +101,8 @@ Loiter::set_loiter_position()
 	_loiter_pos_set = true;
 
 	// set current mission item to loiter
-	set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt());
+	set_loiter_item(&_mission_item, _navigator_core.getRelativeLoiterMinAltitudeMeter());
+
 
 	// convert mission item to current setpoint
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -119,7 +120,7 @@ void
 Loiter::reposition()
 {
 	// we can't reposition if we are not armed yet
-	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
+	if (_navigator_core.isNotArmed()) {
 		return;
 	}
 
@@ -131,10 +132,10 @@ Loiter::reposition()
 		// convert mission item to current setpoint
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		pos_sp_triplet->current.velocity_valid = false;
-		pos_sp_triplet->previous.yaw = _navigator->get_local_position()->heading;
-		pos_sp_triplet->previous.lat = _navigator->get_global_position()->lat;
-		pos_sp_triplet->previous.lon = _navigator->get_global_position()->lon;
-		pos_sp_triplet->previous.alt = _navigator->get_global_position()->alt;
+		pos_sp_triplet->previous.yaw = _navigator_core.getTrueHeadingRad();
+		pos_sp_triplet->previous.lat = _navigator_core.getLatRad();
+		pos_sp_triplet->previous.lon = _navigator_core.getLonRad();
+		pos_sp_triplet->previous.alt = _navigator_core.getAltitudeAMSLMeters();
 		memcpy(&pos_sp_triplet->current, &rep->current, sizeof(rep->current));
 		pos_sp_triplet->next.valid = false;
 

src/modules/navigator/loiter.h

@@ -48,7 +48,7 @@
 class Loiter : public MissionBlock, public ModuleParams
 {
 public:
-	Loiter(Navigator *navigator);
+	Loiter(Navigator *navigator, NavigatorCore &navigator_core);
 	~Loiter() = default;
 
 	void on_inactive() override;

src/modules/navigator/mission.cpp

@@ -62,8 +62,8 @@
 
 using namespace time_literals;
 
-Mission::Mission(Navigator *navigator) :
-	MissionBlock(navigator),
+Mission::Mission(Navigator *navigator, NavigatorCore &navigator_core) :
+	MissionBlock(navigator, navigator_core),
 	ModuleParams(navigator)
 {
 }
@@ -83,7 +83,7 @@ Mission::on_inactive()
 	}
 
 	/* Without home a mission can't be valid yet anyway, let's wait. */
-	if (!_navigator->home_position_valid()) {
+	if (!_navigator_core.isHomeValid()) {
 		return;
 	}
 
@@ -131,7 +131,7 @@ Mission::on_inactive()
 	}
 
 	/* require takeoff after non-loiter or landing */
-	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_land_detected()->landed) {
+	if (!_navigator->get_can_loiter_at_sp() || _navigator_core.getLanded()) {
 		_need_takeoff = true;
 	}
 
@@ -252,7 +252,7 @@ Mission::on_active()
 
 	/* see if we need to update the current yaw heading */
 	if (!_param_mis_mnt_yaw_ctl.get()
-	    && (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
+	    && (_navigator_core.isRotaryWing())
 	    && (_navigator->get_vroi().mode != vehicle_roi_s::ROI_NONE)
 	    && !(_mission_item.nav_cmd == NAV_CMD_TAKEOFF
 		 || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
@@ -330,9 +330,9 @@ Mission::set_execution_mode(const uint8_t mode)
 		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD:
 			if (mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE) {
 				// command a transition if in vtol mc mode
-				if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
-				    _navigator->get_vstatus()->is_vtol &&
-				    !_navigator->get_land_detected()->landed) {
+				if (_navigator_core.isRotaryWing() &&
+				    _navigator_core.isVTOL() &&
+				    !_navigator_core.getLanded()) {
 
 					set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
 
@@ -418,16 +418,16 @@ Mission::find_mission_land_start()
 			_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;
+					     _navigator_core.getHomeAltAMSLMeter() : missionitem.altitude;
 			_land_start_available = true;
 		}
 
-		if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) ||
+		if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator_core.isVTOL()) ||
 		    (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 :
+			_landing_alt = missionitem.altitude_is_relative ?	missionitem.altitude + _navigator_core.getHomeAltAMSLMeter() :
 				       missionitem.altitude;
 
 			// don't have a valid land start yet, use the landing item itself then
@@ -529,7 +529,7 @@ Mission::update_mission()
 		 * 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) {
+		    !_navigator_core.getLanded()) {
 			_mission_waypoints_changed = true;
 		}
 
@@ -640,7 +640,7 @@ Mission::set_mission_items()
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
 
-			if (_navigator->get_land_detected()->landed) {
+			if (_navigator_core.getLanded()) {
 				mavlink_log_info(_navigator->get_mavlink_log_pub(),
 						 _mission_execution_mode == mission_result_s::MISSION_EXECUTION_MODE_REVERSE ? "Reverse Mission finished, landed" :
 						 "Mission finished, landed.");
@@ -661,7 +661,7 @@ Mission::set_mission_items()
 		_mission_type = MISSION_TYPE_NONE;
 
 		/* set loiter mission item and ensure that there is a minimum clearance from home */
-		set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt());
+		set_loiter_item(&_mission_item, _navigator_core.getRelativeTakeoffMinAltitudeMeter());
 
 		/* update position setpoint triplet  */
 		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -683,7 +683,7 @@ Mission::set_mission_items()
 			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
 			 */
 
-			if (_navigator->get_land_detected()->landed) {
+			if (_navigator_core.getLanded()) {
 				/* landed, refusing to take off without a mission */
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "No valid mission available, refusing takeoff.");
 
@@ -711,7 +711,7 @@ Mission::set_mission_items()
 		case mission_result_s::MISSION_EXECUTION_MODE_NORMAL:
 		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: {
 				/* force vtol land */
-				if (_navigator->force_vtol() && _mission_item.nav_cmd == NAV_CMD_LAND) {
+				if (_navigator_core.forceVTOL() && _mission_item.nav_cmd == NAV_CMD_LAND) {
 					_mission_item.nav_cmd = NAV_CMD_VTOL_LAND;
 				}
 
@@ -733,13 +733,13 @@ Mission::set_mission_items()
 					float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
 
 					mavlink_log_info(_navigator->get_mavlink_log_pub(), "Takeoff to %.1f meters above home.",
-							 (double)(takeoff_alt - _navigator->get_home_position()->alt));
+							 (double)(takeoff_alt - _navigator_core.getHomeAltAMSLMeter()));
 
 					_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
-					_mission_item.lat = _navigator->get_global_position()->lat;
-					_mission_item.lon = _navigator->get_global_position()->lon;
+					_mission_item.lat = _navigator_core.getLatRad();
+					_mission_item.lon = _navigator_core.getLonRad();
 					/* hold heading for takeoff items */
-					_mission_item.yaw = _navigator->get_local_position()->heading;
+					_mission_item.yaw = _navigator_core.getTrueHeadingRad();
 					_mission_item.altitude = takeoff_alt;
 					_mission_item.altitude_is_relative = false;
 					_mission_item.autocontinue = true;
@@ -748,7 +748,7 @@ Mission::set_mission_items()
 				} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
 					   && _work_item_type == WORK_ITEM_TYPE_DEFAULT
 					   && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
-					   && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+					   && _navigator_core.isRotaryWing()) {
 
 					/* if there is no need to do a takeoff but we have a takeoff item, treat is as waypoint */
 					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@@ -759,7 +759,7 @@ Mission::set_mission_items()
 					   && _work_item_type == WORK_ITEM_TYPE_DEFAULT
 					   && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
 
-					if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+					if (_navigator_core.isRotaryWing()) {
 						/* haven't transitioned yet, trigger vtol takeoff logic below */
 						_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
 
@@ -786,15 +786,15 @@ Mission::set_mission_items()
 				if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF &&
 				    _work_item_type == WORK_ITEM_TYPE_TAKEOFF &&
 				    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) {
+				    _navigator_core.isRotaryWing() &&
+				    !_navigator_core.getLanded()) {
 
 					/* disable weathervane before front transition for allowing yaw to align */
 					pos_sp_triplet->current.disable_weather_vane = true;
 
 					/* set yaw setpoint to heading of VTOL_TAKEOFF wp against current position */
 					_mission_item.yaw = get_bearing_to_next_waypoint(
-								    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+								    _navigator_core.getLatRad(), _navigator_core.getLonRad(),
 								    _mission_item.lat, _mission_item.lon);
 
 					_mission_item.force_heading = true;
@@ -802,15 +802,15 @@ Mission::set_mission_items()
 					new_work_item_type = WORK_ITEM_TYPE_ALIGN;
 
 					/* set position setpoint to current while aligning */
-					_mission_item.lat = _navigator->get_global_position()->lat;
-					_mission_item.lon = _navigator->get_global_position()->lon;
+					_mission_item.lat = _navigator_core.getLatRad();
+					_mission_item.lon = _navigator_core.getLonRad();
 				}
 
 				/* heading is aligned now, prepare transition */
 				if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF &&
 				    _work_item_type == WORK_ITEM_TYPE_ALIGN &&
-				    _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
-				    !_navigator->get_land_detected()->landed) {
+				    _navigator_core.isRotaryWing() &&
+				    !_navigator_core.getLanded()) {
 
 					/* re-enable weather vane again after alignment */
 					pos_sp_triplet->current.disable_weather_vane = false;
@@ -821,7 +821,7 @@ Mission::set_mission_items()
 					}
 
 					set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
-					_mission_item.yaw = _navigator->get_local_position()->heading;
+					_mission_item.yaw = _navigator_core.getTrueHeadingRad();
 
 					/* set position setpoint to target during the transition */
 					generate_waypoint_from_heading(&pos_sp_triplet->current, _mission_item.yaw);
@@ -842,7 +842,7 @@ Mission::set_mission_items()
 				if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
 				    && (_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF)
 				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
-				    && !_navigator->get_land_detected()->landed) {
+				    && !_navigator_core.getLanded()) {
 
 					new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
 
@@ -850,7 +850,7 @@ Mission::set_mission_items()
 					mission_item_next_position = _mission_item;
 					has_next_position_item = true;
 
-					float altitude = _navigator->get_global_position()->alt;
+					float altitude = _navigator_core.getAltitudeAMSLMeters();
 
 					if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
 						altitude = pos_sp_triplet->current.alt;
@@ -868,11 +868,11 @@ Mission::set_mission_items()
 				if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
 				    && _work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND
 				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT
-				    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
-				    && !_navigator->get_land_detected()->landed) {
+				    && _navigator_core.isFixedWing()
+				    && !_navigator_core.getLanded()) {
 
 					set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
-					_mission_item.altitude = _navigator->get_global_position()->alt;
+					_mission_item.altitude = _navigator_core.getAltitudeAMSLMeters();
 					_mission_item.altitude_is_relative = false;
 
 					new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND_AFTER_TRANSITION;
@@ -901,7 +901,7 @@ Mission::set_mission_items()
 					 * 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 = _navigator->get_global_position()->alt;
+					float altitude = _navigator_core.getAltitudeAMSLMeters();
 
 					if (pos_sp_triplet->current.valid
 					    && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
@@ -1001,8 +1001,8 @@ Mission::set_mission_items()
 				if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
 				    && _work_item_type == WORK_ITEM_TYPE_DEFAULT
 				    && 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
+				    && _navigator_core.isRotaryWing()
+				    && !_navigator_core.getLanded()
 				    && has_next_position_item) {
 
 					/* disable weathervane before front transition for allowing yaw to align */
@@ -1134,9 +1134,8 @@ Mission::set_mission_items()
 		}
 
 	} else {
-		/* allow the vehicle to decelerate before reaching a wp with a hold time */
-		const bool brake_for_hold = _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
-					    && get_time_inside(_mission_item) > FLT_EPSILON;
+		const bool brake_for_hold = _navigator_core.isRotaryWing()
+					    && get_time_inside(_mission_item) > FLT_EPSILON; //allow the vehicle to decelerate before reaching a wp with a hold time
 
 		if (_mission_item.autocontinue && !brake_for_hold) {
 			/* try to process next mission item */
@@ -1162,19 +1161,19 @@ Mission::set_mission_items()
 bool
 Mission::do_need_vertical_takeoff()
 {
-	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+	if (_navigator_core.isRotaryWing()) {
 
 		float takeoff_alt = calculate_takeoff_altitude(&_mission_item);
 
-		if (_navigator->get_land_detected()->landed) {
+		if (_navigator_core.getLanded()) {
 			/* force takeoff if landed (additional protection) */
 			_need_takeoff = true;
 
-		} else if (_navigator->get_global_position()->alt > takeoff_alt - _navigator->get_altitude_acceptance_radius()) {
+		} else if (_navigator_core.getAltitudeAMSLMeters() > takeoff_alt - _navigator_core.getAltAcceptanceRadiusMeter()) {
 			/* if in-air and already above takeoff height, don't do takeoff */
 			_need_takeoff = false;
 
-		} else if (_navigator->get_global_position()->alt <= takeoff_alt - _navigator->get_altitude_acceptance_radius()
+		} else if (_navigator_core.getAltitudeAMSLMeters() <= takeoff_alt - _navigator_core.getAltAcceptanceRadiusMeter()
 			   && (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
 			       || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)) {
 			/* if in-air but below takeoff height and we have a takeoff item */
@@ -1200,13 +1199,13 @@ Mission::do_need_vertical_takeoff()
 bool
 Mission::do_need_move_to_land()
 {
-	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+	if (_navigator_core.isRotaryWing()
 	    && (_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,
-				  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+				  _navigator_core.getLatRad(), _navigator_core.getLonRad());
 
-		return d_current > _navigator->get_acceptance_radius();
+		return d_current > _navigator_core.getAcceptanceRadiusMeter();
 	}
 
 	return false;
@@ -1215,13 +1214,13 @@ Mission::do_need_move_to_land()
 bool
 Mission::do_need_move_to_takeoff()
 {
-	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+	if (_navigator_core.isRotaryWing()
 	    && _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
 
 		float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon,
-				  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+				  _navigator_core.getLatRad(), _navigator_core.getLonRad());
 
-		return d_current > _navigator->get_acceptance_radius();
+		return d_current > _navigator_core.getAcceptanceRadiusMeter();
 	}
 
 	return false;
@@ -1236,9 +1235,9 @@ Mission::copy_position_if_valid(struct mission_item_s *mission_item, struct posi
 		mission_item->altitude = setpoint->alt;
 
 	} else {
-		mission_item->lat = _navigator->get_global_position()->lat;
-		mission_item->lon = _navigator->get_global_position()->lon;
-		mission_item->altitude = _navigator->get_global_position()->alt;
+		mission_item->lat = _navigator_core.getLatRad();
+		mission_item->lon = _navigator_core.getLonRad();
+		mission_item->altitude = _navigator_core.getAltitudeAMSLMeters();
 	}
 
 	mission_item->altitude_is_relative = false;
@@ -1253,7 +1252,7 @@ Mission::set_align_mission_item(struct mission_item_s *mission_item, struct miss
 	mission_item->autocontinue = true;
 	mission_item->time_inside = 0.0f;
 	mission_item->yaw = get_bearing_to_next_waypoint(
-				    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+				    _navigator_core.getLatRad(), _navigator_core.getLonRad(),
 				    mission_item_next->lat, mission_item_next->lon);
 	mission_item->force_heading = true;
 }
@@ -1265,11 +1264,13 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
 	float takeoff_alt = get_absolute_altitude_for_item(*mission_item);
 
 	/* takeoff to at least MIS_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
-	if (_navigator->get_land_detected()->landed) {
-		takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _navigator->get_takeoff_min_alt());
+	if (_navigator_core.getLanded()) {
+		takeoff_alt = fmaxf(takeoff_alt, _navigator_core.getAltitudeAMSLMeters() +
+				    _navigator_core.getRelativeTakeoffMinAltitudeMeter());
 
 	} else {
-		takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _navigator->get_takeoff_min_alt());
+		takeoff_alt = fmaxf(takeoff_alt, _navigator_core.getHomeAltAMSLMeter() +
+				    _navigator_core.getRelativeTakeoffMinAltitudeMeter());
 	}
 
 	return takeoff_alt;
@@ -1284,11 +1285,11 @@ Mission::heading_sp_update()
 	// Only update if current triplet is valid
 	if (pos_sp_triplet->current.valid) {
 
-		double point_from_latlon[2] = { _navigator->get_global_position()->lat,
-						_navigator->get_global_position()->lon
+		double point_from_latlon[2] = { _navigator_core.getLatRad(),
+						_navigator_core.getLonRad()
 					      };
-		double point_to_latlon[2] = { _navigator->get_global_position()->lat,
-					      _navigator->get_global_position()->lon
+		double point_to_latlon[2] = { _navigator_core.getLatRad(),
+					      _navigator_core.getLonRad()
 					    };
 		float yaw_offset = 0.0f;
 
@@ -1327,7 +1328,7 @@ Mission::heading_sp_update()
 		float d_current = get_distance_to_next_waypoint(point_from_latlon[0],
 				  point_from_latlon[1], point_to_latlon[0], point_to_latlon[1]);
 
-		if (d_current > _navigator->get_acceptance_radius()) {
+		if (d_current > _navigator_core.getAcceptanceRadiusMeter()) {
 			float yaw = matrix::wrap_pi(
 					    get_bearing_to_next_waypoint(point_from_latlon[0],
 							    point_from_latlon[1], point_to_latlon[0],
@@ -1339,7 +1340,7 @@ Mission::heading_sp_update()
 
 		} else {
 			if (!pos_sp_triplet->current.yaw_valid) {
-				_mission_item.yaw = _navigator->get_local_position()->heading;
+				_mission_item.yaw = _navigator_core.getTrueHeadingRad();
 				pos_sp_triplet->current.yaw = _mission_item.yaw;
 				pos_sp_triplet->current.yaw_valid = true;
 			}
@@ -1383,15 +1384,15 @@ Mission::do_abort_landing()
 	// loiter at the larger of MIS_LTRMIN_ALT above the landing point
 	//  or 2 * FW_CLMBOUT_DIFF above the current altitude
 	const float alt_landing = get_absolute_altitude_for_item(_mission_item);
-	const float alt_sp = math::max(alt_landing + _navigator->get_loiter_min_alt(),
-				       _navigator->get_global_position()->alt + 20.0f);
+	const float alt_sp = math::max(alt_landing + _navigator_core.getRelativeLoiterMinAltitudeMeter(),
+				       _navigator_core.getAltitudeAMSLMeters() + 20.0f);
 
 	// turn current landing waypoint into an indefinite loiter
 	_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
 	_mission_item.altitude_is_relative = false;
 	_mission_item.altitude = alt_sp;
-	_mission_item.loiter_radius = _navigator->get_loiter_radius();
-	_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+	_mission_item.loiter_radius = _navigator_core.getLoiterRadiusMeter();
+	_mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter();
 	_mission_item.autocontinue = false;
 	_mission_item.origin = ORIGIN_ONBOARD;
 
@@ -1659,7 +1660,7 @@ Mission::set_current_mission_item()
 void
 Mission::check_mission_valid(bool force)
 {
-	if ((!_home_inited && _navigator->home_position_valid()) || force) {
+	if ((!_home_inited && _navigator_core.isHomeValid()) || force) {
 
 		MissionFeasibilityChecker _missionFeasibilityChecker(_navigator);
 
@@ -1672,7 +1673,7 @@ Mission::check_mission_valid(bool force)
 		_navigator->get_mission_result()->seq_total = _mission.count;
 		_navigator->increment_mission_instance_count();
 		_navigator->set_mission_result_updated();
-		_home_inited = _navigator->home_position_valid();
+		_home_inited = _navigator_core.isHomeValid();
 
 		// find and store landing start marker (if available)
 		find_mission_land_start();
@@ -1733,7 +1734,7 @@ bool
 Mission::need_to_reset_mission()
 {
 	/* reset mission state when disarmed */
-	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset) {
+	if (_navigator_core.isNotArmed() && _need_mission_reset) {
 		_need_mission_reset = false;
 		return true;
 	}
@@ -1745,7 +1746,7 @@ void
 Mission::generate_waypoint_from_heading(struct position_setpoint_s *setpoint, float yaw)
 {
 	waypoint_from_heading_and_distance(
-		_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+		_navigator_core.getLatRad(), _navigator_core.getLonRad(),
 		yaw, 1000000.0f,
 		&(setpoint->lat), &(setpoint->lon));
 	setpoint->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
@@ -1773,13 +1774,13 @@ Mission::index_closest_mission_item() const
 		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))) {
+			      (_navigator_core.isFixedWing()) &&
+			      (!_navigator_core.isVTOL()))) {
 				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,
+						_navigator_core.getLatRad(),
+						_navigator_core.getLonRad(),
+						_navigator_core.getAltitudeAMSLMeters(),
 						&dist_xy, &dist_z);
 
 				if (dist < min_dist) {
@@ -1793,12 +1794,12 @@ Mission::index_closest_mission_item() const
 	// 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,
+				     _navigator_core.getHomeLatRad(),
+				     _navigator_core.getHomeLonRad(),
+				     _navigator_core.getHomeAltAMSLMeter(),
+				     _navigator_core.getLatRad(),
+				     _navigator_core.getLonRad(),
+				     _navigator_core.getAltitudeAMSLMeters(),
 				     &dist_xy, &dist_z);
 
 		if (dist < min_dist) {

src/modules/navigator/mission.h

@@ -69,7 +69,7 @@ class Navigator;
 class Mission : public MissionBlock, public ModuleParams
 {
 public:
-	Mission(Navigator *navigator);
+	Mission(Navigator *navigator, NavigatorCore &navigator_core);
 	~Mission() override = default;
 
 	void on_inactive() override;

src/modules/navigator/mission_block.cpp

@@ -56,14 +56,15 @@
 
 using matrix::wrap_pi;
 
-MissionBlock::MissionBlock(Navigator *navigator) :
-	NavigatorMode(navigator)
+MissionBlock::MissionBlock(Navigator *navigator, NavigatorCore &navigator_core) :
+	NavigatorMode(navigator),
+	_navigator_core(navigator_core)
 {
 	_mission_item.lat = (double)NAN;
 	_mission_item.lon = (double)NAN;
 	_mission_item.yaw = NAN;
-	_mission_item.loiter_radius = _navigator->get_loiter_radius();
-	_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+	//_mission_item.loiter_radius = _navigator_core.getLoiterRadiusMeter();
+	//_mission_item.acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter();
 	_mission_item.time_inside = 0.0f;
 	_mission_item.autocontinue = true;
 	_mission_item.origin = ORIGIN_ONBOARD;
@@ -80,7 +81,7 @@ MissionBlock::is_mission_item_reached()
 
 	case NAV_CMD_LAND: /* fall through */
 	case NAV_CMD_VTOL_LAND:
-		return _navigator->get_land_detected()->landed;
+		return _navigator_core.getLanded();
 
 	case NAV_CMD_IDLE: /* fall through */
 	case NAV_CMD_LOITER_UNLIMITED:
@@ -114,11 +115,11 @@ MissionBlock::is_mission_item_reached()
 
 		if (int(_mission_item.params[0]) == 3) {
 			// transition to RW requested, only accept waypoint if vehicle state has changed accordingly
-			return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+			return _navigator_core.isRotaryWing();
 
 		} else if (int(_mission_item.params[0]) == 4) {
 			// transition to FW requested, only accept waypoint if vehicle state has changed accordingly
-			return _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
+			return _navigator_core.isFixedWing();
 
 		} else {
 			// invalid vtol transition request
@@ -136,8 +137,7 @@ MissionBlock::is_mission_item_reached()
 
 	hrt_abstime now = hrt_absolute_time();
 
-	if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) {
-
+	if (!_navigator_core.getLanded() && !_waypoint_position_reached) {
 		float dist = -1.0f;
 		float dist_xy = -1.0f;
 		float dist_z = -1.0f;
@@ -145,23 +145,22 @@ MissionBlock::is_mission_item_reached()
 		const float mission_item_altitude_amsl = get_absolute_altitude_for_item(_mission_item);
 
 		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, mission_item_altitude_amsl,
-				_navigator->get_global_position()->lat,
-				_navigator->get_global_position()->lon,
-				_navigator->get_global_position()->alt,
+				_navigator_core.getLatRad(),
+				_navigator_core.getLonRad(),
+				_navigator_core.getAltitudeAMSLMeters(),
 				&dist_xy, &dist_z);
 
 		if ((_mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF)
-		    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-
+		    && _navigator_core.isRotaryWing()) {
 			/* We want to avoid the edge case where the acceptance radius is bigger or equal than
 			 * the altitude of the takeoff waypoint above home. Otherwise, we do not really follow
 			 * take-off procedures like leaving the landing gear down. */
 
 			float takeoff_alt = _mission_item.altitude_is_relative ?
 					    _mission_item.altitude :
-					    (_mission_item.altitude - _navigator->get_home_position()->alt);
+					    (_mission_item.altitude - _navigator_core.getHomeAltAMSLMeter());
 
-			float altitude_acceptance_radius = _navigator->get_altitude_acceptance_radius();
+			float altitude_acceptance_radius = _navigator_core.getAltAcceptanceRadMeter();
 
 			/* It should be safe to just use half of the takoeff_alt as an acceptance radius. */
 			if (takeoff_alt > 0 && takeoff_alt < altitude_acceptance_radius) {
@@ -169,26 +168,26 @@ MissionBlock::is_mission_item_reached()
 			}
 
 			/* require only altitude for takeoff for multicopter */
-			if (_navigator->get_global_position()->alt >
+			if (_navigator_core.getAltitudeAMSLMeters() >
 			    mission_item_altitude_amsl - altitude_acceptance_radius) {
 				_waypoint_position_reached = true;
 			}
 
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
-			   && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
+			   && _navigator_core.isRover()) {
 			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
-			if (dist_xy >= 0.0f && dist_xy <= _navigator->get_acceptance_radius()) {
+			if (dist_xy >= 0.0f && dist_xy <= _navigator_core.getHorAcceptanceRadiusMeter()) {
 				_waypoint_position_reached = true;
 			}
 
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
 			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
-			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()
-			    && dist_z <= _navigator->get_altitude_acceptance_radius()) {
+			if (dist >= 0.0f && dist <= _navigator_core.getHorAcceptanceRadiusMeter()
+			    && dist_z <= _navigator_core.getAltAcceptanceRadMeter()) {
 				_waypoint_position_reached = true;
 			}
 
-		} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING &&
+		} else if (_navigator_core.isFixedWing() &&
 			   (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
 			    _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT)) {
 
@@ -200,8 +199,8 @@ MissionBlock::is_mission_item_reached()
 			 */
 
 			// check if within loiter radius around wp, if yes then set altitude sp to mission item
-			if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
-			    && dist_z <= _navigator->get_altitude_acceptance_radius()) {
+			if (dist >= 0.0f && dist_xy <= (_navigator_core.getHorAcceptanceRadiusMeter() + fabsf(_mission_item.loiter_radius))
+			    && dist_z <= _navigator_core.getAltAcceptanceRadMeter()) {
 
 				_waypoint_position_reached = true;
 			}
@@ -216,22 +215,22 @@ MissionBlock::is_mission_item_reached()
 				dist_z = -1.0f;
 
 				dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, curr_sp->alt,
-						_navigator->get_global_position()->lat,
-						_navigator->get_global_position()->lon,
-						_navigator->get_global_position()->alt,
+						_navigator_core.getLatRad(),
+						_navigator_core.getLonRad(),
+						_navigator_core.getAltitudeAMSLMeters(),
 						&dist_xy, &dist_z);
 
 				// check if within loiter radius around wp, if yes then set altitude sp to mission item
-				if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
-				    && dist_z <= _navigator->get_default_altitude_acceptance_radius()) {
+				if (dist >= 0.0f && dist_xy <= (_navigator_core.getHorAcceptanceRadiusMeter() + fabsf(_mission_item.loiter_radius))
+				    && dist_z <= _navigator_core.getDefaultAltAcceptanceRadiusMeter()) {
 
 					curr_sp->alt = mission_item_altitude_amsl;
 					curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 					_navigator->set_position_setpoint_triplet_updated();
 				}
 
-			} else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
-				   && dist_z <= _navigator->get_altitude_acceptance_radius()) {
+			} else if (dist >= 0.f && dist_xy <= (_navigator_core.getAcceptanceRadiusMeter() + fabsf(_mission_item.loiter_radius))
+				   && dist_z <= _navigator_core.getAltAcceptanceRadiusMeter()) {
 				// loitering, check if new altitude is reached, while still also having check on position
 
 				_waypoint_position_reached = true;
@@ -257,7 +256,7 @@ MissionBlock::is_mission_item_reached()
 
 				// system position
 				matrix::Vector2f vehicle_pos;
-				map_projection_project(&ref_pos, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+				map_projection_project(&ref_pos, _navigator_core.getLatRad(), _navigator_core.getLonRad(),
 						       &vehicle_pos(0), &vehicle_pos(1));
 				const float dot_product = vehicle_pos.dot(gate_to_curr_sp.normalized());
 
@@ -279,14 +278,14 @@ MissionBlock::is_mission_item_reached()
 		} else {
 			/*normal mission items */
 
-			float mission_acceptance_radius = _navigator->get_acceptance_radius();
+			float mission_acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter();
 
 			/* for vtol back transition calculate acceptance radius based on time and ground speed */
 			if (_mission_item.vtol_back_transition
-			    && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			    && _navigator_core.isFixedWing()) {
 
-				float velocity = sqrtf(_navigator->get_local_position()->vx * _navigator->get_local_position()->vx +
-						       _navigator->get_local_position()->vy * _navigator->get_local_position()->vy);
+				float velocity = sqrtf(_navigator_core.getVelNorthMPS() * _navigator_core.getVelNorthMPS() +
+						       _navigator_core.getVelEastMPS() * _navigator_core.getVelEastMPS());
 
 				const float back_trans_dec = _navigator->get_vtol_back_trans_deceleration();
 				const float reverse_delay = _navigator->get_vtol_reverse_delay();
@@ -299,7 +298,7 @@ MissionBlock::is_mission_item_reached()
 			}
 
 			if (dist_xy >= 0.0f && dist_xy <= mission_acceptance_radius
-			    && dist_z <= _navigator->get_altitude_acceptance_radius()) {
+			    && dist_z <= _navigator_core.getAltAcceptanceRadiusMeter()) {
 				_waypoint_position_reached = true;
 			}
 		}
@@ -310,7 +309,7 @@ MissionBlock::is_mission_item_reached()
 		}
 
 		// consider yaw reached for non-rotary wing vehicles (such as fixed-wing)
-		if (_navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		if (!_navigator_core.isRotaryWing()) {
 			_waypoint_yaw_reached = true;
 		}
 	}
@@ -319,22 +318,22 @@ MissionBlock::is_mission_item_reached()
 
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
-		if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+		if (_navigator_core.isRotaryWing()
 		    && PX4_ISFINITE(_navigator->get_yaw_acceptance(_mission_item.yaw))) {
 
-			const float yaw_err = wrap_pi(_mission_item.yaw - _navigator->get_local_position()->heading);
+			const float yaw_err = wrap_pi(_mission_item.yaw - _navigator_core.getTrueHeadingRad());
 
 			/* accept yaw if reached or if timeout is set in which case we ignore not forced headings */
-			if (fabsf(yaw_err) < _navigator->get_yaw_threshold()
-			    || (_navigator->get_yaw_timeout() >= FLT_EPSILON && !_mission_item.force_heading)) {
+			if (fabsf(yaw_err) < _navigator_core.getWaypointHeadingAcceptanceRad()
+			    || (_navigator_core.getWaypointHeadingTimeoutSeconds() >= FLT_EPSILON && !_mission_item.force_heading)) {
 
 				_waypoint_yaw_reached = true;
 			}
 
 			/* if heading needs to be reached, the timeout is enabled and we don't make it, abort mission */
 			if (!_waypoint_yaw_reached && _mission_item.force_heading &&
-			    (_navigator->get_yaw_timeout() >= FLT_EPSILON) &&
-			    (now - _time_wp_reached >= (hrt_abstime)_navigator->get_yaw_timeout() * 1e6f)) {
+			    (_navigator_core.getWaypointHeadingTimeoutSeconds() >= FLT_EPSILON) &&
+			    (now - _time_wp_reached >= (hrt_abstime)_navigator_core.getWaypointHeadingTimeoutSeconds() * 1e6f)) {
 
 				_navigator->set_mission_failure("unable to reach heading within timeout");
 			}
@@ -365,7 +364,7 @@ MissionBlock::is_mission_item_reached()
 
 			/* enforce exit heading if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set,
 			   or if loitering to achieve altitdue at a NAV_CMD_WAYPOINT */
-			const bool enforce_exit_heading = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+			const bool enforce_exit_heading = !_navigator_core.isRotaryWing()
 							  &&
 							  next_sp.valid &&
 							  curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER &&
@@ -379,12 +378,12 @@ MissionBlock::is_mission_item_reached()
 
 				float yaw_err = 0.0f;
 
-				if (dist_current_next >  1.2f * _navigator->get_loiter_radius()) {
+				if (dist_current_next >  1.2f * _navigator_core.getLoiterRadiusMeter()) {
 					// set required yaw from bearing to the next mission item
-					_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
-							    _navigator->get_global_position()->lon,
+					_mission_item.yaw = get_bearing_to_next_waypoint(_navigator_core.getLatRad(),
+							    _navigator_core.getLonRad(),
 							    next_sp.lat, next_sp.lon);
-					const float cog = atan2f(_navigator->get_local_position()->vy, _navigator->get_local_position()->vx);
+					const float cog = atan2f(_navigator_core.getVelEastMPS(), _navigator_core.getVelNorthMPS());
 					yaw_err = wrap_pi(_mission_item.yaw - cog);
 
 
@@ -392,7 +391,7 @@ MissionBlock::is_mission_item_reached()
 				}
 
 
-				if (fabsf(yaw_err) < _navigator->get_yaw_threshold()) {
+				if (fabsf(yaw_err) < _navigator_core.getWaypointHeadingAcceptanceRad()) {
 					exit_heading_reached = true;
 				}
 
@@ -489,7 +488,7 @@ MissionBlock::issue_command(const mission_item_s &item)
 		if (item.nav_cmd == NAV_CMD_DO_SET_ROI_LOCATION && item.altitude_is_relative) {
 			vcmd.param5 = item.lat;
 			vcmd.param6 = item.lon;
-			vcmd.param7 = item.altitude + _navigator->get_home_position()->alt;
+			vcmd.param7 = item.altitude + _navigator_core.getHomeAltAMSLMeter();
 
 		} else {
 			vcmd.param5 = (double)item.params[4];
@@ -505,7 +504,7 @@ float
 MissionBlock::get_time_inside(const mission_item_s &item) const
 {
 	if ((item.nav_cmd == NAV_CMD_WAYPOINT
-	     && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) ||
+	     && _navigator_core.isRotaryWing()) ||
 	    item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 	    item.nav_cmd == NAV_CMD_DELAY) {
 
@@ -553,10 +552,11 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	sp->lat = item.lat;
 	sp->lon = item.lon;
 	sp->alt = get_absolute_altitude_for_item(item);
+	sp->alt_valid = true;
 	sp->yaw = item.yaw;
 	sp->yaw_valid = PX4_ISFINITE(item.yaw);
 	sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) :
-			    _navigator->get_loiter_radius();
+			    _navigator_core.getLoiterRadiusMeter();
 	sp->loiter_direction = (item.loiter_radius > 0) ? 1 : -1;
 
 	if (item.acceptance_radius > 0.0f && PX4_ISFINITE(item.acceptance_radius)) {
@@ -564,7 +564,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 		sp->acceptance_radius = item.acceptance_radius;
 
 	} else {
-		sp->acceptance_radius = _navigator->get_default_acceptance_radius();
+		sp->acceptance_radius = _navigator->getCore().getDefaultHorAcceptanceRadiusMeter();
 	}
 
 	sp->cruising_speed = _navigator->get_cruising_speed();
@@ -578,9 +578,8 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	case NAV_CMD_TAKEOFF:
 
 		// if already flying (armed and !landed) treat TAKEOFF like regular POSITION
-		if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED)
-		    && !_navigator->get_land_detected()->landed && !_navigator->get_land_detected()->maybe_landed) {
-
+		if ((_navigator_core.getArmingState() == vehicle_status_s::ARMING_STATE_ARMED)
+		    && !_navigator_core.getLanded() && !_navigator_core.getMaybeLanded()) {
 			sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 
 		} else {
@@ -601,12 +600,12 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	case NAV_CMD_LOITER_TO_ALT:
 
 		// initially use current altitude, and switch to mission item altitude once in loiter position
-		if (_navigator->get_loiter_min_alt() > 0.f) { // ignore _param_loiter_min_alt if smaller than 0
-			sp->alt = math::max(_navigator->get_global_position()->alt,
-					    _navigator->get_home_position()->alt + _navigator->get_loiter_min_alt());
+		if (_navigator_core.getRelativeLoiterMinAltitudeMeter() > 0.f) { // ignore _param_loiter_min_alt if smaller than 0
+			sp->alt = math::max(_navigator_core.getAltitudeAMSLMeters(),
+					    _navigator_core.getHomeAltAMSLMeter() + _navigator_core.getRelativeLoiterMinAltitudeMeter());
 
 		} else {
-			sp->alt = _navigator->get_global_position()->alt;
+			sp->alt = _navigator_core.getAltitudeAMSLMeters();
 		}
 
 	// FALLTHROUGH
@@ -630,7 +629,7 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 void
 MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 {
-	if (_navigator->get_land_detected()->landed) {
+	if (_navigator_core.getLanded()) {
 		/* landed, don't takeoff, but switch to IDLE mode */
 		item->nav_cmd = NAV_CMD_IDLE;
 
@@ -647,19 +646,19 @@ MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 
 		} else {
 			/* use current position and use return altitude as clearance */
-			item->lat = _navigator->get_global_position()->lat;
-			item->lon = _navigator->get_global_position()->lon;
-			item->altitude = _navigator->get_global_position()->alt;
+			item->lat = _navigator_core.getLatRad();
+			item->lon = _navigator_core.getLonRad();
+			item->altitude = _navigator_core.getAltitudeAMSLMeters();
 
-			if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
-				item->altitude = _navigator->get_home_position()->alt + min_clearance;
+			if (min_clearance > 0.0f && item->altitude < _navigator_core.getHomeAltAMSLMeter() + min_clearance) {
+				item->altitude = _navigator_core.getHomeAltAMSLMeter() + min_clearance;
 			}
 		}
 
 		item->altitude_is_relative = false;
 		item->yaw = NAN;
-		item->loiter_radius = _navigator->get_loiter_radius();
-		item->acceptance_radius = _navigator->get_acceptance_radius();
+		item->loiter_radius = _navigator_core.getLoiterRadiusMeter();
+		item->acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter();
 		item->time_inside = 0.0f;
 		item->autocontinue = false;
 		item->origin = ORIGIN_ONBOARD;
@@ -672,14 +671,14 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude)
 	item->nav_cmd = NAV_CMD_TAKEOFF;
 
 	/* use current position */
-	item->lat = _navigator->get_global_position()->lat;
-	item->lon = _navigator->get_global_position()->lon;
-	item->yaw = _navigator->get_local_position()->heading;
+	item->lat = _navigator_core.getLatRad();
+	item->lon = _navigator_core.getLonRad();
+	item->yaw = _navigator_core.getTrueHeadingRad();
 
 	item->altitude = abs_altitude;
 	item->altitude_is_relative = false;
 
-	item->loiter_radius = _navigator->get_loiter_radius();
+	item->loiter_radius = _navigator_core.getLoiterRadiusMeter();
 	item->autocontinue = false;
 	item->origin = ORIGIN_ONBOARD;
 }
@@ -688,7 +687,7 @@ void
 MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_location)
 {
 	/* VTOL transition to RW before landing */
-	if (_navigator->force_vtol()) {
+	if (_navigator_core.forceVTOL()) {
 
 		vehicle_command_s vcmd = {};
 		vcmd.command = NAV_CMD_DO_VTOL_TRANSITION;
@@ -703,19 +702,19 @@ MissionBlock::set_land_item(struct mission_item_s *item, bool at_current_locatio
 	if (at_current_location) {
 		item->lat = (double)NAN; //descend at current position
 		item->lon = (double)NAN; //descend at current position
-		item->yaw = _navigator->get_local_position()->heading;
+		item->yaw = _navigator_core.getTrueHeadingRad();
 
 	} else {
 		/* use home position */
-		item->lat = _navigator->get_home_position()->lat;
-		item->lon = _navigator->get_home_position()->lon;
-		item->yaw = _navigator->get_home_position()->yaw;
+		item->lat = _navigator_core.getHomeLatRad();
+		item->lon = _navigator_core.getHomeLonRad();
+		item->yaw = _navigator_core.getHomeTrueHeadingRad();
 	}
 
 	item->altitude = 0;
 	item->altitude_is_relative = false;
-	item->loiter_radius = _navigator->get_loiter_radius();
-	item->acceptance_radius = _navigator->get_acceptance_radius();
+	item->loiter_radius = _navigator_core.getLoiterRadiusMeter();
+	item->acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter();
 	item->time_inside = 0.0f;
 	item->autocontinue = true;
 	item->origin = ORIGIN_ONBOARD;
@@ -725,13 +724,13 @@ void
 MissionBlock::set_idle_item(struct mission_item_s *item)
 {
 	item->nav_cmd = NAV_CMD_IDLE;
-	item->lat = _navigator->get_home_position()->lat;
-	item->lon = _navigator->get_home_position()->lon;
+	item->lat = _navigator_core.getHomeLatRad();
+	item->lon = _navigator_core.getHomeLonRad();
 	item->altitude_is_relative = false;
-	item->altitude = _navigator->get_home_position()->alt;
+	item->altitude = _navigator_core.getHomeAltAMSLMeter();
 	item->yaw = NAN;
-	item->loiter_radius = _navigator->get_loiter_radius();
-	item->acceptance_radius = _navigator->get_acceptance_radius();
+	item->loiter_radius = _navigator_core.getLoiterRadiusMeter();
+	item->acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter();
 	item->time_inside = 0.0f;
 	item->autocontinue = true;
 	item->origin = ORIGIN_ONBOARD;
@@ -742,7 +741,7 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_
 {
 	item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
 	item->params[0] = (float) new_mode;
-	item->yaw = _navigator->get_local_position()->heading;
+	item->yaw = _navigator_core.getTrueHeadingRad();
 	item->autocontinue = true;
 }
 
@@ -754,18 +753,18 @@ MissionBlock::mission_apply_limitation(mission_item_s &item)
 	 */
 
 	/* do nothing if altitude max is negative */
-	if (_navigator->get_land_detected()->alt_max > 0.0f) {
+	if (_navigator_core.getLandDetectedAltMaxMeter() > 0.0f) {
 
 		/* absolute altitude */
 		float altitude_abs = item.altitude_is_relative
-				     ? item.altitude + _navigator->get_home_position()->alt
+				     ? item.altitude + _navigator_core.getHomeAltAMSLMeter()
 				     : item.altitude;
 
 		/* limit altitude to maximum allowed altitude */
-		if ((_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt) < altitude_abs) {
+		if ((_navigator_core.getLandDetectedAltMaxMeter() + _navigator_core.getHomeAltAMSLMeter()) < altitude_abs) {
 			item.altitude = item.altitude_is_relative ?
-					_navigator->get_land_detected()->alt_max :
-					_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt;
+					_navigator_core.getLandDetectedAltMaxMeter() :
+					_navigator_core.getLandDetectedAltMaxMeter() + _navigator_core.getHomeAltAMSLMeter();
 
 		}
 	}
@@ -779,7 +778,7 @@ float
 MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item) const
 {
 	if (mission_item.altitude_is_relative) {
-		return mission_item.altitude + _navigator->get_home_position()->alt;
+		return mission_item.altitude + _navigator_core.getHomeAltAMSLMeter();
 
 	} else {
 		return mission_item.altitude;

src/modules/navigator/mission_block.h

@@ -42,6 +42,7 @@
 
 #include "navigator_mode.h"
 #include "navigation.h"
+#include "NavigatorCore.h"
 
 #include <drivers/drv_hrt.h>
 #include <systemlib/mavlink_log.h>
@@ -61,7 +62,7 @@ public:
 	/**
 	 * Constructor
 	 */
-	MissionBlock(Navigator *navigator);
+	MissionBlock(Navigator *navigator, NavigatorCore &navigator_core);
 	virtual ~MissionBlock() = default;
 
 	MissionBlock(const MissionBlock &) = delete;
@@ -151,5 +152,7 @@ protected:
 
 	hrt_abstime _time_wp_reached{0};
 
+	NavigatorCore &_navigator_core;
+
 	uORB::Publication<actuator_controls_s>	_actuator_pub{ORB_ID(actuator_controls_2)};
 };

src/modules/navigator/mission_feasibility_checker.cpp

@@ -67,8 +67,8 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
 	bool warned = false;
 
 	// first check if we have a valid position
-	const bool home_valid = _navigator->home_position_valid();
-	const bool home_alt_valid = _navigator->home_alt_valid();
+	const bool home_valid = _navigator->getCore().isHomeValid();
+	const bool home_alt_valid = _navigator->getCore().isHomeAltValid();
 
 	if (!home_alt_valid) {
 		failed = true;
@@ -79,7 +79,7 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
 		failed = failed || !checkDistanceToFirstWaypoint(mission, max_distance_to_1st_waypoint);
 	}
 
-	const float home_alt = _navigator->get_home_position()->alt;
+	const float home_alt = _navigator->getCore().getHomeAltAMSLMeter();
 
 	// check if all mission item commands are supported
 	failed = failed || !checkMissionItemValidity(mission);
@@ -87,10 +87,10 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
 	failed = failed || !checkGeofence(mission, home_alt, home_valid);
 	failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid, warned);
 
-	if (_navigator->get_vstatus()->is_vtol) {
+	if (_navigator->getCore().isVTOL()) {
 		failed = failed || !checkVTOL(mission, home_alt, false);
 
-	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+	} else if (_navigator->getCore().isRotaryWing()) {
 		failed = failed || !checkRotarywing(mission, home_alt);
 
 	} else {
@@ -300,7 +300,7 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
 		}
 
 		// check if the mission starts with a land command while the vehicle is landed
-		if ((i == 0) && missionitem.nav_cmd == NAV_CMD_LAND && _navigator->get_land_detected()->landed) {
+		if ((i == 0) && missionitem.nav_cmd == NAV_CMD_LAND && _navigator->getCore().getLanded()) {
 
 			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: starts with landing");
 			return false;
@@ -336,7 +336,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
 					    : missionitem.altitude - home_alt;
 
 			// check if we should use default acceptance radius
-			float acceptance_radius = _navigator->get_default_acceptance_radius();
+			float acceptance_radius = _navigator->getCore().getDefaultHorAcceptanceRadiusMeter();
 
 			if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) {
 				acceptance_radius = missionitem.acceptance_radius;
@@ -408,7 +408,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
 		}
 	}
 
-	if (_navigator->get_takeoff_required() && _navigator->get_land_detected()->landed) {
+	if (_navigator->getCore().isTakeoffRequired() && _navigator->getCore().getLanded()) {
 		// check for a takeoff waypoint, after the above conditions have been met
 		// MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission
 		// while the vehicle is flying and it does not require a takeoff waypoint
@@ -653,7 +653,7 @@ MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission
 		/* check distance from current position to item */
 		float dist_to_1wp = get_distance_to_next_waypoint(
 					    mission_item.lat, mission_item.lon,
-					    _navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
+					    _navigator->getCore().getHomeLatRad(), _navigator->getCore().getHomeLonRad());
 
 		if (dist_to_1wp < max_distance) {
 

src/modules/navigator/navigator.h

@@ -122,6 +122,8 @@ public:
 
 	void		publish_vehicle_cmd(vehicle_command_s *vcmd);
 
+	void		params_update();
+
 	/**
 	 * Generate an artificial traffic indication
 	 *
@@ -165,39 +167,39 @@ public:
 	const vehicle_roi_s &get_vroi() { return _vroi; }
 	void reset_vroi() { _vroi = {}; }
 
-	bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); }
-	bool home_position_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos && _home_pos.valid_lpos); }
+	//bool home_alt_valid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt); }
+	//bool _navigator_core.isHomeValid() { return (_home_pos.timestamp > 0 && _home_pos.valid_alt && _home_pos.valid_hpos && _home_pos.valid_lpos); }
 
 	Geofence	&get_geofence() { return _geofence; }
 
 	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
-	float		get_loiter_radius() { return _param_nav_loiter_rad.get(); }
+	//float		_navigator_core.getLoiterRadiusMeter() { return _param_nav_loiter_rad.get(); }
 
 	/**
 	 * Returns the default acceptance radius defined by the parameter
 	 */
-	float		get_default_acceptance_radius();
+	//float		get_default_acceptance_radius();
 
 	/**
 	 * Get the acceptance radius
 	 *
 	 * @return the distance at which the next waypoint should be used
 	 */
-	float		get_acceptance_radius();
+	//float		get_acceptance_radius();
 
 	/**
 	 * Get the default altitude acceptance radius (i.e. from parameters)
 	 *
 	 * @return the distance from the target altitude before considering the waypoint reached
 	 */
-	float		get_default_altitude_acceptance_radius();
+	//float		get_default_altitude_acceptance_radius();
 
 	/**
 	 * Get the altitude acceptance radius
 	 *
 	 * @return the distance from the target altitude before considering the waypoint reached
 	 */
-	float		get_altitude_acceptance_radius();
+	//float		get_altitude_acceptance_radius();
 
 	/**
 	 * Get the cruising speed
@@ -290,43 +292,21 @@ public:
 
 	void geofence_breach_check(bool &have_geofence_position_data);
 
-	// Param access
-	float		get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); }
-	float		get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); }
-	bool		get_takeoff_required() const { return _param_mis_takeoff_req.get(); }
-	float		get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); }
-	float		get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); }
-
 	float		get_vtol_back_trans_deceleration() const { return _param_back_trans_dec_mss; }
 	float		get_vtol_reverse_delay() const { return _param_reverse_delay; }
 
-	bool		force_vtol();
+	//bool		force_vtol();
 
 	void		acquire_gimbal_control();
 	void		release_gimbal_control();
 
+	NavigatorCore &getCore() { return _navigator_core; }
+
 private:
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad,	/**< loiter radius for fixedwing */
-		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,	/**< acceptance for takeoff */
-		(ParamFloat<px4::params::NAV_FW_ALT_RAD>)
-		_param_nav_fw_alt_rad,	/**< acceptance radius for fixedwing altitude */
-		(ParamFloat<px4::params::NAV_FW_ALTL_RAD>)
-		_param_nav_fw_altl_rad,	/**< acceptance radius for fixedwing altitude before landing*/
-		(ParamFloat<px4::params::NAV_MC_ALT_RAD>)
-		_param_nav_mc_alt_rad,	/**< acceptance radius for multicopter altitude */
-		(ParamInt<px4::params::NAV_FORCE_VT>) _param_nav_force_vt,	/**< acceptance radius for multicopter altitude */
 		(ParamInt<px4::params::NAV_TRAFF_AVOID>) _param_nav_traff_avoid,	/**< avoiding other aircraft is enabled */
 		(ParamFloat<px4::params::NAV_TRAFF_A_RADU>) _param_nav_traff_a_radu,	/**< avoidance Distance Unmanned*/
-		(ParamFloat<px4::params::NAV_TRAFF_A_RADM>) _param_nav_traff_a_radm,	/**< avoidance Distance Manned*/
-
-		// non-navigator parameters
-		// Mission (MIS_*)
-		(ParamFloat<px4::params::MIS_LTRMIN_ALT>) _param_mis_ltrmin_alt,
-		(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
-		(ParamBool<px4::params::MIS_TAKEOFF_REQ>) _param_mis_takeoff_req,
-		(ParamFloat<px4::params::MIS_YAW_TMT>) _param_mis_yaw_tmt,
-		(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err
+		(ParamFloat<px4::params::NAV_TRAFF_A_RADM>) _param_nav_traff_a_radm	/**< avoidance Distance Manned*/
 	)
 
 	int		_local_pos_sub{-1};
@@ -385,6 +365,8 @@ private:
 	bool 		_pos_sp_triplet_published_invalid_once{false};	/**< flags if position SP triplet has been published once to UORB */
 	bool		_mission_result_updated{false};		/**< flags if mission result has seen an update */
 
+	NavigatorCore _navigator_core;
+
 	NavigatorMode	*_navigation_mode{nullptr};		/**< abstract pointer to current navigation mode class */
 	Mission		_mission;			/**< class that handles the missions */
 	Loiter		_loiter;			/**< class that handles loiter */
@@ -411,7 +393,7 @@ private:
 	// if mission mode is inactive, this flag will be cleared after 2 seconds
 
 	// update subscriptions
-	void		params_update();
+
 
 	/**
 	 * Publish a new position setpoint triplet for position controllers

src/modules/navigator/navigator_main.cpp

@@ -76,15 +76,16 @@ Navigator::Navigator() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
 	_geofence(this),
 	_gf_breach_avoidance(this),
-	_mission(this),
-	_loiter(this),
-	_takeoff(this),
-	_land(this),
-	_precland(this),
-	_rtl(this),
-	_engineFailure(this),
-	_gpsFailure(this),
-	_follow_target(this)
+	_navigator_core(),
+	_mission(this, _navigator_core),
+	_loiter(this, _navigator_core),
+	_takeoff(this, _navigator_core),
+	_land(this, _navigator_core),
+	_precland(this, _navigator_core),
+	_rtl(this, _navigator_core),
+	_engineFailure(this, _navigator_core),
+	_gpsFailure(this, _navigator_core),
+	_follow_target(this, _navigator_core)
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
@@ -177,7 +178,9 @@ Navigator::run()
 		perf_begin(_loop_perf);
 
 		orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
+		_navigator_core.updateLocalPosition(_local_pos);
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vstatus);
+		_navigator_core.updateVehicleStatus(_vstatus);
 
 		if (fds[2].revents & POLLIN) {
 			// copy mission to clear any update
@@ -197,6 +200,7 @@ Navigator::run()
 		/* global position updated */
 		if (_global_pos_sub.updated()) {
 			_global_pos_sub.copy(&_global_pos);
+			_navigator_core.updateGlobalPosition(_global_pos);
 
 			if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
 				have_geofence_position_data = true;
@@ -214,8 +218,10 @@ Navigator::run()
 		}
 
 		_land_detected_sub.update(&_land_detected);
+		_navigator_core.updateLandedState(_land_detected);
 		_position_controller_status_sub.update();
 		_home_pos_sub.update(&_home_pos);
+		_navigator_core.updateHomePosition(_home_pos);
 
 		while (_vehicle_command_sub.updated()) {
 			const unsigned last_generation = _vehicle_command_sub.get_last_generation();
@@ -254,7 +260,7 @@ Navigator::run()
 						}
 
 						reposition_valid = _geofence.check(test_reposition_validity, _gps_pos, _home_pos,
-										   home_position_valid());
+										   _navigator_core.isHomeValid());
 					}
 				}
 
@@ -268,7 +274,7 @@ Navigator::run()
 					rep->previous.lon = get_global_position()->lon;
 					rep->previous.alt = get_global_position()->alt;
 
-					rep->current.loiter_radius = get_loiter_radius();
+					rep->current.loiter_radius = _navigator_core.getLoiterRadiusMeter();
 					rep->current.loiter_direction = 1;
 					rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 
@@ -281,7 +287,7 @@ Navigator::run()
 					}
 
 					rep->current.cruising_throttle = get_cruising_throttle();
-					rep->current.acceptance_radius = get_acceptance_radius();
+					rep->current.acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter();
 
 					// Go on and check which changes had been requested
 					if (PX4_ISFINITE(cmd.param4)) {
@@ -351,11 +357,11 @@ Navigator::run()
 				rep->previous.lon = get_global_position()->lon;
 				rep->previous.alt = get_global_position()->alt;
 
-				rep->current.loiter_radius = get_loiter_radius();
+				rep->current.loiter_radius = _navigator_core.getLoiterRadiusMeter();
 				rep->current.loiter_direction = 1;
 				rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
 
-				if (home_position_valid()) {
+				if (_navigator_core.isHomeValid()) {
 					rep->current.yaw = cmd.param4;
 
 					rep->previous.valid = true;
@@ -721,7 +727,7 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 			vertical_test_point_distance = _gf_breach_avoidance.computeVerticalBrakingDistanceMultirotor();
 
 		} else {
-			test_point_distance = 2.0f * get_loiter_radius();
+			test_point_distance = 2.0f * _navigator_core.getLoiterRadiusMeter();
 			vertical_test_point_distance = 5.0f;
 
 			if (hrt_absolute_time() - pos_ctrl_status.timestamp < 100000 && PX4_ISFINITE(pos_ctrl_status.nav_bearing)) {
@@ -739,7 +745,7 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 		_gf_breach_avoidance.setMaxHorDistHome(_geofence.getMaxHorDistanceHome());
 		_gf_breach_avoidance.setMaxVerDistHome(_geofence.getMaxVerDistanceHome());
 
-		if (home_position_valid()) {
+		if (_navigator_core.isHomeValid()) {
 			_gf_breach_avoidance.setHomePosition(_home_pos.lat, _home_pos.lon, _home_pos.alt);
 		}
 
@@ -802,12 +808,12 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 					rep->current.lon = lointer_center_lat_lon(1);
 					rep->current.alt = loiter_altitude_amsl;
 					rep->current.valid = true;
-					rep->current.loiter_radius = get_loiter_radius();
+					rep->current.loiter_radius = _navigator_core.getLoiterRadiusMeter();
 					rep->current.alt_valid = true;
 					rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 					rep->current.loiter_direction = 1;
 					rep->current.cruising_throttle = get_cruising_throttle();
-					rep->current.acceptance_radius = get_acceptance_radius();
+					rep->current.acceptance_radius = _navigator_core.getHorAcceptanceRadiusMeter();
 					rep->current.cruising_speed = get_cruising_speed();
 
 				}
@@ -872,36 +878,13 @@ Navigator::publish_position_setpoint_triplet()
 	_pos_sp_triplet_updated = false;
 }
 
-float
-Navigator::get_default_acceptance_radius()
-{
-	return _param_nav_acc_rad.get();
-}
+// float
+// Navigator::get_default_acceptance_radius()
+// {
+// 	return _navigator_core.getDefaultHorAcceptanceRadiusMeter();
+// }
 
-float
-Navigator::get_default_altitude_acceptance_radius()
-{
-	if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-		return _param_nav_fw_alt_rad.get();
-
-	} else if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
-		return INFINITY;
-
-	} else {
-		float alt_acceptance_radius = _param_nav_mc_alt_rad.get();
-
-		const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get();
-
-		if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp)
-		    && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) {
-			alt_acceptance_radius = pos_ctrl_status.altitude_acceptance;
-		}
-
-		return alt_acceptance_radius;
-	}
-}
-
-float
+/* float
 Navigator::get_altitude_acceptance_radius()
 {
 	if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
@@ -914,7 +897,7 @@ Navigator::get_altitude_acceptance_radius()
 	}
 
 	return get_default_altitude_acceptance_radius();
-}
+} */
 
 float
 Navigator::get_cruising_speed()
@@ -973,8 +956,8 @@ Navigator::reset_position_setpoint(position_setpoint_s &sp)
 	sp.timestamp = hrt_absolute_time();
 	sp.lat = static_cast<double>(NAN);
 	sp.lon = static_cast<double>(NAN);;
-	sp.loiter_radius = get_loiter_radius();
-	sp.acceptance_radius = get_default_acceptance_radius();
+	sp.loiter_radius = _navigator_core.getLoiterRadiusMeter();
+	sp.acceptance_radius = _navigator_core.getDefaultHorAcceptanceRadiusMeter();
 	sp.cruising_speed = get_cruising_speed();
 	sp.cruising_throttle = get_cruising_throttle();
 	sp.valid = false;
@@ -994,7 +977,7 @@ Navigator::get_cruising_throttle()
 	}
 }
 
-float
+/* float
 Navigator::get_acceptance_radius()
 {
 	float acceptance_radius = get_default_acceptance_radius(); // the value specified in the parameter NAV_ACC_RAD
@@ -1008,7 +991,7 @@ Navigator::get_acceptance_radius()
 	}
 
 	return acceptance_radius;
-}
+} */
 
 float
 Navigator::get_yaw_acceptance(float mission_item_yaw)
@@ -1270,13 +1253,13 @@ Navigator::abort_landing()
 	return should_abort;
 }
 
-bool
+/* bool
 Navigator::force_vtol()
 {
 	return _vstatus.is_vtol &&
 	       (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING || _vstatus.in_transition_to_fw)
 	       && _param_nav_force_vt.get();
-}
+} */
 
 int Navigator::custom_command(int argc, char *argv[])
 {

src/modules/navigator/precland.cpp

@@ -59,8 +59,8 @@
 
 #define STATE_TIMEOUT 10000000 // [us] Maximum time to spend in any state
 
-PrecLand::PrecLand(Navigator *navigator) :
-	MissionBlock(navigator),
+PrecLand::PrecLand(Navigator *navigator, NavigatorCore &navigator_core) :
+	MissionBlock(navigator, _navigator_core),
 	ModuleParams(navigator)
 {
 	_handle_param_acceleration_hor = param_find("MPC_ACC_HOR");
@@ -90,9 +90,9 @@ PrecLand::on_activation()
 	// Check that the current position setpoint is valid, otherwise land at current position
 	if (!pos_sp_triplet->current.valid) {
 		PX4_WARN("Resetting landing position to current position");
-		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
+		pos_sp_triplet->current.lat = _navigator_core.getLatRad();
+		pos_sp_triplet->current.lon = _navigator_core.getLonRad();
+		pos_sp_triplet->current.alt = _navigator_core.getAltitudeAMSLMeters();
 		pos_sp_triplet->current.valid = true;
 		pos_sp_triplet->current.timestamp = hrt_absolute_time();
 	}
@@ -121,7 +121,7 @@ PrecLand::on_active()
 	}
 
 	// stop if we are landed
-	if (_navigator->get_land_detected()->landed) {
+	if (_navigator_core.getLanded()) {
 		switch_to_state_done();
 	}
 
@@ -198,10 +198,10 @@ PrecLand::run_state_start()
 
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	float dist = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, pos_sp_triplet->current.lon,
-			_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
+			_navigator_core.getLatRad(), _navigator_core.getLonRad());
 
 	// check if we've reached the start point
-	if (dist < _navigator->get_acceptance_radius()) {
+	if (dist < _navigator_core.getAcceptanceRadiusMeter()) {
 		if (!_point_reached_time) {
 			_point_reached_time = hrt_absolute_time();
 		}
@@ -235,9 +235,9 @@ PrecLand::run_state_horizontal_approach()
 		PX4_WARN("Lost landing target while landing (horizontal approach).");
 
 		// Stay at current position for searching for the landing target
-		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
+		pos_sp_triplet->current.lat = _navigator_core.getLatRad();
+		pos_sp_triplet->current.lon = _navigator_core.getLonRad();
+		pos_sp_triplet->current.alt = _navigator_core.getAltitudeAMSLMeters();
 
 		if (!switch_to_state_start()) {
 			if (!switch_to_state_fallback()) {
@@ -300,9 +300,9 @@ PrecLand::run_state_descend_above_target()
 			PX4_WARN("Lost landing target while landing (descending).");
 
 			// Stay at current position for searching for the target
-			pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-			pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-			pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
+			pos_sp_triplet->current.lat = _navigator_core.getLatRad();
+			pos_sp_triplet->current.lon = _navigator_core.getLonRad();
+			pos_sp_triplet->current.alt = _navigator_core.getAltitudeAMSLMeters();
 
 			if (!switch_to_state_start()) {
 				if (!switch_to_state_fallback()) {
@@ -341,7 +341,7 @@ PrecLand::run_state_search()
 			// target just became visible. Stop climbing, but give it some margin so we don't stop too apruptly
 			_target_acquired_time = hrt_absolute_time();
 			position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-			float new_alt = _navigator->get_global_position()->alt + 1.0f;
+			float new_alt = _navigator_core.getAltitudeAMSLMeters() + 1.0f;
 			pos_sp_triplet->current.alt = new_alt < pos_sp_triplet->current.alt ? new_alt : pos_sp_triplet->current.alt;
 			_navigator->set_position_setpoint_triplet_updated();
 		}
@@ -395,7 +395,7 @@ bool
 PrecLand::switch_to_state_horizontal_approach()
 {
 	if (check_state_conditions(PrecLandState::HorizontalApproach)) {
-		_approach_alt = _navigator->get_global_position()->alt;
+		_approach_alt = _navigator_core.getAltitudeAMSLMeters();
 
 		_point_reached_time = 0;
 
@@ -454,9 +454,9 @@ PrecLand::switch_to_state_fallback()
 {
 	PX4_WARN("Falling back to normal land.");
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-	pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-	pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-	pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
+	pos_sp_triplet->current.lat = _navigator_core.getLatRad();
+	pos_sp_triplet->current.lon = _navigator_core.getLonRad();
+	pos_sp_triplet->current.alt = _navigator_core.getAltitudeAMSLMeters();
 	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
 	_navigator->set_position_setpoint_triplet_updated();
 
@@ -557,8 +557,8 @@ void PrecLand::slewrate(float &sp_x, float &sp_y)
 		// set a best guess for previous setpoints for smooth transition
 		map_projection_project(&_map_ref, _navigator->get_position_setpoint_triplet()->current.lat,
 				       _navigator->get_position_setpoint_triplet()->current.lon, &_sp_pev(0), &_sp_pev(1));
-		_sp_pev_prev(0) = _sp_pev(0) - _navigator->get_local_position()->vx * dt;
-		_sp_pev_prev(1) = _sp_pev(1) - _navigator->get_local_position()->vy * dt;
+		_sp_pev_prev(0) = _sp_pev(0) - _navigator_core.getVelNorthMPS() * dt;
+		_sp_pev_prev(1) = _sp_pev(1) - _navigator_core.getVelEastMPS() * dt;
 	}
 
 	_last_slewrate_time = now;

src/modules/navigator/precland.h

@@ -67,7 +67,7 @@ enum class PrecLandMode {
 class PrecLand : public MissionBlock, public ModuleParams
 {
 public:
-	PrecLand(Navigator *navigator);
+	PrecLand(Navigator *navigator, NavigatorCore &navigator_core);
 	~PrecLand() override = default;
 
 	void on_activation() override;

src/modules/navigator/rtl.cpp

@@ -52,8 +52,8 @@ static constexpr float DELAY_SIGMA = 0.01f;
 using namespace time_literals;
 using namespace math;
 
-RTL::RTL(Navigator *navigator) :
-	MissionBlock(navigator),
+RTL::RTL(Navigator *navigator, NavigatorCore &navigator_core) :
+	MissionBlock(navigator, navigator_core),
 	ModuleParams(navigator)
 {
 }
@@ -80,14 +80,14 @@ void RTL::find_RTL_destination()
 		return;
 	}
 
-	if (!_navigator->home_position_valid()) {
+	if (!_navigator_core.isHomeValid()) {
 		return;
 	}
 
 	_destination_check_time = hrt_absolute_time();
 
 	// get home position:
-	home_position_s &home_landing_position = *_navigator->get_home_position();
+	home_position_s &home_landing_position = _navigator_core.getHomePosition();
 
 	// get global position
 	const vehicle_global_position_s &global_position = *_navigator->get_global_position();
@@ -110,8 +110,8 @@ void RTL::find_RTL_destination()
 
 	_destination.type = RTL_DESTINATION_HOME;
 
-	const bool vtol_in_rw_mode = _navigator->get_vstatus()->is_vtol
-				     && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+	const bool vtol_in_rw_mode = _navigator_core.isVTOL()
+				     && _navigator_core.isRotaryWing();
 
 
 	// consider the mission landing if not RTL_HOME type set
@@ -237,8 +237,8 @@ void RTL::find_RTL_destination()
 void RTL::on_activation()
 {
 
-	_deny_mission_landing = _navigator->get_vstatus()->is_vtol
-				&& _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+	_deny_mission_landing = _navigator_core.isVTOL()
+				&& _navigator_core.isRotaryWing();
 
 	// output the correct message, depending on where the RTL destination is
 	switch (_destination.type) {
@@ -259,16 +259,15 @@ void RTL::on_activation()
 
 	_rtl_loiter_rad = _param_rtl_loiter_rad.get();
 
-	if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+	if (_navigator_core.isRotaryWing()) {
 		_rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get());
 
 	} else {
 		_rtl_alt = max(global_position.alt, max(_destination.alt,
-							_navigator->get_home_position()->alt + _param_rtl_return_alt.get()));
+							_navigator_core.getHomeAltAMSLMeter() + _param_rtl_return_alt.get()));
 	}
 
-
-	if (_navigator->get_land_detected()->landed) {
+	if (_navigator_core.getLanded()) {
 		// For safety reasons don't go into RTL if landed.
 		_rtl_state = RTL_STATE_LANDED;
 
@@ -343,7 +342,7 @@ void RTL::set_rtl_item()
 
 			// do not use LOITER_TO_ALT for rotary wing mode as it would then always climb to at least MIS_LTRMIN_ALT,
 			// even if current climb altitude is below (e.g. RTL immediately after take off)
-			if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+			if (_navigator_core.isRotaryWing()) {
 				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 
 			} else {
@@ -354,8 +353,8 @@ void RTL::set_rtl_item()
 			_mission_item.lon = gpos.lon;
 			_mission_item.altitude = _rtl_alt;
 			_mission_item.altitude_is_relative = false;
-			_mission_item.yaw = _navigator->get_local_position()->heading;
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+			_mission_item.yaw = _navigator_core.getTrueHeadingRad();
+			_mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter();
 			_mission_item.time_inside = 0.0f;
 			_mission_item.autocontinue = true;
 			_mission_item.origin = ORIGIN_ONBOARD;
@@ -383,7 +382,7 @@ void RTL::set_rtl_item()
 				_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _destination.lat, _destination.lon);
 			}
 
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+			_mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter();
 			_mission_item.time_inside = 0.0f;
 			_mission_item.autocontinue = true;
 			_mission_item.origin = ORIGIN_ONBOARD;
@@ -405,18 +404,18 @@ void RTL::set_rtl_item()
 			// Except for vtol which might be still off here and should point towards this location.
 			const float d_current = get_distance_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
 
-			if (_navigator->get_vstatus()->is_vtol && (d_current > _navigator->get_acceptance_radius())) {
+			if (_navigator_core.isVTOL() && (d_current > _navigator_core.getAcceptanceRadiusMeter())) {
 				_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
 
 			} else {
 				_mission_item.yaw = _destination.yaw;
 			}
 
-			if (_navigator->get_vstatus()->is_vtol) {
+			if (_navigator->getCore().isVTOL()) {
 				_mission_item.loiter_radius = _rtl_loiter_rad;
 			}
 
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+			_mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter();
 			_mission_item.time_inside = 0.0f;
 			_mission_item.autocontinue = true;
 			_mission_item.origin = ORIGIN_ONBOARD;
@@ -438,8 +437,8 @@ void RTL::set_rtl_item()
 			_mission_item.altitude = loiter_altitude;
 			_mission_item.altitude_is_relative = false;
 			_mission_item.yaw = _destination.yaw;
-			_mission_item.loiter_radius = _navigator->get_loiter_radius();
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+			_mission_item.loiter_radius = _navigator_core.getLoiterRadiusMeter();
+			_mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter();
 			_mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f);
 			_mission_item.autocontinue = autoland;
 			_mission_item.origin = ORIGIN_ONBOARD;
@@ -467,7 +466,7 @@ void RTL::set_rtl_item()
 			_mission_item.altitude = loiter_altitude;
 			_mission_item.altitude_is_relative = false;
 			_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+			_mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter();
 			_mission_item.time_inside = 0.0f;
 			_mission_item.autocontinue = true;
 			_mission_item.origin = ORIGIN_ONBOARD;
@@ -489,7 +488,7 @@ void RTL::set_rtl_item()
 			_mission_item.altitude = loiter_altitude;
 			_mission_item.altitude_is_relative = false;
 			_mission_item.yaw = get_bearing_to_next_waypoint(gpos.lat, gpos.lon, _mission_item.lat, _mission_item.lon);
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+			_mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter();
 			_mission_item.origin = ORIGIN_ONBOARD;
 			break;
 		}
@@ -502,7 +501,7 @@ void RTL::set_rtl_item()
 			_mission_item.yaw = _destination.yaw;
 			_mission_item.altitude = _destination.alt;
 			_mission_item.altitude_is_relative = false;
-			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+			_mission_item.acceptance_radius = _navigator_core.getAcceptanceRadiusMeter();
 			_mission_item.time_inside = 0.0f;
 			_mission_item.autocontinue = true;
 			_mission_item.origin = ORIGIN_ONBOARD;
@@ -552,8 +551,8 @@ void RTL::advance_rtl()
 	const bool descend_and_loiter = _param_rtl_land_delay.get() < -DELAY_SIGMA || _param_rtl_land_delay.get() > DELAY_SIGMA;
 
 	// vehicle is a vtol and currently in fixed wing mode
-	const bool vtol_in_fw_mode = _navigator->get_vstatus()->is_vtol
-				     && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
+	const bool vtol_in_fw_mode = _navigator_core.isVTOL()
+				     && _navigator_core.isFixedWing();
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB:
@@ -635,11 +634,11 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
 	// We choose the minimum height to be two times the distance from the land position in order to
 	// avoid the vehicle touching the ground while still moving horizontally.
 	const float return_altitude_min_outside_acceptance_rad_amsl = _destination.alt + 2.0f *
-			_navigator->get_acceptance_radius();
+			_navigator_core.getAcceptanceRadiusMeter();
 
 	float return_altitude_amsl = _destination.alt + _param_rtl_return_alt.get();
 
-	if (destination_dist <= _navigator->get_acceptance_radius()) {
+	if (destination_dist <= _navigator_core.getAcceptanceRadiusMeter()) {
 		return_altitude_amsl = _destination.alt + 2.0f * destination_dist;
 
 	} else {
@@ -663,7 +662,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
 
 void RTL::get_rtl_xy_z_speed(float &xy, float &z)
 {
-	uint8_t vehicle_type = _navigator->get_vstatus()->vehicle_type;
+	uint8_t vehicle_type = _navigator_core.getVehicleType();
 	// Caution: here be dragons!
 	// Use C API to allow this code to be compiled with builds that don't have FW/MC/Rover
 

src/modules/navigator/rtl.h

@@ -72,7 +72,7 @@ public:
 		RTL_DESTINATION_SAFE_POINT,
 	};
 
-	RTL(Navigator *navigator);
+	RTL(Navigator *navigator, NavigatorCore &navigator_core);
 
 	~RTL() = default;
 

src/modules/navigator/takeoff.cpp

@@ -41,8 +41,10 @@
 #include "takeoff.h"
 #include "navigator.h"
 
-Takeoff::Takeoff(Navigator *navigator) :
-	MissionBlock(navigator)
+using matrix::wrap_pi;
+
+Takeoff::Takeoff(Navigator *navigator, NavigatorCore &navigator_core) :
+	MissionBlock(navigator, navigator_core)
 {
 }
 
@@ -64,7 +66,6 @@ Takeoff::on_active()
 	} else if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
-
 		// set loiter item so position controllers stop doing takeoff logic
 		set_loiter_item(&_mission_item);
 		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
@@ -83,22 +84,22 @@ Takeoff::set_takeoff_position()
 
 	float min_abs_altitude;
 
-	if (_navigator->home_position_valid()) { //only use home position if it is valid
-		min_abs_altitude = _navigator->get_global_position()->alt + _navigator->get_takeoff_min_alt();
+	if (_navigator_core.isHomeValid()) { //only use home position if it is valid
+		min_abs_altitude = _navigator_core.getAltitudeAMSLMeters() + _navigator_core.getRelativeTakeoffMinAltitudeMeter();
 
 	} else { //e.g. flow
-		min_abs_altitude = _navigator->get_takeoff_min_alt();
+		min_abs_altitude = _navigator_core.getRelativeTakeoffMinAltitudeMeter();
 	}
 
 	// Use altitude if it has been set. If home position is invalid use min_abs_altitude
-	if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator->home_position_valid()) {
+	if (rep->current.valid && PX4_ISFINITE(rep->current.alt) && _navigator_core.isHomeValid()) {
 		abs_altitude = rep->current.alt;
 
 		// If the altitude suggestion is lower than home + minimum clearance, raise it and complain.
 		if (abs_altitude < min_abs_altitude) {
 			if (abs_altitude < min_abs_altitude - 0.1f) { // don't complain if difference is smaller than 10cm
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
-						     "Using minimum takeoff altitude: %.2f m", (double)_navigator->get_takeoff_min_alt());
+						     "Using minimum takeoff altitude: %.2f m", (double)_navigator_core.getRelativeTakeoffMinAltitudeMeter());
 			}
 
 			abs_altitude = min_abs_altitude;
@@ -108,13 +109,13 @@ Takeoff::set_takeoff_position()
 		// Use home + minimum clearance but only notify.
 		abs_altitude = min_abs_altitude;
 		mavlink_log_info(_navigator->get_mavlink_log_pub(),
-				 "Using minimum takeoff altitude: %.2f m", (double)_navigator->get_takeoff_min_alt());
+				 "Using minimum takeoff altitude: %.2f m", (double)_navigator_core.getRelativeTakeoffMinAltitudeMeter());
 	}
 
 
-	if (abs_altitude < _navigator->get_global_position()->alt) {
+	if (abs_altitude < _navigator_core.getAltitudeAMSLMeters()) {
 		// If the suggestion is lower than our current alt, let's not go down.
-		abs_altitude = _navigator->get_global_position()->alt;
+		abs_altitude = _navigator_core.getAltitudeAMSLMeters();
 		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already higher than takeoff altitude");
 	}
 

src/modules/navigator/takeoff.h

@@ -46,7 +46,7 @@
 class Takeoff : public MissionBlock
 {
 public:
-	Takeoff(Navigator *navigator);
+	Takeoff(Navigator *navigator, NavigatorCore &navigator_core);
 	~Takeoff() = default;
 
 	void on_activation() override;