navigator: wrote unit tests for Takeoff navigation mode

Signed-off-by: RomanBapst <bapstroman@gmail.com>
ROMFS: reduce LOGGER_BUF default to 8 kB on older boards
2026-05-27 04:00:05 +08:00 · 2021-05-12 18:39:34 +03:00 · 2021-05-12 17:06:33 +02:00 · 2021-05-11 13:14:42 -04:00 · 2021-05-11 18:12:56 +02:00 · 2021-05-11 08:34:23 +02:00
101 changed files with 1751 additions and 885 deletions
@@ -468,26 +468,27 @@ validate_module_configs:
 .PHONY: clean submodulesclean submodulesupdate gazeboclean distclean

 clean:
-	@rm -rf "$(SRC_DIR)"/build
-	@git submodule foreach git clean -df
+	@find "$(SRC_DIR)/build" -mindepth 1 -maxdepth 1 -type d -exec sh -c "echo {}; cmake --build {} -- clean || rm -rf {}" \; # use generated build system to clean, wipe build directory if it fails
+	@git submodule foreach git clean -dX --force # some submodules generate build artifacts in source

 submodulesclean:
 	@git submodule foreach --quiet --recursive git clean -ff -x -d
 	@git submodule update --quiet --init --recursive --force || true
 	@git submodule sync --recursive
-	@git submodule update --init --recursive --force
+	@git submodule update --init --recursive --force --jobs 4

 submodulesupdate:
-	@git submodule update --quiet --init --recursive || true
+	@git submodule update --quiet --init --recursive --jobs 4 || true
 	@git submodule sync --recursive
-	@git submodule update --init --recursive
+	@git submodule update --init --recursive --jobs 4

 gazeboclean:
 	@rm -rf ~/.gazebo/*

 distclean: gazeboclean
-	@git submodule deinit -f .
-	@git clean -ff -x -d -e ".cproject" -e ".idea" -e ".project" -e ".settings" -e ".vscode"
+	@git submodule deinit --force $(SRC_DIR)
+	@rm -rf "$(SRC_DIR)/build"
+	@git clean --force -X "$(SRC_DIR)/msg/" "$(SRC_DIR)/platforms/" "$(SRC_DIR)/posix-configs/" "$(SRC_DIR)/ROMFS/" "$(SRC_DIR)/src/" "$(SRC_DIR)/test/" "$(SRC_DIR)/Tools/"

 # Help / Error
 # --------------------------------------------------------------------
@@ -115,6 +115,7 @@ add_custom_command(
 set(romfs_extract_stamp ${CMAKE_CURRENT_BINARY_DIR}/romfs_extract.stamp)
 add_custom_command(
 	OUTPUT ${romfs_extract_stamp}
+	COMMAND ${CMAKE_COMMAND} -E remove_directory ${romfs_gen_root_dir}/*
 	COMMAND ${CMAKE_COMMAND} -E tar xf ${romfs_tar_file}
 	COMMAND ${CMAKE_COMMAND} -E touch ${romfs_extract_stamp}
 	WORKING_DIRECTORY ${romfs_gen_root_dir}
@@ -14,7 +14,8 @@

 param set-default EKF2_ARSP_THR 8
 param set-default EKF2_FUSE_BETA 1
-param set-default ASPD_STALL 10.0
+
+param set-default FW_AIRSPD_STALL 8

 param set-default FW_P_RMAX_NEG 20.0
 param set-default FW_P_RMAX_POS 60.0
@@ -30,7 +30,7 @@ set FRC /fs/microsd/etc/rc.txt
 set IOFW "/etc/extras/px4_io-v2_default.bin"
 set IO_PRESENT no
 set LOGGER_ARGS ""
-set LOGGER_BUF  14
+set LOGGER_BUF 8
 set MAV_TYPE none
 set MIXER none
 set MIXER_AUX none
@@ -63,7 +63,7 @@ CONFIG_HAVE_CXXINITIALIZE=y
 CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_IOB_NBUFFERS=48
-CONFIG_IOB_NCHAINS=16
+CONFIG_IOB_NCHAINS=18
 CONFIG_KINETIS_ADC0=y
 CONFIG_KINETIS_ADC1=y
 CONFIG_KINETIS_CRC=y
@@ -82,7 +82,7 @@ CONFIG_I2C_RESET=y
 CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
-CONFIG_LIBC_STRERROR=y
+CONFIG_LIBC_STRERROR=n
 CONFIG_LIBC_STRERROR_SHORT=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
@@ -33,6 +33,7 @@ px4_add_board(
 		imu/bosch/bmi088
 		imu/invensense/icm20602
 		imu/invensense/icm20948 # required for ak09916 mag
+		imu/invensense/icm20649
 		imu/invensense/icm42688p
 		irlock
 		lights # all available light drivers
@@ -8,13 +8,19 @@ board_adc start
 ina226 -X -b 1 -t 1 -k start
 ina226 -X -b 2 -t 2 -k start

-if ver hwtypecmp V5X90 V5X91 V5Xa0 V5Xa1
+if ver hwtypecmp V5X90 V5X91 V5X92 V5Xa0 V5Xa1 V5Xa2
 then
 	#SKYNODE base fmu board orientation

-	# Internal SPI BMI088
-	bmi088 -A -R 2 -s start
-	bmi088 -G -R 2 -s start
+	if ver hwtypecmp V5X90 V5X91 V5Xa0 V5Xa1
+	then
+		# Internal SPI BMI088
+		bmi088 -A -R 2 -s start
+		bmi088 -G -R 2 -s start
+	else
+		# Internal SPI bus ICM20649
+		icm20649 -s -R 4 start
+	fi

 	# Internal SPI bus ICM42688p
 	icm42688p -R 4 -s start
@@ -28,9 +34,15 @@ then
 else
 	#FMUv5Xbase board orientation

-	# Internal SPI BMI088
-	bmi088 -A -R 4 -s start
-	bmi088 -G -R 4 -s start
+	if ver hwtypecmp V5X00 V5X01
+	then
+		# Internal SPI BMI088
+		bmi088 -A -R 4 -s start
+		bmi088 -G -R 4 -s start
+	else
+		# Internal SPI bus ICM20649
+		icm20649 -s -R 6 start
+	fi

 	# Internal SPI bus ICM42688p
 	icm42688p -R 6 -s start
@@ -185,6 +185,18 @@
 #define HW_INFO_INIT           {'V','5','X','x', 'x',0}
 #define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
 #define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */
+#define BOARD_NUM_SPI_CFG_HW_VERSIONS 3
+// Base                   FMUM
+#define V5X00   HW_VER_REV(0x0,0x0) // FMUV5X,                 Rev 0
+#define V5X10   HW_VER_REV(0x1,0x0) // NO PX4IO,               Rev 0
+#define V5X01   HW_VER_REV(0x0,0x1) // FMUV5X I2C2 BMP388,     Rev 1
+#define V5X02   HW_VER_REV(0x0,0x2) // FMUV5X,                 Rev 2
+#define V5X90   HW_VER_REV(0x9,0x0) // NO USB,                 Rev 0
+#define V5X91   HW_VER_REV(0x9,0x1) // NO USB I2C2 BMP388,     Rev 1
+#define V5X92   HW_VER_REV(0x9,0x2) // NO USB I2C2 BMP388,     Rev 2
+#define V5Xa0   HW_VER_REV(0xa,0x0) // NO USB (Q),             Rev 0
+#define V5Xa1   HW_VER_REV(0xa,0x1) // NO USB (Q) I2C2 BMP388, Rev 1
+#define V5Xa2   HW_VER_REV(0xa,0x2) // NO USB (Q) I2C2 BMP388, Rev 2

 /* HEATER
 * PWM in future
@@ -175,12 +175,6 @@ stm32_boardinitialize(void)
 	const uint32_t gpio[] = PX4_GPIO_INIT_LIST;
 	px4_gpio_init(gpio, arraySize(gpio));

-	/* configure SPI interfaces (we can do this here as long as we only have a single SPI hw config version -
-	 * otherwise we need to move this after board_determine_hw_info()) */
-	static_assert(BOARD_NUM_SPI_CFG_HW_VERSIONS == 1, "Need to move the SPI initialization for multi-version support");
-
-	stm32_spiinitialize();
-
 	/* configure USB interfaces */

 	stm32_usbinitialize();
@@ -220,7 +214,6 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 	VDD_3V3_SD_CARD_EN(true);
 	VDD_5V_PERIPH_EN(true);
 	VDD_5V_HIPOWER_EN(true);
-	board_spi_reset(10, 0xffff);
 	VDD_3V3_SENSORS4_EN(true);
 	VDD_3V3_SPEKTRUM_POWER_EN(true);

@@ -237,6 +230,9 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 		syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n");
 	}

+	stm32_spiinitialize();
+
+	board_spi_reset(10, 0xffff);

 	/* configure the DMA allocator */

@@ -111,16 +111,19 @@ static const px4_hw_mft_item_t hw_mft_list_v0509[] = {
 };

 static px4_hw_mft_list_entry_t mft_lists[] = {
-	{0x0000, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)},
-	{0x0001, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)},
-	{0x0100, hw_mft_list_v0510, arraySize(hw_mft_list_v0510)},
-	{0x0900, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)},
-	{0x0901, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)},
-	{0x0a00, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)},
-	{0x0a01, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)},
+//  ver_rev
+	{V5X00, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X,                 Rev 0
+	{V5X01, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X,                 Rev 1
+	{V5X02, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // FMUV5X,                 Rev 2
+	{V5X10, hw_mft_list_v0510, arraySize(hw_mft_list_v0510)}, // NO PX4IO,               Rev 0
+	{V5X90, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB,                 Rev 0
+	{V5X91, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388,     Rev 1
+	{V5X92, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388,     Rev 2
+	{V5Xa0, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q),             Rev 0
+	{V5Xa1, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 1
+	{V5Xa2, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 2
 };

-
 /************************************************************************************
 * Name: board_query_manifest
 *
@@ -35,28 +35,77 @@
 #include <drivers/drv_sensor.h>
 #include <nuttx/spi/spi.h>

-constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
-	initSPIBus(SPI::Bus::SPI1, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
-	}, {GPIO::PortI, GPIO::Pin11}),
-	initSPIBus(SPI::Bus::SPI2, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}),
-	}, {GPIO::PortD, GPIO::Pin15}),
-	initSPIBus(SPI::Bus::SPI3, {
-		initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
-		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}),
-	}, {GPIO::PortE, GPIO::Pin7}),
-//	initSPIBus(SPI::Bus::SPI4, {
-//		// no devices
-// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
-//	}, {GPIO::PortG, GPIO::Pin8}),
-	initSPIBus(SPI::Bus::SPI5, {
-		initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+constexpr px4_spi_bus_all_hw_t px4_spi_buses_all_hw[BOARD_NUM_SPI_CFG_HW_VERSIONS] = {
+	initSPIHWVersion(HW_VER_REV(0, 0), {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}),
+		}, {GPIO::PortD, GPIO::Pin15}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//	initSPIBus(SPI::Bus::SPI4, {
+		//		// no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//	}, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
 	}),
-	initSPIBusExternal(SPI::Bus::SPI6, {
-		initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
-		initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+
+	initSPIHWVersion(HW_VER_REV(0, 1), {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}),
+		}, {GPIO::PortD, GPIO::Pin15}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin8}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+			initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortI, GPIO::Pin4}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
+	}),
+
+	initSPIHWVersion(HW_VER_REV(0, 2), {
+		initSPIBus(SPI::Bus::SPI1, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortI, GPIO::Pin9}, SPI::DRDY{GPIO::PortF, GPIO::Pin2}),
+		}, {GPIO::PortI, GPIO::Pin11}),
+		initSPIBus(SPI::Bus::SPI2, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortH, GPIO::Pin12}),
+		}, {GPIO::PortD, GPIO::Pin15}),
+		initSPIBus(SPI::Bus::SPI3, {
+			initSPIDevice(DRV_IMU_DEVTYPE_ICM20649, SPI::CS{GPIO::PortI, GPIO::Pin4}, SPI::DRDY{GPIO::PortI, GPIO::Pin7}),
+		}, {GPIO::PortE, GPIO::Pin7}),
+		//  initSPIBus(SPI::Bus::SPI4, {
+		//    // no devices
+		// TODO: if enabled, remove GPIO_VDD_3V3_SENSORS4_EN from board_config.h
+		//  }, {GPIO::PortG, GPIO::Pin8}),
+		initSPIBus(SPI::Bus::SPI5, {
+			initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortG, GPIO::Pin7})
+		}),
+		initSPIBusExternal(SPI::Bus::SPI6, {
+			initSPIConfigExternal(SPI::CS{GPIO::PortI, GPIO::Pin10}, SPI::DRDY{GPIO::PortD, GPIO::Pin11}),
+			initSPIConfigExternal(SPI::CS{GPIO::PortA, GPIO::Pin15}, SPI::DRDY{GPIO::PortD, GPIO::Pin12}),
+		}),
 	}),
 };

-static constexpr bool unused = validateSPIConfig(px4_spi_buses);
+static constexpr bool unused = validateSPIConfig(px4_spi_buses_all_hw);
@@ -89,6 +89,7 @@ bool high_latency_data_link_lost 			# Set to true if the high latency data link

 bool engine_failure				# Set to true if an engine failure is detected
 bool mission_failure				# Set to true if mission could not continue/finish
+bool geofence_violated

 uint8 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]

@@ -262,6 +262,7 @@

 #if defined(BOARD_HAS_HW_VERSIONING)
 #  define BOARD_HAS_VERSIONING 1
+#  define HW_VER_REV(v,r)       ((uint32_t)((v) & 0xff) << 8) | ((uint32_t)(r) & 0xff)
 #endif

 /* Default LED logical to color mapping */
@@ -71,7 +71,7 @@ struct px4_spi_bus_t {

 struct px4_spi_bus_all_hw_t {
 	px4_spi_bus_t buses[SPI_BUS_MAX_BUS_ITEMS];
-	int board_hw_version{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version), -1=unused
+	int board_hw_version_revision{-1}; ///< 0=default, >0 for a specific revision (see board_get_hw_version & board_get_hw_revision), -1=unused
 };

 #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1
@@ -39,14 +39,19 @@
 #if BOARD_NUM_SPI_CFG_HW_VERSIONS > 1
 void px4_set_spi_buses_from_hw_version()
 {
-	int hw_version = board_get_hw_version();
+#if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
+	int hw_version_revision = board_get_hw_version();
+#else
+	int hw_version_revision = board_get_hw_revision();
+#endif
+

 	for (int i = 0; i < BOARD_NUM_SPI_CFG_HW_VERSIONS; ++i) {
-		if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_version == 0) {
+		if (!px4_spi_buses && px4_spi_buses_all_hw[i].board_hw_version_revision == 0) {
 			px4_spi_buses = px4_spi_buses_all_hw[i].buses;
 		}

-		if (px4_spi_buses_all_hw[i].board_hw_version == hw_version) {
+		if (px4_spi_buses_all_hw[i].board_hw_version_revision == hw_version_revision) {
 			px4_spi_buses = px4_spi_buses_all_hw[i].buses;
 		}
 	}
@@ -9,7 +9,7 @@ param set-default SENS_IMU_MODE 1
 param set-default EKF2_MULTI_MAG 0
 param set-default SENS_MAG_MODE 1

-set LOGGER_BUF 12
+set LOGGER_BUF 8

 if param greater -s UAVCAN_ENABLE 1
 then
@@ -3,7 +3,7 @@
 # S32K1XX specific defaults
 #------------------------------------------------------------------------------

-set LOGGER_BUF 12
+set LOGGER_BUF 8

 if param greater -s UAVCAN_ENABLE 1
 then
@@ -9,7 +9,7 @@ param set-default SENS_IMU_MODE 1
 param set-default EKF2_MULTI_MAG 0
 param set-default SENS_MAG_MODE 1

-set LOGGER_BUF 12
+set LOGGER_BUF 8

 if param greater -s UAVCAN_ENABLE 1
 then
@@ -129,7 +129,8 @@ static inline constexpr SPI::bus_device_external_cfg_t initSPIConfigExternal(SPI
 struct px4_spi_bus_array_t {
 	px4_spi_bus_t item[SPI_BUS_MAX_BUS_ITEMS];
 };
-static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version, const px4_spi_bus_array_t &bus_items)
+static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version_revision,
+		const px4_spi_bus_array_t &bus_items)
 {
 	px4_spi_bus_all_hw_t ret{};

@@ -137,7 +138,7 @@ static inline constexpr px4_spi_bus_all_hw_t initSPIHWVersion(int hw_version, co
 		ret.buses[i] = bus_items.item[i];
 	}

-	ret.board_hw_version = hw_version;
+	ret.board_hw_version_revision = hw_version_revision;
 	return ret;
 }
 constexpr bool validateSPIConfig(const px4_spi_bus_t spi_buses_conf[SPI_BUS_MAX_BUS_ITEMS]);
@@ -530,11 +530,27 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],

 	} else {
 		for (int i = 0; i < (int)num_outputs; i++) {
-			if (outputs[i] == DSHOT_DISARM_VALUE) {
+
+			uint16_t output = outputs[i];
+
+			// DShot 3D splits the throttle ranges in two.
+			// This is in terms of DShot values, code below is in terms of actuator_output
+			// Direction 1) 48 is the slowest, 1047 is the fastest.
+			// Direction 2) 1049 is the slowest, 2047 is the fastest.
+			if (_param_dshot_3d_enable.get()) {
+				if (output >= _param_dshot_3d_dead_l.get() && output <= _param_dshot_3d_dead_h.get()) {
+					output = DSHOT_DISARM_VALUE;
+
+				} else if (output < 1000 && output > 0) { //Todo: allow actuator 0 or dshot 48 to be used
+					output = 999 - output;
+				}
+			}
+
+			if (output == DSHOT_DISARM_VALUE) {
 				up_dshot_motor_command(i, DShot_cmd_motor_stop, i == requested_telemetry_index);

 			} else {
-				up_dshot_motor_data_set(i, math::min(outputs[i], static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
+				up_dshot_motor_data_set(i, math::min(output, static_cast<uint16_t>(DSHOT_MAX_THROTTLE)),
 							i == requested_telemetry_index);
 			}
 		}
@@ -231,6 +231,9 @@ private:
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::DSHOT_CONFIG>)   _param_dshot_config,
 		(ParamFloat<px4::params::DSHOT_MIN>)    _param_dshot_min,
+		(ParamBool<px4::params::DSHOT_3D_ENABLE>) _param_dshot_3d_enable,
+		(ParamInt<px4::params::DSHOT_3D_DEAD_H>) _param_dshot_3d_dead_h,
+		(ParamInt<px4::params::DSHOT_3D_DEAD_L>) _param_dshot_3d_dead_l,
 		(ParamInt<px4::params::MOT_POLE_COUNT>) _param_mot_pole_count
 	)
 };
@@ -14,7 +14,7 @@ parameters:
                long: |
                    This enables/disables DShot. The different modes define different
                    speeds, for example DShot150 = 150kb/s. Not all ESCs support all modes.
-                    
+
                    Note: this enables DShot on the FMU outputs. For boards with an IO it is the
                    AUX outputs.
            type: enum
@@ -40,6 +40,37 @@ parameters:
            decimal: 2
            increment: 0.01
            default: 0.055
+        DSHOT_3D_ENABLE:
+            description:
+                short: Allows for 3d mode when using DShot and suitable mixer
+                long: |
+                    WARNING: ESC must be configured for 3D mode, and DSHOT_MIN set to 0.
+                    This splits the throttle ranges in two.
+                    Direction 1) 48 is the slowest, 1047 is the fastest.
+                    Direction 2) 1049 is the slowest, 2047 is the fastest.
+                    When mixer outputs 1000 or value inside DSHOT 3D deadband, DShot 0 is sent.
+            type: boolean
+            default: 0
+        DSHOT_3D_DEAD_H:
+            description:
+                short: DSHOT 3D deadband high
+                long: |
+                    When the actuator_output is between DSHOT_3D_DEAD_L and DSHOT_3D_DEAD_H, motor will not spin.
+                    This value is with respect to the mixer_module range (0-1999), not the DSHOT values.
+            type: int32
+            min: 1000
+            max: 1999
+            default: 1000
+        DSHOT_3D_DEAD_L:
+            description:
+                short: DSHOT 3D deadband low
+                long: |
+                    When the actuator_output is between DSHOT_3D_DEAD_L and DSHOT_3D_DEAD_H, motor will not spin.
+                    This value is with respect to the mixer_module range (0-1999), not the DSHOT values.
+            type: int32
+            min: 0
+            max: 1000
+            default: 1000
        MOT_POLE_COUNT: # only used by dshot so far, so keep it under the dshot group
            description:
                short: Number of magnetic poles of the motors
@@ -51,4 +82,3 @@ parameters:
                    Typical motors for 5 inch props have 14 poles.
            type: int32
            default: 14
-
@@ -75,7 +75,8 @@
 #include <linux/spi/spidev.h>
 #endif /* __PX4_LINUX */

-#define TIMEOUT_5HZ 500
+#define TIMEOUT_1HZ		1300	//!< Timeout time in mS, 1000 mS (1Hz) + 300 mS delta for error
+#define TIMEOUT_5HZ		500		//!< Timeout time in mS,  200 mS (5Hz) + 300 mS delta for error
 #define RATE_MEASUREMENT_PERIOD 5000000

 typedef enum {
@@ -837,8 +838,15 @@ GPS::run()
 			}

 			int helper_ret;
+			unsigned receive_timeout = TIMEOUT_5HZ;

-			while ((helper_ret = _helper->receive(TIMEOUT_5HZ)) > 0 && !should_exit()) {
+			if (ubx_mode == GPSDriverUBX::UBXMode::RoverWithMovingBase) {
+				/* The MB rover will wait as long as possible to compute a navigation solution,
+				 * possibly lowering the navigation rate all the way to 1 Hz while doing so. */
+				receive_timeout = TIMEOUT_1HZ;
+			}
+
+			while ((helper_ret = _helper->receive(receive_timeout)) > 0 && !should_exit()) {

 				if (helper_ret & 1) {
 					publish();
@@ -84,8 +84,6 @@ int PAW3902::init()
 		return PX4_ERROR;
 	}

-	Reset();
-
 	// force to low light mode (1) initially
 	ChangeMode(Mode::LowLight, true);

@@ -150,24 +148,6 @@ bool PAW3902::DataReadyInterruptDisable()
 	return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
 }

-bool PAW3902::Reset()
-{
-	DataReadyInterruptDisable();
-
-	// Power on reset
-	RegisterWrite(Register::Power_Up_Reset, 0x5A);
-	usleep(1000);
-
-	// Read from registers 0x02, 0x03, 0x04, 0x05 and 0x06 one time regardless of the motion state
-	RegisterRead(Register::Motion);
-	RegisterRead(Register::Delta_X_L);
-	RegisterRead(Register::Delta_X_H);
-	RegisterRead(Register::Delta_Y_L);
-	RegisterRead(Register::Delta_Y_H);
-
-	return true;
-}
-
 void PAW3902::exit_and_cleanup()
 {
 	DataReadyInterruptDisable();
@@ -183,6 +163,7 @@ bool PAW3902::ChangeMode(Mode newMode, bool force)

 		// Issue a soft reset
 		RegisterWrite(Register::Power_Up_Reset, 0x5A);
+		px4_usleep(1000);

 		uint32_t interval_us = 0;

@@ -206,6 +187,10 @@ bool PAW3902::ChangeMode(Mode newMode, bool force)
 			break;
 		}

+		EnableLed();
+
+		_discard_reading = 3;
+
 		if (DataReadyInterruptConfigure()) {
 			// backup schedule as a watchdog timeout
 			ScheduleDelayed(500_ms);
@@ -214,17 +199,6 @@ bool PAW3902::ChangeMode(Mode newMode, bool force)
 			ScheduleOnInterval(interval_us);
 		}

-		// Discard the first three motion data.
-		for (int i = 0; i < 3; i++) {
-			RegisterRead(Register::Motion);
-			RegisterRead(Register::Delta_X_L);
-			RegisterRead(Register::Delta_X_H);
-			RegisterRead(Register::Delta_Y_L);
-			RegisterRead(Register::Delta_Y_H);
-		}
-
-		EnableLed();
-
 		_mode = newMode;
 	}

@@ -232,11 +206,6 @@ bool PAW3902::ChangeMode(Mode newMode, bool force)
 	_low_to_superlow_counter = 0;
 	_low_to_bright_counter = 0;
 	_superlow_to_low_counter = 0;
-	// Approximate Resolution = (Register Value + 1) * (50 / 8450) ≈ 0.6% of data point in Figure 19
-	// The maximum register value is 0xA8. The minimum register value is 0.
-	uint8_t resolution = RegisterRead(Register::Resolution);
-	PX4_DEBUG("Resolution: %X", resolution);
-	PX4_DEBUG("Resolution is approx: %.3f", (double)((resolution + 1.0f) * (50.0f / 8450.0f)));

 	return true;
 }
@@ -582,9 +551,9 @@ void PAW3902::ModeSuperLowLight()
 void PAW3902::EnableLed()
 {
 	// Enable LED_N controls
-	RegisterWriteVerified(0x7F, 0x14);
-	RegisterWriteVerified(0x6F, 0x1c);
-	RegisterWriteVerified(0x7F, 0x00);
+	RegisterWrite(0x7F, 0x14);
+	RegisterWrite(0x6F, 0x1c);
+	RegisterWrite(0x7F, 0x00);
 }

 uint8_t PAW3902::RegisterRead(uint8_t reg, int retries)
@@ -663,11 +632,17 @@ void PAW3902::RunImpl()
 	// update for next iteration
 	_previous_collect_timestamp = timestamp_sample;

+	if (_discard_reading > 0) {
+		_discard_reading--;
+		ResetAccumulatedData();
+		return;
+	}
+
 	// check SQUAL & Shutter values
 	// To suppress false motion reports, discard Delta X and Delta Y values if the SQUAL and Shutter values meet the condition
-	// Bright Mode,			SQUAL < 0x19, Shutter ≥ 0x1FF0
-	// Low Light Mode,		SQUAL < 0x46, Shutter ≥ 0x1FF0
-	// Super Low Light Mode,	SQUAL < 0x55, Shutter ≥ 0x0BC0
+	// Bright Mode,          SQUAL < 0x19, Shutter ≥ 0x1FF0
+	// Low Light Mode,       SQUAL < 0x46, Shutter ≥ 0x1FF0
+	// Super Low Light Mode, SQUAL < 0x55, Shutter ≥ 0x0BC0
 	const uint16_t shutter = (buf.data.Shutter_Upper << 8) | buf.data.Shutter_Lower;

 	bool data_valid = true;
@@ -677,7 +652,6 @@ void PAW3902::RunImpl()
 		if ((buf.data.SQUAL < 0x19) && (shutter >= 0x1FF0)) {
 			// false motion report, discarding
 			perf_count(_false_motion_perf);
-			ResetAccumulatedData();
 			data_valid = false;
 		}

@@ -699,7 +673,6 @@ void PAW3902::RunImpl()
 		if ((buf.data.SQUAL < 0x46) && (shutter >= 0x1FF0)) {
 			// false motion report, discarding
 			perf_count(_false_motion_perf);
-			ResetAccumulatedData();
 			data_valid = false;
 		}

@@ -732,11 +705,14 @@ void PAW3902::RunImpl()
 		if ((buf.data.SQUAL < 0x55) && (shutter >= 0x0BC0)) {
 			// false motion report, discarding
 			perf_count(_false_motion_perf);
-			ResetAccumulatedData();
 			data_valid = false;
 		}

-		if (shutter < 0x03E8) {
+		if (shutter < 0x01F4) {
+			// should not operate with Shutter < 0x01F4 in Mode 2
+			ChangeMode(Mode::LowLight);
+
+		} else if (shutter < 0x03E8) {
 			// SuperLowLight -> LowLight
 			_superlow_to_low_counter++;

@@ -90,8 +90,6 @@ private:
 	void RegisterWrite(uint8_t reg, uint8_t data);
 	bool RegisterWriteVerified(uint8_t reg, uint8_t data, int retries = 5);

-	bool Reset();
-
 	void EnableLed();

 	void ModeBright();
@@ -124,6 +122,8 @@ private:

 	matrix::Dcmf	_rotation;

+	int             _discard_reading{3};
+
 	int		_flow_sum_x{0};
 	int		_flow_sum_y{0};

@@ -126,7 +126,7 @@ int16_t CanardNuttXCDev::receive(CanardFrame *received_frame)
 	// Any recieved CAN messages will cause the poll statement to unblock and run
 	// This way CAN read runs with minimal latency.
 	// Note that multiple messages may be received in a short time, so this will try to read any availible in a loop
-	::poll(&fds, 1, 10);
+	::poll(&fds, 1, 0);

 	// Only execute this part if can0 is changed.
 	if (fds.revents & POLLIN) {
@@ -162,12 +162,12 @@ int16_t CanardSocketCAN::transmit(const CanardFrame &txf, int timeout_ms)
 	_send_tv->tv_usec = txf.timestamp_usec % 1000000ULL;
 	_send_tv->tv_sec = (txf.timestamp_usec - _send_tv->tv_usec) / 1000000ULL;

-	return sendmsg(_fd, &_send_msg, 1000);
+	return sendmsg(_fd, &_send_msg, 0);
 }

 int16_t CanardSocketCAN::receive(CanardFrame *rxf)
 {
-	int32_t result = recvmsg(_fd, &_recv_msg, 1000);
+	int32_t result = recvmsg(_fd, &_recv_msg, MSG_DONTWAIT);

 	if (result < 0) {
 		return result;
@@ -39,16 +39,20 @@
 * @author Peter van der Perk <peter.vanderperk@nxp.com>
 */

+#define RETRY_COUNT 10
+
 #include "NodeManager.hpp"

 bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
 {
 	if (msg.allocated_node_id.count == 0) {
+		uint32_t i;
+
 		msg.allocated_node_id.count = 1;
 		msg.allocated_node_id.elements[0].value = CANARD_NODE_ID_UNSET;

 		/* Search for an available NodeID to assign */
-		for (uint32_t i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
+		for (i = 1; i < sizeof(nodeid_registry) / sizeof(nodeid_registry[0]); i++) {
 			if (i == _canard_instance.node_id) {
 				continue; // Don't give our NodeID to a node

@@ -63,6 +67,10 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
 			}
 		}

+		nodeid_registry[i].register_setup = false; // Re-instantiate register setup
+		nodeid_registry[i].register_index = 0;
+		nodeid_registry[i].retry_count = 0;
+
 		if (msg.allocated_node_id.elements[0].value != CANARD_NODE_ID_UNSET) {

 			PX4_INFO("Received NodeID allocation request assigning %i", msg.allocated_node_id.elements[0].value);
@@ -120,17 +128,15 @@ void NodeManager::HandleListResponse(CanardNodeID node_id, uavcan_register_List_
 			if (nodeid_registry[i].node_id == node_id) {
 				nodeid_registry[i].register_index++; // Increment index counter for next update()
 				nodeid_registry[i].register_setup = false;
+				nodeid_registry[i].retry_count = 0;
 			}
 		}

+		if (_access_request.setPortId(node_id, msg.name)) {
+			PX4_INFO("Set portID succesfull");

-		if (strncmp((char *)msg.name.name.elements, gps_uorb_register_name,
-			    msg.name.name.count) == 0) {
-			_access_request.setPortId(node_id, gps_uorb_register_name, 1235); //TODO configurable and combine with ParamManager.
-
-		} else if (strncmp((char *)msg.name.name.elements, bms_status_register_name,
-				   msg.name.name.count) == 0) { //Battery status publisher
-			_access_request.setPortId(node_id, bms_status_register_name, 1234); //TODO configurable and combine with ParamManager.
+		} else {
+			PX4_INFO("Register not found %.*s", msg.name.name.count, msg.name.name.elements);
 		}
 	}
 }
@@ -142,7 +148,11 @@ void NodeManager::update()
 			if (nodeid_registry[i].node_id != 0 && nodeid_registry[i].register_setup == false) {
 				//Setting up registers
 				_list_request.request(nodeid_registry[i].node_id, nodeid_registry[i].register_index);
-				nodeid_registry[i].register_setup = true;
+				nodeid_registry[i].retry_count++;
+
+				if (nodeid_registry[i].retry_count > RETRY_COUNT) {
+					nodeid_registry[i].register_setup = true; // Don't update anymore
+				}
 			}
 		}

@@ -63,12 +63,14 @@ typedef struct {
 	uint8_t   unique_id[16];
 	bool      register_setup;
 	uint16_t  register_index;
+	uint16_t  retry_count;
 } UavcanNodeEntry;

 class NodeManager
 {
 public:
-	NodeManager(CanardInstance &ins) : _canard_instance(ins), _access_request(ins), _list_request(ins) { };
+	NodeManager(CanardInstance &ins, UavcanParamManager &pmgr) : _canard_instance(ins), _access_request(ins, pmgr),
+		_list_request(ins) { };

 	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
 	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
@@ -89,8 +91,4 @@ private:
 	bool nodeRegisterSetup = 0;

 	hrt_abstime _register_request_last{0};
-
-	//TODO work this out
-	const char *gps_uorb_register_name = "uavcan.pub.gnss_uorb.id";
-	const char *bms_status_register_name = "uavcan.pub.battery_status.id";
 };
@@ -40,6 +40,7 @@
 */

 #include "ParamManager.hpp"
+#include <px4_platform_common/defines.h>

 bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_Value_1_0 &value)
 {
@@ -56,8 +57,16 @@ bool UavcanParamManager::GetParamByName(const char *param_name, uavcan_register_
 			case PARAM_TYPE_INT32: {
 					int32_t out_val {};
 					param_get(param_handle, &out_val);
-					value.integer32.value.elements[0] = out_val;
-					uavcan_register_Value_1_0_select_integer32_(&value);
+
+					if (uavcan_register_Value_1_0_is_natural16_(&value)) { //FIXME param rewrite
+						value.natural16.value.elements[0] = (uint16_t)out_val;
+						uavcan_register_Value_1_0_select_natural16_(&value);
+
+					} else {
+						value.integer32.value.elements[0] = out_val;
+						uavcan_register_Value_1_0_select_integer32_(&value);
+					}
+
 					break;
 				}

@@ -91,15 +100,23 @@ bool UavcanParamManager::GetParamByName(const uavcan_register_Name_1_0 &name, ua
 			switch (param_type(param_handle)) {
 			case PARAM_TYPE_INT32: {
 					int32_t out_val {};
-					param_set(param_handle, &out_val);
-					value.integer32.value.elements[0] = out_val;
-					uavcan_register_Value_1_0_select_integer32_(&value);
+					param_get(param_handle, &out_val);
+
+					if (uavcan_register_Value_1_0_is_natural16_(&value)) { //FIXME param rewrite
+						value.natural16.value.elements[0] = (uint16_t)out_val;
+						uavcan_register_Value_1_0_select_natural16_(&value);
+
+					} else {
+						value.integer32.value.elements[0] = out_val;
+						uavcan_register_Value_1_0_select_integer32_(&value);
+					}
+
 					break;
 				}

 			case PARAM_TYPE_FLOAT: {
 					float out_val {};
-					param_set(param_handle, &out_val);
+					param_get(param_handle, &out_val);
 					value.real32.value.elements[0] = out_val;
 					uavcan_register_Value_1_0_select_real32_(&value);
 					break;
@@ -63,7 +63,7 @@ public:

 private:

-	const UavcanParamBinder _uavcan_params[10] {
+	const UavcanParamBinder _uavcan_params[11] {
 		{"uavcan.pub.esc.0.id",              "UCAN1_ESC_PUB"},
 		{"uavcan.pub.servo.0.id",            "UCAN1_SERVO_PUB"},
 		{"uavcan.pub.gps.0.id",              "UCAN1_GPS_PUB"},
@@ -74,6 +74,7 @@ private:
 		{"uavcan.sub.battery_status.0.id",     "UCAN1_BMS_BS_PID"},
 		{"uavcan.sub.battery_parameters.0.id", "UCAN1_BMS_BP_PID"},
 		{"uavcan.sub.legacy_bms.0.id",         "UCAN1_LG_BMS_PID"},
+		{"uavcan.sub.uorb.sensor_gps.0.id",    "UCAN1_UORB_GPS"},
 		//{"uavcan.sub.bms.0.id",   "UCAN1_BMS0_PID"}, //FIXME instancing
 		//{"uavcan.sub.bms.1.id",   "UCAN1_BMS1_PID"},
 	};
@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   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 sensor_gps.hpp
+ *
+* Defines uORB over UAVCANv1 sensor_gps publisher
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <uORB/topics/sensor_gps.h>
+
+#include "../Publisher.hpp"
+
+class UORB_over_UAVCAN_sensor_gps_Publisher : public UavcanPublisher
+{
+public:
+	UORB_over_UAVCAN_sensor_gps_Publisher(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanPublisher(ins, pmgr, "sensor_gps", instance)
+	{};
+
+	// Update the uORB Subscription and broadcast a UAVCAN message
+	virtual void update() override
+	{
+		// Not sure if actuator_armed is a good indication of readiness but seems close to it
+		if (_sensor_gps_sub.updated() && _port_id != CANARD_PORT_ID_UNSET && _port_id != 0) {
+			sensor_gps_s gps_msg {};
+			_sensor_gps_sub.update(&gps_msg);
+
+			CanardTransfer transfer = {
+				.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+				.priority       = CanardPriorityNominal,
+				.transfer_kind  = CanardTransferKindMessage,
+				.port_id        = _port_id, // This is the subject-ID.
+				.remote_node_id = CANARD_NODE_ID_UNSET,
+				.transfer_id    = _transfer_id,
+				.payload_size   = sizeof(struct sensor_gps_s),
+				.payload        = &gps_msg,
+			};
+
+			if (result == 0) {
+				// set the data ready in the buffer and chop if needed
+				++_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+				result = canardTxPush(&_canard_instance, &transfer);
+			}
+		}
+	};
+
+private:
+	uORB::Subscription _sensor_gps_sub{ORB_ID(sensor_gps)};
+};
@@ -52,45 +52,55 @@
 class UavcanAccessServiceRequest
 {
 public:
-	UavcanAccessServiceRequest(CanardInstance &ins) :
-		_canard_instance(ins) { };
+	UavcanAccessServiceRequest(CanardInstance &ins, UavcanParamManager &pmgr) :
+		_canard_instance(ins), _param_manager(pmgr) { };

-	void setPortId(CanardNodeID node_id, const char *register_name, uint16_t port_id)
+	bool setPortId(CanardNodeID node_id, uavcan_register_Name_1_0 &name)
 	{
 		int result {0};

 		uavcan_register_Access_Request_1_0 request_msg;
-		strncpy((char *)&request_msg.name.name.elements[0], register_name, sizeof(uavcan_register_Name_1_0));
-		request_msg.name.name.count = strlen(register_name);
-
-		uavcan_register_Value_1_0_select_natural16_(&request_msg.value);
 		request_msg.value.natural16.value.count = 1;
-		request_msg.value.natural16.value.elements[0] = port_id;
+		uavcan_register_Value_1_0_select_natural16_(&request_msg.value); // Set to natural16 so that ParamManager casts type

-		uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];
+		//FIXME ParamManager only has notion of being either sub/pub have to find a portable way to address trhis
+		name.name.elements[7] = 's'; //HACK Change pub into sub

-		CanardTransfer transfer = {
-			.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
-			.priority       = CanardPriorityNominal,
-			.transfer_kind  = CanardTransferKindRequest,
-			.port_id        = uavcan_register_Access_1_0_FIXED_PORT_ID_,                // This is the subject-ID.
-			.remote_node_id = node_id,       // Messages cannot be unicast, so use UNSET.
-			.transfer_id    = access_request_transfer_id,
-			.payload_size   = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
-			.payload        = &request_payload_buffer,
-		};
+		if (_param_manager.GetParamByName(name, request_msg.value)) {
+			name.name.elements[7] = 'p'; //HACK Change sub into pub
+			memcpy(&request_msg.name, &name, sizeof(request_msg.name));

-		result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
+			uint8_t request_payload_buffer[uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_];

-		if (result == 0) {
-			// set the data ready in the buffer and chop if needed
-			++access_request_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
-			result = canardTxPush(&_canard_instance, &transfer);
+			CanardTransfer transfer = {
+				.timestamp_usec = hrt_absolute_time() + PUBLISHER_DEFAULT_TIMEOUT_USEC,
+				.priority       = CanardPriorityNominal,
+				.transfer_kind  = CanardTransferKindRequest,
+				.port_id        = uavcan_register_Access_1_0_FIXED_PORT_ID_,                // This is the subject-ID.
+				.remote_node_id = node_id,       // Messages cannot be unicast, so use UNSET.
+				.transfer_id    = access_request_transfer_id,
+				.payload_size   = uavcan_register_Access_Request_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_,
+				.payload        = &request_payload_buffer,
+			};
+
+			result = uavcan_register_Access_Request_1_0_serialize_(&request_msg, request_payload_buffer, &transfer.payload_size);
+
+			if (result == 0) {
+				// set the data ready in the buffer and chop if needed
+				++access_request_transfer_id;  // The transfer-ID shall be incremented after every transmission on this subject.
+				result = canardTxPush(&_canard_instance, &transfer);
+			}
+
+			return result > 0;
+
+		} else {
+			return false;
 		}
 	};

 private:
 	CanardInstance &_canard_instance;
 	CanardTransferID access_request_transfer_id = 0;
+	UavcanParamManager &_param_manager;

 };
@@ -68,25 +68,27 @@ public:

 			// Set _port_id from _uavcan_param
 			uavcan_register_Value_1_0 value;
-			_param_manager.GetParamByName(uavcan_param, value);
-			int32_t new_id = value.integer32.value.elements[0];

-			/* FIXME how about partial subscribing */
-			if (curSubj->_canard_sub._port_id != new_id) {
-				if (new_id == CANARD_PORT_ID_UNSET) {
-					// Cancel subscription
-					unsubscribe();
+			if (_param_manager.GetParamByName(uavcan_param, value)) {
+				int32_t new_id = value.integer32.value.elements[0];

-				} else {
-					if (curSubj->_canard_sub._port_id != CANARD_PORT_ID_UNSET) {
-						// Already active; unsubscribe first
+				/* FIXME how about partial subscribing */
+				if (curSubj->_canard_sub._port_id != new_id) {
+					if (new_id == CANARD_PORT_ID_UNSET) {
+						// Cancel subscription
 						unsubscribe();
-					}

-					// Subscribe on the new port ID
-					curSubj->_canard_sub._port_id = (CanardPortID)new_id;
-					PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub._port_id);
-					subscribe();
+					} else {
+						if (curSubj->_canard_sub._port_id != CANARD_PORT_ID_UNSET) {
+							// Already active; unsubscribe first
+							unsubscribe();
+						}
+
+						// Subscribe on the new port ID
+						curSubj->_canard_sub._port_id = (CanardPortID)new_id;
+						PX4_INFO("Subscribing %s.%d on port %d", curSubj->_subject_name, _instance, curSubj->_canard_sub._port_id);
+						subscribe();
+					}
 				}
 			}

@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ *   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 sensor_gps.hpp
+ *
+ * Defines uORB over UAVCANv1 sensor_gps subscriber
+ *
+ * @author Peter van der Perk <peter.vanderperk@nxp.com>
+ */
+
+#pragma once
+
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/PublicationMulti.hpp>
+
+#include "../DynamicPortSubscriber.hpp"
+
+class UORB_over_UAVCAN_sensor_gps_Subscriber : public UavcanDynamicPortSubscriber
+{
+public:
+	UORB_over_UAVCAN_sensor_gps_Subscriber(CanardInstance &ins, UavcanParamManager &pmgr, uint8_t instance = 0) :
+		UavcanDynamicPortSubscriber(ins, pmgr, "uorb.sensor_gps", instance) { };
+
+	void subscribe() override
+	{
+		// Subscribe to messages uORB sensor_gps payload over UAVCAN
+		canardRxSubscribe(&_canard_instance,
+				  CanardTransferKindMessage,
+				  _subj_sub._canard_sub._port_id,
+				  sizeof(struct sensor_gps_s),
+				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC * 10000,
+				  &_subj_sub._canard_sub);
+	};
+
+	void callback(const CanardTransfer &receive) override
+	{
+		//PX4_INFO("uORB sensor_gps Callback");
+
+		if (receive.payload_size == sizeof(struct sensor_gps_s)) {
+			sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
+			_sensor_gps_pub.publish(*gps_msg);
+
+		} else {
+			PX4_ERR("uORB over UAVCAN %s playload size mismatch got %d expected %d",
+				_subj_sub._subject_name, receive.payload_size, sizeof(struct sensor_gps_s));
+		}
+	};
+
+private:
+	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
+
+};
@@ -56,8 +56,7 @@ static void memFree(CanardInstance *const ins, void *const pointer) { o1heapFree
 UavcanNode::UavcanNode(CanardInterface *interface, uint32_t node_id) :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
-	_can_interface(interface)//,
-	// _subscribers{&_gps0_sub, &_gps1_sub, &_bms0_sub, &_bms1_sub}
+	_can_interface(interface)
 {
 	pthread_mutex_init(&_node_mutex, nullptr);

@@ -168,13 +167,6 @@ void UavcanNode::init()
 					  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
 					  &_heartbeat_subscription);

-			canardRxSubscribe(&_canard_instance, // uORB over UAVCAN GPS message
-					  CanardTransferKindMessage,
-					  gps_port_id,
-					  sizeof(struct sensor_gps_s),
-					  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
-					  &_drone_srv_gps_subscription);
-
 			for (auto &subscriber : _subscribers) {
 				subscriber->subscribe();
 			}
@@ -238,7 +230,69 @@ void UavcanNode::Run()

 	_node_manager.update();

-	// Transmitting
+	transmit();
+
+	/* Process received messages */
+
+	uint8_t data[64] {};
+	CanardFrame received_frame{};
+	received_frame.payload = &data;
+
+	while (_can_interface->receive(&received_frame) > 0) {
+		CanardTransfer receive{};
+		int32_t result = canardRxAccept(&_canard_instance, &received_frame, 0, &receive);
+
+		if (result < 0) {
+			// An error has occurred: either an argument is invalid or we've ran out of memory.
+			// It is possible to statically prove that an out-of-memory will never occur for a given application if
+			// the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
+			// Reception of an invalid frame is NOT an error.
+			PX4_ERR("Receive error %d\n", result);
+
+		} else if (result == 1) {
+			// A transfer has been received, process it.
+			// PX4_INFO("received Port ID: %d", receive.port_id);
+
+			if (receive.port_id > 0) {
+				// If not a fixed port ID, check any subscribers which may have registered it
+				for (auto &subscriber : _subscribers) {
+					if (subscriber->hasPortID(receive.port_id)) {
+						subscriber->callback(receive);
+					}
+				}
+
+				for (auto &subscriber : _dynsubscribers) {
+					if (subscriber->hasPortID(receive.port_id)) {
+						subscriber->callback(receive);
+					}
+				}
+			}
+
+			// Deallocate the dynamic memory afterwards.
+			_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
+
+		} else {
+			//PX4_INFO("RX canard %d", result);
+		}
+	}
+
+	// Pop canardTx queue to send out responses to requets
+	transmit();
+
+	perf_end(_cycle_perf);
+
+	if (_task_should_exit.load()) {
+		_can_interface->close();
+
+		ScheduleClear();
+		_instance = nullptr;
+	}
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
+void UavcanNode::transmit()
+{
 	// Look at the top of the TX queue.
 	for (const CanardFrame *txf = nullptr; (txf = canardTxPeek(&_canard_instance)) != nullptr;) {
 		// Attempt transmission only if the frame is not yet timed out while waiting in the TX queue.
@@ -268,67 +322,6 @@ void UavcanNode::Run()
 			}
 		}
 	}
-
-	/* Process received messages */
-
-	uint8_t data[64] {};
-	CanardFrame received_frame{};
-	received_frame.payload = &data;
-
-	/* FIXME this flawed we've to go through the whole loop to get the next frame in the buffer  */
-
-	if (_can_interface->receive(&received_frame) > 0) {
-
-		CanardTransfer receive{};
-		int32_t result = canardRxAccept(&_canard_instance, &received_frame, 0, &receive);
-
-		if (result < 0) {
-			// An error has occurred: either an argument is invalid or we've ran out of memory.
-			// It is possible to statically prove that an out-of-memory will never occur for a given application if
-			// the heap is sized correctly; for background, refer to the Robson's Proof and the documentation for O1Heap.
-			// Reception of an invalid frame is NOT an error.
-			PX4_ERR("Receive error %d\n", result);
-
-		} else if (result == 1) {
-			// A transfer has been received, process it.
-			// PX4_INFO("received Port ID: %d", receive.port_id);
-
-			if (receive.port_id == gps_port_id) {
-				result = handleUORBSensorGPS(receive);
-
-			} else if (receive.port_id > 0) {
-				// If not a fixed port ID, check any subscribers which may have registered it
-				for (auto &subscriber : _subscribers) {
-					if (subscriber->hasPortID(receive.port_id)) {
-						subscriber->callback(receive);
-					}
-				}
-
-				for (auto &subscriber : _dynsubscribers) {
-					if (subscriber->hasPortID(receive.port_id)) {
-						subscriber->callback(receive);
-					}
-				}
-			}
-
-			// Deallocate the dynamic memory afterwards.
-			_canard_instance.memory_free(&_canard_instance, (void *)receive.payload);
-
-		} else {
-			//PX4_INFO("RX canard %d\r\n", result);
-		}
-	}
-
-	perf_end(_cycle_perf);
-
-	if (_task_should_exit.load()) {
-		_can_interface->close();
-
-		ScheduleClear();
-		_instance = nullptr;
-	}
-
-	pthread_mutex_unlock(&_node_mutex);
 }

 void UavcanNode::print_info()
@@ -338,6 +331,13 @@ void UavcanNode::print_info()
 	perf_print_counter(_cycle_perf);
 	perf_print_counter(_interval_perf);

+	O1HeapDiagnostics heap_diagnostics = o1heapGetDiagnostics(uavcan_allocator);
+
+	PX4_INFO("Heap status %zu/%zu Peak alloc %zu Peak req %zu OOM count %" PRIu64,
+		 heap_diagnostics.allocated, heap_diagnostics.capacity,
+		 heap_diagnostics.peak_allocated, heap_diagnostics.peak_request_size,
+		 heap_diagnostics.oom_count);
+
 	for (auto &publisher : _publishers) {
 		publisher->printInfo();
 	}
@@ -346,6 +346,10 @@ void UavcanNode::print_info()
 		subscriber->printInfo();
 	}

+	for (auto &subscriber : _dynsubscribers) {
+		subscriber->printInfo();
+	}
+
 	_mixing_output.printInfo();
 	_esc_controller.printInfo();

@@ -442,16 +446,6 @@ void UavcanNode::sendHeartbeat()
 	}
 }

-int UavcanNode::handleUORBSensorGPS(const CanardTransfer &receive)
-{
-	PX4_INFO("NodeID %i GPS sensor msg", receive.remote_node_id);
-
-	sensor_gps_s *gps_msg = (sensor_gps_s *)receive.payload;
-
-	return _sensor_gps_pub.publish(*gps_msg) ? 0 : -1;
-}
-
-
 bool UavcanMixingInterface::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
 		unsigned num_control_groups_updated)
 {
@@ -78,6 +78,9 @@
 #include "Subscribers/NodeIDAllocationData.hpp"
 #include "Subscribers/LegacyBatteryInfo.hpp"

+// uORB over UAVCAN subscribers
+#include "Subscribers/uORB/sensor_gps.hpp"
+
 #include "ServiceClients/GetInfo.hpp"
 #include "ServiceClients/Access.hpp"

@@ -162,14 +165,11 @@ private:
 	void Run() override;
 	void fill_node_info();

+	void transmit();
+
 	// Sends a heartbeat at 1s intervals
 	void sendHeartbeat();

-	int handlePnpNodeIDAllocationData(const CanardTransfer &receive);
-	int handleRegisterList(const CanardTransfer &receive);
-	int handleRegisterAccess(const CanardTransfer &receive);
-	int handleUORBSensorGPS(const CanardTransfer &receive);
-
 	void *_uavcan_heap{nullptr};

 	CanardInterface *const _can_interface;
@@ -185,14 +185,9 @@ private:
 	pthread_mutex_t _node_mutex;

 	CanardRxSubscription _heartbeat_subscription;
-	CanardRxSubscription _drone_srv_gps_subscription;

-	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};

-	uORB::Publication<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};
-	uORB::Publication<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
-
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")};
 	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")};

@@ -201,15 +196,6 @@ private:
 	hrt_abstime _uavcan_node_heartbeat_last{0};
 	CanardTransferID _uavcan_node_heartbeat_transfer_id{0};

-	/* Temporary hardcoded port IDs used by the register interface
-	* for demo purposes untill we have nice interface (QGC or latter)
-	* to configure the nodes
-	*/
-	const uint16_t gps_port_id = 1235;
-
-	CanardTransferID _uavcan_register_list_request_transfer_id{0};
-	CanardTransferID _uavcan_register_access_request_transfer_id{0};
-
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::UAVCAN_V1_ENABLE>) _param_uavcan_v1_enable,
 		(ParamInt<px4::params::UAVCAN_V1_ID>) _param_uavcan_v1_id,
@@ -220,7 +206,7 @@ private:

 	UavcanParamManager _param_manager;

-	NodeManager _node_manager {_canard_instance};
+	NodeManager _node_manager {_canard_instance, _param_manager};

 	UavcanGnssPublisher _gps_pub {_canard_instance, _param_manager};

@@ -238,6 +224,8 @@ private:
 	UavcanLegacyBatteryInfoSubscriber  _legacybms_sub {_canard_instance, _param_manager, 0};
 	UavcanNodeIDAllocationDataSubscriber _nodeid_sub {_canard_instance, _node_manager};

+	UORB_over_UAVCAN_sensor_gps_Subscriber _sensor_gps_sub {_canard_instance, _param_manager, 0};
+
 	UavcanGetInfoResponse _getinfo_rsp {_canard_instance};
 	UavcanAccessResponse  _access_rsp {_canard_instance, _param_manager};

@@ -245,7 +233,7 @@ private:
 	UavcanListServiceReply   _list_service {_canard_instance, _node_manager};

 	// Subscriber objects: Any object used to bridge a UAVCAN message to a uORB message
-	UavcanDynamicPortSubscriber *_dynsubscribers[5] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_esc_sub, &_legacybms_sub}; /// TODO: turn into List<UavcanSubscription*>
+	UavcanDynamicPortSubscriber *_dynsubscribers[6] {&_gps0_sub, &_gps1_sub, &_bms0_sub, &_esc_sub, &_legacybms_sub, &_sensor_gps_sub}; /// TODO: turn into List<UavcanSubscription*>
 	UavcanBaseSubscriber *_subscribers[5] {&_nodeid_sub, &_getinfo_rsp, &_access_rsp, &_access_service, &_list_service}; /// TODO: turn into List<UavcanSubscription*>

 	UavcanMixingInterface _mixing_output {_node_mutex, _esc_controller};
@@ -103,3 +103,6 @@ PARAM_DEFINE_INT32(UCAN1_LG_BMS_PID, 0);
 PARAM_DEFINE_INT32(UCAN1_ESC_PUB, 0);
 PARAM_DEFINE_INT32(UCAN1_GPS_PUB, 0);
 PARAM_DEFINE_INT32(UCAN1_SERVO_PUB, 0);
+
+// uORB over UAVCAN
+PARAM_DEFINE_INT32(UCAN1_UORB_GPS, 0);
@@ -381,7 +381,7 @@ static int canard_daemon(int argc, char *argv[])
 	/* Init UAVCAN register interfaces */
 	uavcan_node_GetInfo_Response_1_0 node_information; // TODO ADD INFO
 	uavcan_register_interface_init(&ins, &node_information);
-	uavcan_register_interface_add_entry("gnss_uorb", set_gps_uorb_port_id, get_gps_uorb_port_id);
+	uavcan_register_interface_add_entry("uorb.sensor_gps.0", set_gps_uorb_port_id, get_gps_uorb_port_id);
 	uavcan_register_interface_add_entry("gnss_fix", set_gps_fix_port_id, get_gps_fix_port_id);
 	uavcan_register_interface_add_entry("gnss_aux", set_gps_aux_port_id, get_gps_aux_port_id);

@@ -238,10 +238,13 @@ int32_t uavcan_register_interface_list_response(CanardInstance *ins, CanardTrans

 	// Reponse magic start

-	if (msg.index <= register_list_size) {
+	if (msg.index < register_list_size) {
 		response_msg.name.name.count = sprintf(response_msg.name.name.elements,
 						       "uavcan.pub.%s.id",
 						       register_list[msg.index].name);
+
+	} else {
+		response_msg.name.name.count = 0;
 	}

 	//TODO more option then pub (sub rate
@@ -164,6 +164,14 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}

+	// 2021-04-30: translate ASPD_STALL to FW_AIRSPD_STALL
+	{
+		if (strcmp("ASPD_STALL", node->name) == 0) {
+			strcpy(node->name, "FW_AIRSPD_STALL");
+			PX4_INFO("copying %s -> %s", "ASPD_STALL", "FW_AIRSPD_STALL");
+		}
+	}
+
 	// translate (SPI) calibration ID parameters. This can be removed after the next release (current release=1.10)

 	if (node->type != BSON_INT32) {
@@ -171,7 +171,8 @@ private:
 		(ParamFloat<px4::params::ASPD_FS_INTEG>) _tas_innov_integ_threshold, /**< innovation check integrator threshold */
 		(ParamInt<px4::params::ASPD_FS_T_STOP>) _checks_fail_delay, /**< delay to declare airspeed invalid */
 		(ParamInt<px4::params::ASPD_FS_T_START>) _checks_clear_delay, /**<  delay to declare airspeed valid again */
-		(ParamFloat<px4::params::ASPD_STALL>) _airspeed_stall /**<  stall speed*/
+
+		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall
 	)

 	void 		init(); 	/**< initialization of the airspeed validator instances */
@@ -295,8 +296,6 @@ AirspeedModule::Run()
 		update_params();
 	}

-
-
 	bool armed = (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);

 	// check for new connected airspeed sensors as long as we're disarmed
@@ -315,7 +314,7 @@ AirspeedModule::Run()
 		// for fixed-wing landings.
 		const bool in_air_fixed_wing = !_vehicle_land_detected.landed &&
 					       _position_setpoint.type != position_setpoint_s::SETPOINT_TYPE_LAND &&
-					       _vehicle_status.system_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
+					       _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;

 		// Prepare data for airspeed_validator
 		struct airspeed_validator_update_data input_data = {};
@@ -349,7 +348,9 @@ AirspeedModule::Run()
 				input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius;

 				// takeoff situation is active from start till one of the sensors' IAS or groundspeed_CAS is above stall speed
-				if (airspeed_raw.indicated_airspeed_m_s > _airspeed_stall.get() || _ground_minus_wind_CAS > _airspeed_stall.get()) {
+				if (_in_takeoff_situation &&
+				    (airspeed_raw.indicated_airspeed_m_s > _param_fw_airspd_stall.get() ||
+				     _ground_minus_wind_CAS > _param_fw_airspd_stall.get())) {
 					_in_takeoff_situation = false;
 				}

@@ -410,7 +411,7 @@ void AirspeedModule::update_params()
 		_airspeed_validator[i].set_tas_innov_integ_threshold(_tas_innov_integ_threshold.get());
 		_airspeed_validator[i].set_checks_fail_delay(_checks_fail_delay.get());
 		_airspeed_validator[i].set_checks_clear_delay(_checks_clear_delay.get());
-		_airspeed_validator[i].set_airspeed_stall(_airspeed_stall.get());
+		_airspeed_validator[i].set_airspeed_stall(_param_fw_airspd_stall.get());
 	}

 	// if the airspeed scale estimation is enabled and the airspeed is valid,
@@ -184,15 +184,3 @@ PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2);
 * @max 1000
 */
 PARAM_DEFINE_INT32(ASPD_FS_T_START, -1);
-
-/**
- * Airspeed fault detection stall airspeed
- *
- * This is the minimum indicated airspeed at which the wing can produce 1g of lift.
- * It is used by the airspeed sensor fault detection and failsafe calculation to detect a
- * significant airspeed low measurement error condition and should be set based on flight test for reliable operation.
- *
- * @group Airspeed Validator
- * @unit m/s
- */
-PARAM_DEFINE_FLOAT(ASPD_STALL, 10.0f);
@@ -184,10 +184,10 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		int32_t max_airspeed_check_en = 0;
 		param_get(param_find("COM_ARM_ARSP_EN"), &max_airspeed_check_en);

-		float airspeed_stall = 10.0f;
-		param_get(param_find("ASPD_STALL"), &airspeed_stall);
+		float airspeed_trim = 10.0f;
+		param_get(param_find("FW_AIRSPD_TRIM"), &airspeed_trim);

-		const float arming_max_airspeed_allowed = airspeed_stall / 2.0f; // set to half of stall speed
+		const float arming_max_airspeed_allowed = airspeed_trim / 2.0f; // set to half of trim airspeed

 		if (!airspeedCheck(mavlink_log_pub, status, optional, report_failures, prearm, (bool)max_airspeed_check_en,
 				   arming_max_airspeed_allowed)
@@ -86,6 +86,7 @@ public:
 		bool esc_check = false;
 		bool global_position = false;
 		bool mission = false;
+		bool geofence = false;
 	};

 	static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
@@ -65,7 +65,12 @@ bool PreFlightCheck::accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_s

 		device_id = accel.get().device_id;

-		calibration_valid = (calibration::FindCalibrationIndex("ACC", device_id) >= 0);
+		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
+			calibration_valid = true;
+
+		} else {
+			calibration_valid = (calibration::FindCalibrationIndex("ACC", device_id) >= 0);
+		}

 		if (!calibration_valid) {
 			if (report_fail) {
@@ -64,7 +64,12 @@ bool PreFlightCheck::gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &

 		device_id = gyro.get().device_id;

-		calibration_valid = (calibration::FindCalibrationIndex("GYRO", device_id) >= 0);
+		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
+			calibration_valid = true;
+
+		} else {
+			calibration_valid = (calibration::FindCalibrationIndex("GYRO", device_id) >= 0);
+		}

 		if (!calibration_valid) {
 			if (report_fail) {
@@ -66,7 +66,12 @@ bool PreFlightCheck::magnetometerCheck(orb_advert_t *mavlink_log_pub, vehicle_st

 		device_id = magnetometer.get().device_id;

-		calibration_valid = (calibration::FindCalibrationIndex("MAG", device_id) >= 0);
+		if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
+			calibration_valid = true;
+
+		} else {
+			calibration_valid = (calibration::FindCalibrationIndex("MAG", device_id) >= 0);
+		}

 		if (!calibration_valid) {
 			if (report_fail) {
@@ -63,6 +63,11 @@ bool PreFlightCheck::powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_sta
 		return true;
 	}

+	if (status.hil_state == vehicle_status_s::HIL_STATE_ON) {
+		// Ignore power check in HITL.
+		return true;
+	}
+
 	uORB::SubscriptionData<system_power_s> system_power_sub{ORB_ID(system_power)};
 	system_power_sub.update();
 	const system_power_s &system_power = system_power_sub.get();
@@ -168,6 +168,14 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
 		}
 	}

+	if (arm_requirements.geofence && status.geofence_violated) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Arming denied, vehicle outside geofence");
+		}
+
+		prearm_ok = false;
+	}
+
 	// Arm Requirements: authorization
 	// check last, and only if everything else has passed
 	if (arm_requirements.arm_authorization && prearm_ok) {
@@ -1733,6 +1733,7 @@ Commander::run()
 			_arm_requirements.esc_check = _param_escs_checks_required.get();
 			_arm_requirements.global_position = !_param_arm_without_gps.get();
 			_arm_requirements.mission = _param_arm_mission_required.get();
+			_arm_requirements.geofence = _param_geofence_action.get() > geofence_result_s::GF_ACTION_NONE;

 			/* flight mode slots */
 			_flight_mode_slots[0] = _param_fltmode_1.get();
@@ -2114,6 +2115,7 @@ Commander::run()

 		/* start geofence result check */
 		_geofence_result_sub.update(&_geofence_result);
+		_status.geofence_violated = _geofence_result.geofence_violated;

 		const bool in_low_battery_failsafe = _battery_warning > battery_status_s::BATTERY_WARNING_LOW;

@@ -199,7 +199,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
 	/* wait 500 ms to ensure parameter propagated through the system */
 	px4_usleep(500 * 1000);

-	calibration_log_critical(mavlink_log_pub, "[cal] Blow across front of pitot without touching");
+	calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot without touching");

 	calibration_counter = 0;

@@ -989,7 +989,7 @@ PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f);
 /**
 * Enable preflight check for maximal allowed airspeed when arming.
 *
-* Deny arming if the current airspeed measurement is greater than half the stall speed (ASPD_STALL).
+* Deny arming if the current airspeed measurement is greater than half the cruise airspeed (FW_AIRSPD_TRIM).
 * Excessive airspeed measurements on ground are either caused by wind or bad airspeed calibration.
 *
 * @group Commander
@@ -467,6 +467,18 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta

 calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_mask)
 {
+	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
+	const unsigned orb_mag_count = orb_group_count(ORB_ID(sensor_mag));
+
+	// Warn that we will not calibrate more than MAX_GYROS gyroscopes
+	if (orb_mag_count > MAX_MAGS) {
+		calibration_log_critical(mavlink_log_pub, "Detected %u mags, but will calibrate only %u", orb_mag_count, MAX_MAGS);
+
+	} else if (orb_mag_count < 1) {
+		calibration_log_critical(mavlink_log_pub, "No mags found");
+		return calibrate_return_error;
+	}
+
 	calibrate_return result = calibrate_return_ok;

 	mag_worker_data_t worker_data{};
@@ -1742,7 +1742,8 @@ int EKF2::task_spawn(int argc, char *argv[])

 		while ((multi_instances_allocated < multi_instances)
 		       && (vehicle_status_sub.get().arming_state != vehicle_status_s::ARMING_STATE_ARMED)
-		       && (hrt_elapsed_time(&time_started) < 30_s)) {
+		       && ((hrt_elapsed_time(&time_started) < 30_s)
+			   || (vehicle_status_sub.get().hil_state == vehicle_status_s::HIL_STATE_ON))) {

 			vehicle_status_sub.update();

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -37,7 +37,6 @@ using namespace time_literals;
 using matrix::Quatf;
 using matrix::Vector2f;
 using math::constrain;
-using math::max;
 using math::radians;

 EKF2Selector::EKF2Selector() :
@@ -118,12 +117,6 @@ bool EKF2Selector::SelectInstance(uint8_t ekf_instance)
 			_instance[i].relative_test_ratio = 0;
 		}

-		// publish new data immediately with resets
-		PublishVehicleAttitude(true);
-		PublishVehicleLocalPosition(true);
-		PublishVehicleGlobalPosition(true);
-		PublishWindEstimate(true);
-
 		return true;
 	}

@@ -322,6 +315,11 @@ bool EKF2Selector::UpdateErrorScores()
 				if (error_delta > 0 || error_delta < -threshold) {
 					_instance[i].relative_test_ratio += error_delta;
 					_instance[i].relative_test_ratio = constrain(_instance[i].relative_test_ratio, -_rel_err_score_lim, _rel_err_score_lim);
+
+					if ((error_delta < -threshold) && (_instance[i].relative_test_ratio < 1.f)) {
+						// increase status publication rate if there's movement towards a potential instance change
+						_selector_status_publish = true;
+					}
 				}
 			}
 		}
@@ -330,104 +328,152 @@ bool EKF2Selector::UpdateErrorScores()
 	return (primary_updated || updated);
 }

-void EKF2Selector::PublishVehicleAttitude(bool reset)
+void EKF2Selector::PublishVehicleAttitude()
 {
+	// selected estimator_attitude -> vehicle_attitude
 	vehicle_attitude_s attitude;

-	if (_instance[_selected_instance].estimator_attitude_sub.copy(&attitude)) {
-		if (reset) {
-			// on reset compute deltas from last published data
-			++_quat_reset_counter;
+	if (_instance[_selected_instance].estimator_attitude_sub.update(&attitude)) {
+		bool instance_change = false;

-			_delta_q_reset = (Quatf(attitude.q) * Quatf(_attitude_last.q).inversed()).normalized();
-
-			// ensure monotonically increasing timestamp_sample through reset
-			attitude.timestamp_sample = max(attitude.timestamp_sample, _attitude_last.timestamp_sample);
-
-		} else {
-			// otherwise propogate deltas from estimator data while maintaining the overall reset counts
-			if (attitude.quat_reset_counter > _attitude_last.quat_reset_counter) {
-				++_quat_reset_counter;
-				_delta_q_reset = Quatf{attitude.delta_q_reset};
-			}
+		if (_instance[_selected_instance].estimator_attitude_sub.get_instance() != _attitude_instance_prev) {
+			_attitude_instance_prev = _instance[_selected_instance].estimator_attitude_sub.get_instance();
+			instance_change = true;
 		}

-		// save last primary estimator_attitude
+		if (_attitude_last.timestamp != 0) {
+			if (!instance_change && (attitude.quat_reset_counter == _attitude_last.quat_reset_counter + 1)) {
+				// propogate deltas from estimator data while maintaining the overall reset counts
+				++_quat_reset_counter;
+				_delta_q_reset = Quatf{attitude.delta_q_reset};
+
+			} else if (instance_change || (attitude.quat_reset_counter != _attitude_last.quat_reset_counter)) {
+				// on reset compute deltas from last published data
+				++_quat_reset_counter;
+				_delta_q_reset = (Quatf(attitude.q) * Quatf(_attitude_last.q).inversed()).normalized();
+			}
+
+		} else {
+			_quat_reset_counter = attitude.quat_reset_counter;
+			_delta_q_reset = Quatf{attitude.delta_q_reset};
+		}
+
+		bool publish = true;
+
+		// ensure monotonically increasing timestamp_sample through reset, don't publish
+		//  estimator's attitude for system (vehicle_attitude) if it's stale
+		if ((attitude.timestamp_sample <= _attitude_last.timestamp_sample)
+		    || (attitude.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+
+			publish = false;
+		}
+
+		// save last primary estimator_attitude as published with original resets
 		_attitude_last = attitude;

-		// republish with total reset count and current timestamp
-		attitude.quat_reset_counter = _quat_reset_counter;
-		_delta_q_reset.copyTo(attitude.delta_q_reset);
+		if (publish) {
+			// republish with total reset count and current timestamp
+			attitude.quat_reset_counter = _quat_reset_counter;
+			_delta_q_reset.copyTo(attitude.delta_q_reset);

-		attitude.timestamp = hrt_absolute_time();
-		_vehicle_attitude_pub.publish(attitude);
-
-		_instance[_selected_instance].timestamp_sample_last = attitude.timestamp_sample;
+			attitude.timestamp = hrt_absolute_time();
+			_vehicle_attitude_pub.publish(attitude);
+		}
 	}
 }

-void EKF2Selector::PublishVehicleLocalPosition(bool reset)
+void EKF2Selector::PublishVehicleLocalPosition()
 {
-	// vehicle_local_position
+	// selected estimator_local_position -> vehicle_local_position
 	vehicle_local_position_s local_position;

-	if (_instance[_selected_instance].estimator_local_position_sub.copy(&local_position)) {
-		if (reset) {
-			// on reset compute deltas from last published data
-			++_xy_reset_counter;
-			++_z_reset_counter;
-			++_vxy_reset_counter;
-			++_vz_reset_counter;
-			++_heading_reset_counter;
+	if (_instance[_selected_instance].estimator_local_position_sub.update(&local_position)) {
+		bool instance_change = false;

-			_delta_xy_reset = Vector2f{local_position.x, local_position.y} - Vector2f{_local_position_last.x, _local_position_last.y};
-			_delta_z_reset = local_position.z - _local_position_last.z;
-			_delta_vxy_reset = Vector2f{local_position.vx, local_position.vy} - Vector2f{_local_position_last.vx, _local_position_last.vy};
-			_delta_vz_reset = local_position.vz - _local_position_last.vz;
-			_delta_heading_reset = matrix::wrap_2pi(local_position.heading - _local_position_last.heading);
-
-			// ensure monotonically increasing timestamp_sample through reset
-			local_position.timestamp_sample = max(local_position.timestamp_sample, _local_position_last.timestamp_sample);
-
-		} else {
-			// otherwise propogate deltas from estimator data while maintaining the overall reset counts
+		if (_instance[_selected_instance].estimator_local_position_sub.get_instance() != _local_position_instance_prev) {
+			_local_position_instance_prev = _instance[_selected_instance].estimator_local_position_sub.get_instance();
+			instance_change = true;
+		}

+		if (_local_position_last.timestamp != 0) {
 			// XY reset
-			if (local_position.xy_reset_counter > _local_position_last.xy_reset_counter) {
+			if (!instance_change && (local_position.xy_reset_counter == _local_position_last.xy_reset_counter + 1)) {
 				++_xy_reset_counter;
 				_delta_xy_reset = Vector2f{local_position.delta_xy};
+
+			} else if (instance_change || (local_position.xy_reset_counter != _local_position_last.xy_reset_counter)) {
+				++_xy_reset_counter;
+				_delta_xy_reset = Vector2f{local_position.x, local_position.y} - Vector2f{_local_position_last.x, _local_position_last.y};
 			}

 			// Z reset
-			if (local_position.z_reset_counter > _local_position_last.z_reset_counter) {
+			if (!instance_change && (local_position.z_reset_counter == _local_position_last.z_reset_counter + 1)) {
 				++_z_reset_counter;
 				_delta_z_reset = local_position.delta_z;
+
+			} else if (instance_change || (local_position.z_reset_counter != _local_position_last.z_reset_counter)) {
+				++_z_reset_counter;
+				_delta_z_reset = local_position.z - _local_position_last.z;
 			}

 			// VXY reset
-			if (local_position.vxy_reset_counter > _local_position_last.vxy_reset_counter) {
+			if (!instance_change && (local_position.vxy_reset_counter == _local_position_last.vxy_reset_counter + 1)) {
 				++_vxy_reset_counter;
 				_delta_vxy_reset = Vector2f{local_position.delta_vxy};
+
+			} else if (instance_change || (local_position.vxy_reset_counter != _local_position_last.vxy_reset_counter)) {
+				++_vxy_reset_counter;
+				_delta_vxy_reset = Vector2f{local_position.vx, local_position.vy} - Vector2f{_local_position_last.vx, _local_position_last.vy};
 			}

 			// VZ reset
-			if (local_position.vz_reset_counter > _local_position_last.vz_reset_counter) {
+			if (!instance_change && (local_position.vz_reset_counter == _local_position_last.vz_reset_counter + 1)) {
 				++_vz_reset_counter;
 				_delta_vz_reset = local_position.delta_vz;
+
+			} else if (instance_change || (local_position.vz_reset_counter != _local_position_last.vz_reset_counter)) {
+				++_vz_reset_counter;
+				_delta_vz_reset = local_position.vz - _local_position_last.vz;
 			}

 			// heading reset
-			if (local_position.heading_reset_counter > _local_position_last.heading_reset_counter) {
+			if (!instance_change && (local_position.heading_reset_counter == _local_position_last.heading_reset_counter + 1)) {
 				++_heading_reset_counter;
 				_delta_heading_reset = local_position.delta_heading;
+
+			} else if (instance_change || (local_position.heading_reset_counter != _local_position_last.heading_reset_counter)) {
+				++_heading_reset_counter;
+				_delta_heading_reset = matrix::wrap_pi(local_position.heading - _local_position_last.heading);
 			}
+
+		} else {
+			_xy_reset_counter = local_position.xy_reset_counter;
+			_z_reset_counter = local_position.z_reset_counter;
+			_vxy_reset_counter = local_position.vxy_reset_counter;
+			_vz_reset_counter = local_position.vz_reset_counter;
+			_heading_reset_counter = local_position.heading_reset_counter;
+
+			_delta_xy_reset = Vector2f{local_position.delta_xy};
+			_delta_z_reset = local_position.delta_z;
+			_delta_vxy_reset = Vector2f{local_position.delta_vxy};
+			_delta_vz_reset = local_position.delta_vz;
+			_delta_heading_reset = local_position.delta_heading;
 		}

-		// save last primary estimator_local_position
+		bool publish = true;
+
+		// ensure monotonically increasing timestamp_sample through reset, don't publish
+		//  estimator's local position for system (vehicle_local_position) if it's stale
+		if ((local_position.timestamp_sample <= _local_position_last.timestamp_sample)
+		    || (local_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+
+			publish = false;
+		}
+
+		// save last primary estimator_local_position as published with original resets
 		_local_position_last = local_position;

-		// publish estimator's local position for system (vehicle_local_position) unless it's stale
-		if (local_position.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) {
+		if (publish) {
 			// republish with total reset count and current timestamp
 			local_position.xy_reset_counter = _xy_reset_counter;
 			local_position.z_reset_counter = _z_reset_counter;
@@ -447,47 +493,92 @@ void EKF2Selector::PublishVehicleLocalPosition(bool reset)
 	}
 }

-void EKF2Selector::PublishVehicleGlobalPosition(bool reset)
+void EKF2Selector::PublishVehicleOdometry()
 {
+	// selected estimator_odometry -> vehicle_odometry
+	vehicle_odometry_s odometry;
+
+	if (_instance[_selected_instance].estimator_odometry_sub.update(&odometry)) {
+		bool publish = true;
+
+		// ensure monotonically increasing timestamp_sample through reset, don't publish
+		//  estimator's odometry for system (vehicle_odometry) if it's stale
+		if ((odometry.timestamp_sample <= _odometry_last.timestamp_sample)
+		    || (odometry.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+
+			publish = false;
+		}
+
+		// save last primary estimator_odometry
+		_odometry_last = odometry;
+
+		if (publish) {
+			odometry.timestamp = hrt_absolute_time();
+			_vehicle_odometry_pub.publish(odometry);
+		}
+	}
+}
+
+void EKF2Selector::PublishVehicleGlobalPosition()
+{
+	// selected estimator_global_position -> vehicle_global_position
 	vehicle_global_position_s global_position;

-	if (_instance[_selected_instance].estimator_global_position_sub.copy(&global_position)) {
-		if (reset) {
-			// on reset compute deltas from last published data
-			++_lat_lon_reset_counter;
-			++_alt_reset_counter;
+	if (_instance[_selected_instance].estimator_global_position_sub.update(&global_position)) {
+		bool instance_change = false;

-			_delta_lat_reset = global_position.lat - _global_position_last.lat;
-			_delta_lon_reset = global_position.lon - _global_position_last.lon;
-			_delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt;
-
-			// ensure monotonically increasing timestamp_sample through reset
-			global_position.timestamp_sample = max(global_position.timestamp_sample, _global_position_last.timestamp_sample);
-
-		} else {
-			// otherwise propogate deltas from estimator data while maintaining the overall reset counts
+		if (_instance[_selected_instance].estimator_global_position_sub.get_instance() != _global_position_instance_prev) {
+			_global_position_instance_prev = _instance[_selected_instance].estimator_global_position_sub.get_instance();
+			instance_change = true;
+		}

+		if (_global_position_last.timestamp != 0) {
 			// lat/lon reset
-			if (global_position.lat_lon_reset_counter > _global_position_last.lat_lon_reset_counter) {
+			if (!instance_change && (global_position.lat_lon_reset_counter == _global_position_last.lat_lon_reset_counter + 1)) {
 				++_lat_lon_reset_counter;

 				// TODO: delta latitude/longitude
-				//_delta_lat_reset = global_position.delta_lat;
-				//_delta_lon_reset = global_position.delta_lon;
+				_delta_lat_reset = global_position.lat - _global_position_last.lat;
+				_delta_lon_reset = global_position.lon - _global_position_last.lon;
+
+			} else if (instance_change || (global_position.lat_lon_reset_counter != _global_position_last.lat_lon_reset_counter)) {
+				++_lat_lon_reset_counter;
+
+				_delta_lat_reset = global_position.lat - _global_position_last.lat;
+				_delta_lon_reset = global_position.lon - _global_position_last.lon;
 			}

 			// alt reset
-			if (global_position.alt_reset_counter > _global_position_last.alt_reset_counter) {
+			if (!instance_change && (global_position.alt_reset_counter == _global_position_last.alt_reset_counter + 1)) {
 				++_alt_reset_counter;
 				_delta_alt_reset = global_position.delta_alt;
+
+			} else if (instance_change || (global_position.alt_reset_counter != _global_position_last.alt_reset_counter)) {
+				++_alt_reset_counter;
+				_delta_alt_reset = global_position.delta_alt - _global_position_last.delta_alt;
 			}
+
+		} else {
+			_lat_lon_reset_counter = global_position.lat_lon_reset_counter;
+			_alt_reset_counter = global_position.alt_reset_counter;
+
+			_delta_alt_reset = global_position.delta_alt;
 		}

-		// save last primary estimator_global_position
+		bool publish = true;
+
+		// ensure monotonically increasing timestamp_sample through reset, don't publish
+		//  estimator's global position for system (vehicle_global_position) if it's stale
+		if ((global_position.timestamp_sample <= _global_position_last.timestamp_sample)
+		    || (global_position.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+
+			publish = false;
+		}
+
+		// save last primary estimator_global_position as published with original resets
 		_global_position_last = global_position;

-		// publish estimator's global position for system (vehicle_global_position) unless it's stale
-		if (global_position.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) {
+		if (publish) {
 			// republish with total reset count and current timestamp
 			global_position.lat_lon_reset_counter = _lat_lon_reset_counter;
 			global_position.alt_reset_counter = _alt_reset_counter;
@@ -499,22 +590,31 @@ void EKF2Selector::PublishVehicleGlobalPosition(bool reset)
 	}
 }

-void EKF2Selector::PublishWindEstimate(bool reset)
+void EKF2Selector::PublishWindEstimate()
 {
+	// selected estimator_wind -> wind
 	wind_s wind;

-	if (_instance[_selected_instance].estimator_wind_sub.copy(&wind)) {
-		if (reset) {
-			// ensure monotonically increasing timestamp_sample through reset
-			wind.timestamp_sample = max(wind.timestamp_sample, _wind_last.timestamp_sample);
+	if (_instance[_selected_instance].estimator_wind_sub.update(&wind)) {
+		bool publish = true;
+
+		// ensure monotonically increasing timestamp_sample through reset, don't publish
+		//  estimator's wind for system (wind) if it's stale
+		if ((wind.timestamp_sample <= _wind_last.timestamp_sample)
+		    || (wind.timestamp_sample < _instance[_selected_instance].timestamp_sample_last)) {
+
+			publish = false;
 		}

 		// save last primary wind
 		_wind_last = wind;

-		// republish with current timestamp
-		wind.timestamp = hrt_absolute_time();
-		_wind_pub.publish(wind);
+		// publish estimator's wind for system unless it's stale
+		if (publish) {
+			// republish with current timestamp
+			wind.timestamp = hrt_absolute_time();
+			_wind_pub.publish(wind);
+		}
 	}
 }

@@ -555,7 +655,6 @@ void EKF2Selector::Run()
 	}

 	if (updated) {
-
 		const uint8_t available_instances_prev = _available_instances;
 		const uint8_t selected_instance_prev = _selected_instance;
 		const uint32_t instance_changed_count_prev = _instance_changed_count;
@@ -624,69 +723,47 @@ void EKF2Selector::Run()
 		    || (last_instance_change_prev != _last_instance_change)
 		    || _accel_fault_detected || _gyro_fault_detected) {

-			estimator_selector_status_s selector_status{};
-			selector_status.primary_instance = _selected_instance;
-			selector_status.instances_available = _available_instances;
-			selector_status.instance_changed_count = _instance_changed_count;
-			selector_status.last_instance_change = _last_instance_change;
-			selector_status.accel_device_id = _instance[_selected_instance].accel_device_id;
-			selector_status.baro_device_id = _instance[_selected_instance].baro_device_id;
-			selector_status.gyro_device_id = _instance[_selected_instance].gyro_device_id;
-			selector_status.mag_device_id = _instance[_selected_instance].mag_device_id;
-			selector_status.gyro_fault_detected = _gyro_fault_detected;
-			selector_status.accel_fault_detected = _accel_fault_detected;
-
-			for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
-				selector_status.combined_test_ratio[i] = _instance[i].combined_test_ratio;
-				selector_status.relative_test_ratio[i] = _instance[i].relative_test_ratio;
-				selector_status.healthy[i] = _instance[i].healthy;
-			}
-
-			for (int i = 0; i < IMU_STATUS_SIZE; i++) {
-				selector_status.accumulated_gyro_error[i] = _accumulated_gyro_error[i];
-				selector_status.accumulated_accel_error[i] = _accumulated_accel_error[i];
-			}
-
-			selector_status.timestamp = hrt_absolute_time();
-			_estimator_selector_status_pub.publish(selector_status);
-			_last_status_publish = selector_status.timestamp;
+			PublishEstimatorSelectorStatus();
 			_selector_status_publish = false;
 		}
 	}

 	// republish selected estimator data for system
+	PublishVehicleAttitude();
+	PublishVehicleLocalPosition();
+	PublishVehicleGlobalPosition();
+	PublishVehicleOdometry();
+	PublishWindEstimate();
+}

-	// selected estimator_attitude -> vehicle_attitude
-	if (_instance[_selected_instance].estimator_attitude_sub.updated()) {
-		PublishVehicleAttitude();
+void EKF2Selector::PublishEstimatorSelectorStatus()
+{
+	estimator_selector_status_s selector_status{};
+	selector_status.primary_instance = _selected_instance;
+	selector_status.instances_available = _available_instances;
+	selector_status.instance_changed_count = _instance_changed_count;
+	selector_status.last_instance_change = _last_instance_change;
+	selector_status.accel_device_id = _instance[_selected_instance].accel_device_id;
+	selector_status.baro_device_id = _instance[_selected_instance].baro_device_id;
+	selector_status.gyro_device_id = _instance[_selected_instance].gyro_device_id;
+	selector_status.mag_device_id = _instance[_selected_instance].mag_device_id;
+	selector_status.gyro_fault_detected = _gyro_fault_detected;
+	selector_status.accel_fault_detected = _accel_fault_detected;
+
+	for (int i = 0; i < EKF2_MAX_INSTANCES; i++) {
+		selector_status.combined_test_ratio[i] = _instance[i].combined_test_ratio;
+		selector_status.relative_test_ratio[i] = _instance[i].relative_test_ratio;
+		selector_status.healthy[i] = _instance[i].healthy;
 	}

-	// selected estimator_local_position -> vehicle_local_position
-	if (_instance[_selected_instance].estimator_local_position_sub.updated()) {
-		PublishVehicleLocalPosition();
+	for (int i = 0; i < IMU_STATUS_SIZE; i++) {
+		selector_status.accumulated_gyro_error[i] = _accumulated_gyro_error[i];
+		selector_status.accumulated_accel_error[i] = _accumulated_accel_error[i];
 	}

-	// selected estimator_global_position -> vehicle_global_position
-	if (_instance[_selected_instance].estimator_global_position_sub.updated()) {
-		PublishVehicleGlobalPosition();
-	}
-
-	// selected estimator_wind -> wind
-	if (_instance[_selected_instance].estimator_wind_sub.updated()) {
-		PublishWindEstimate();
-	}
-
-	// selected estimator_odometry -> vehicle_odometry
-	if (_instance[_selected_instance].estimator_odometry_sub.updated()) {
-		vehicle_odometry_s vehicle_odometry;
-
-		if (_instance[_selected_instance].estimator_odometry_sub.update(&vehicle_odometry)) {
-			if (vehicle_odometry.timestamp_sample >= _instance[_selected_instance].timestamp_sample_last) {
-				vehicle_odometry.timestamp = hrt_absolute_time();
-				_vehicle_odometry_pub.publish(vehicle_odometry);
-			}
-		}
-	}
+	selector_status.timestamp = hrt_absolute_time();
+	_estimator_selector_status_pub.publish(selector_status);
+	_last_status_publish = selector_status.timestamp;
 }

 void EKF2Selector::PrintStatus()
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -78,10 +78,12 @@ private:
 	static constexpr uint64_t FILTER_UPDATE_PERIOD{10_ms};

 	void Run() override;
-	void PublishVehicleAttitude(bool reset = false);
-	void PublishVehicleLocalPosition(bool reset = false);
-	void PublishVehicleGlobalPosition(bool reset = false);
-	void PublishWindEstimate(bool reset = false);
+	void PublishEstimatorSelectorStatus();
+	void PublishVehicleAttitude();
+	void PublishVehicleLocalPosition();
+	void PublishVehicleGlobalPosition();
+	void PublishVehicleOdometry();
+	void PublishWindEstimate();
 	bool SelectInstance(uint8_t instance);

 	// Update the error scores for all available instances
@@ -191,6 +193,9 @@ private:
 	uint8_t _vz_reset_counter{0};
 	uint8_t _heading_reset_counter{0};

+	// vehicle_odometry
+	vehicle_odometry_s _odometry_last{};
+
 	// vehicle_global_position: reset counters
 	vehicle_global_position_s _global_position_last{};
 	double _delta_lat_reset{0};
@@ -202,6 +207,10 @@ private:
 	// wind estimate
 	wind_s _wind_last{};

+	uint8_t _attitude_instance_prev{INVALID_INSTANCE};
+	uint8_t _local_position_instance_prev{INVALID_INSTANCE};
+	uint8_t _global_position_instance_prev{INVALID_INSTANCE};
+
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 	uORB::Subscription _sensors_status_imu{ORB_ID(sensors_status_imu)};

@@ -242,21 +242,21 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
 	} else {
 		// VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible
 		// this assumption is good as long as the vehicle is not hovering in a headwind which is much larger
-		// than the minimum airspeed
+		// than the stall airspeed
 		if (_vehicle_status.is_vtol && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 		    && !_vehicle_status.in_transition_mode) {
-			airspeed = _param_fw_airspd_min.get();
+			airspeed = _param_fw_airspd_stall.get();
 		}
 	}

 	/*
-	 * For scaling our actuators using anything less than the min (close to stall)
+	 * For scaling our actuators using anything less than the stall
 	 * speed doesn't make any sense - its the strongest reasonable deflection we
 	 * want to do in flight and its the baseline a human pilot would choose.
 	 *
 	 * Forcing the scaling to this value allows reasonable handheld tests.
 	 */
-	const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_min.get(),
+	const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_stall.get(),
 					   _param_fw_airspd_max.get()), 0.1f, 1000.0f);

 	_airspeed_scaling = (_param_fw_arsp_scale_en.get()) ? (_param_fw_airspd_trim.get() / airspeed_constrained) : 1.0f;
@@ -427,7 +427,7 @@ void FixedwingAttitudeControl::Run()
 			control_input.roll_setpoint = _att_sp.roll_body;
 			control_input.pitch_setpoint = _att_sp.pitch_body;
 			control_input.yaw_setpoint = _att_sp.yaw_body;
-			control_input.airspeed_min = _param_fw_airspd_min.get();
+			control_input.airspeed_min = _param_fw_airspd_stall.get();
 			control_input.airspeed_max = _param_fw_airspd_max.get();
 			control_input.airspeed = airspeed;
 			control_input.scaler = _airspeed_scaling;
@@ -436,11 +436,11 @@ void FixedwingAttitudeControl::Run()
 			if (wheel_control) {
 				_local_pos_sub.update(&_local_pos);

-				/* Use min airspeed to calculate ground speed scaling region.
+				/* Use stall airspeed to calculate ground speed scaling region.
 				* Don't scale below gspd_scaling_trim
 				*/
 				float groundspeed = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
-				float gspd_scaling_trim = (_param_fw_airspd_min.get() * 0.6f);
+				float gspd_scaling_trim = (_param_fw_airspd_stall.get());

 				control_input.groundspeed = groundspeed;

@@ -477,11 +477,11 @@ void FixedwingAttitudeControl::Run()
 			float trim_yaw = _param_trim_yaw.get();

 			if (airspeed < _param_fw_airspd_trim.get()) {
-				trim_roll += gradual(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_r_vmin.get(),
+				trim_roll += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_r_vmin.get(),
 						     0.0f);
-				trim_pitch += gradual(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_p_vmin.get(),
+				trim_pitch += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_p_vmin.get(),
 						      0.0f);
-				trim_yaw += gradual(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_y_vmin.get(),
+				trim_yaw += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_y_vmin.get(),
 						    0.0f);

 			} else {
@@ -147,7 +147,7 @@ private:
 		(ParamFloat<px4::params::FW_ACRO_Z_MAX>) _param_fw_acro_z_max,

 		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
-		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
+		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
 		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
 		(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,

@@ -141,19 +141,31 @@ FixedwingPositionControl::parameters_update()

 	landing_status_publish();

+	int check_ret = PX4_OK;
+
 	// sanity check parameters
-	if ((_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) ||
-	    (_param_fw_airspd_max.get() < 5.0f) ||
-	    (_param_fw_airspd_min.get() > 100.0f) ||
-	    (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get()) ||
-	    (_param_fw_airspd_trim.get() > _param_fw_airspd_max.get())) {
-
-		mavlink_log_critical(&_mavlink_log_pub, "Airspeed parameters invalid");
-
-		return PX4_ERROR;
+	if (_param_fw_airspd_max.get() < _param_fw_airspd_min.get()) {
+		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max smaller than min");
+		check_ret = PX4_ERROR;
 	}

-	return PX4_OK;
+	if (_param_fw_airspd_max.get() < 5.0f || _param_fw_airspd_min.get() > 100.0f) {
+		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed max < 5 m/s or min > 100 m/s");
+		check_ret = PX4_ERROR;
+	}
+
+	if (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get() ||
+	    _param_fw_airspd_trim.get() > _param_fw_airspd_max.get()) {
+		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed cruise out of min or max bounds");
+		check_ret = PX4_ERROR;
+	}
+
+	if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get() * 0.9f) {
+		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Stall airspeed higher than 0.9 of min");
+		check_ret = PX4_ERROR;
+	}
+
+	return check_ret;
 }

 void
@@ -353,6 +353,7 @@ private:
 		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
 		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
 		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
+		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,

 		(ParamFloat<px4::params::FW_CLMBOUT_DIFF>) _param_fw_clmbout_diff,

@@ -433,7 +433,8 @@ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f);
 /**
 * Minimum Airspeed (CAS)
 *
- * If the CAS (calibrated airspeed) falls below this value, the TECS controller will try to
+ * The minimal airspeed (calibrated airspeed) the user is able to command.
+ * Further, if the airspeed falls below this value, the TECS controller will try to
 * increase airspeed more aggressively.
 *
 * @unit m/s
@@ -476,6 +477,22 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
 */
 PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);

+/**
+ * Stall Airspeed (CAS)
+ *
+ * The stall airspeed (calibrated airspeed) of the vehicle.
+ * It is used for airspeed sensor failure detection and for the control
+ * surface scaling airspeed limits.
+ *
+ * @unit m/s
+ * @min 0.5
+ * @max 40
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_STALL, 7.0f);
+
 /**
 * Maximum climb rate
 *
@@ -401,7 +401,7 @@ LogWriterFile::LogFileBuffer::~LogFileBuffer()
 		close(_fd);
 	}

-	delete[] _buffer;
+	free(_buffer);

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

@@ -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;
@@ -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;
@@ -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;
@@ -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;

@@ -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 {
@@ -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;
@@ -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);
@@ -46,7 +46,7 @@
 class Land : public MissionBlock
 {
 public:
-	Land(Navigator *navigator);
+	Land(Navigator *navigator, NavigatorCore &navigator_core);
 	~Land() = default;

 	void on_activation() override;
@@ -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;

@@ -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;
@@ -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) {
@@ -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;
@@ -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;
@@ -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)};
 };
@@ -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) {

@@ -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
@@ -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);
 		}

@@ -767,8 +773,8 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 			/* inform other apps via the mission result */
 			_geofence_result.geofence_violated = true;

-			/* Issue a warning about the geofence violation once */
-			if (!_geofence_violation_warning_sent) {
+			/* Issue a warning about the geofence violation once and only if we are armed */
+			if (!_geofence_violation_warning_sent && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
 				mavlink_log_critical(&_mavlink_log_pub, "Approaching on Geofence");

 				// we have predicted a geofence violation and if the action is to loiter then
@@ -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[])
 {
@@ -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;
@@ -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;
@@ -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

@@ -72,7 +72,7 @@ public:
 		RTL_DESTINATION_SAFE_POINT,
 	};

-	RTL(Navigator *navigator);
+	RTL(Navigator *navigator, NavigatorCore &navigator_core);

 	~RTL() = default;

@@ -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");
 	}

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

 	void on_activation() override;
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
RomanBapst	18a5bb32bc	navigator: wrote unit tests for Takeoff navigation mode Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-05-12 18:39:34 +03:00
Daniel Agar	ad0482155e	ROMFS: reduce LOGGER_BUF default to 8 kB on older boards	2021-05-12 17:06:33 +02:00
Beat Küng	d300a879f1	cmake: remove romfs content before tar extraction This avoids incremental build errors when switching between branches with a different set of airframes. E.g: Aborting due to missing @type tag in file: 'Firmware/build/px4_fmu-v5_default/etc/init.d/airframes/13030_generic_vtol_quad_tiltrotor'	2021-05-11 13:14:42 -04:00
Beat Küng	e77b4418a5	fix logger: use free() instead of 'delete[]' for _buffer The allocation got changed to px4_cache_aligned_alloc	2021-05-11 18:12:56 +02:00
Igor Mišić	05a2d4d5a9	gps: updated submodule to fix for heading	2021-05-11 08:34:23 +02:00
Igor Mišić	d9e31d67aa	gps: Updated timeout time for the rover with moving base The MB rover will wait as long as possible to compute a navigation solution, possibly lowering the navigation rate all the way to 1 Hz while doing so.	2021-05-11 08:34:23 +02:00
Silvan Fuhrer	b7e563bdbe	Airspeed selector: fix in_air_fixed_wing condition Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-05-10 13:43:05 +02:00
Silvan Fuhrer	596da5b7d3	Airspeed selector: use module params for FW_AIRSPD_STALL Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-05-10 13:43:05 +02:00
Silvan Fuhrer	2f73115b54	translate ASPD_STALL to FW_AIRSPD_STALL Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-05-10 13:43:05 +02:00
Silvan Fuhrer	bf311ed77d	addressed review comments (fixes in error message and comments) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-05-10 13:43:05 +02:00
Silvan Fuhrer	63a53d48e7	FW Position controller: improve parameter sanity checks (provide more feedback) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-05-10 13:43:05 +02:00
Silvan Fuhrer	c8ec6b3d08	Airspeed selector: remove ASPD_STALL and replace by FW_AIRSPD_STALL Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-05-10 13:43:05 +02:00
RomanBapst	3b27864e53	vehicle_status: added field for geofence violation Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-05-10 10:46:49 +03:00
RomanBapst	3ac8c23dd0	commander: added prearm check for geofence violation - if geofence action is not none, then don't allow arming outside of geofence Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-05-10 10:46:49 +03:00
RomanBapst	6215e6c7ec	navigator: do not emit geofence warnings if system is not armed Signed-off-by: RomanBapst <bapstroman@gmail.com>	2021-05-10 10:46:49 +03:00
David Sidrane	e87a6c755d	px4_fmu-v5x:Add support for Revision 2 FMUM HW using ICM20649 not BMI088	2021-05-10 09:11:50 +02:00
David Sidrane	69e0c2fc10	px4:platform support SPI configuration selection on HW REV	2021-05-10 09:11:50 +02:00
Julian Oes	e4ee7c7d98	sitl_gazebo: update submodule This fixes the simulation stalling after 30mins due to an int overflow.	2021-05-09 15:17:25 +02:00
David Sidrane	541697d193	NuttX Backports Fixing SDIO/SDMMC Data Timeouts stm32, F7 and H7	2021-05-08 04:40:54 -07:00
Daniel Agar	c49c8932de	commander: mag_calibration fail immediately if no mags available	2021-05-08 13:03:42 +02:00
Peter van der Perk	0c926250a2	UAVCANv1 cleanup and uORB over UAVCANV1 move to own subclass	2021-05-08 13:03:02 +02:00
Daniel Agar	dfb4ec56b1	Makefile: clean and distclean updates - update clean to recurse all build directories and use build system clean - git clean is used to remove submodule generated build artifacts that are left in the source tree - distclean now discards all build directories and any gitignored filess that were generated in source directories (but not top level)	2021-05-08 12:29:16 +02:00
Daniel Agar	f15eefcc95	ekf2: selector increase status rate before potential instance change	2021-05-07 22:38:47 -04:00
Julian Oes	29730e30fa	ekf2: don't timeout in HITL mode Otherwise ekf2 might not start if HITL isn't started within 30 seconds.	2021-05-07 22:38:03 -04:00
Julian Oes	ac97b5520c	commander: assume power is fine for HITL This means that the preflight check indicator in QGC is green for HITL.	2021-05-07 22:38:03 -04:00
Julian Oes	648a21f11d	commander: ignore calibration in HITL The calibration is not found in HITL mode. Therefore, I suggest to ignore this step and assume the calibration is fine. This mostly fixes the preflight check indicator in QGC, arming was (for some reason?) already possible.	2021-05-07 22:38:03 -04:00
Hamish Willee	d3fd03a014	airspeed calibration: instruct to blow into front of pitot ... rather than across it	2021-05-07 21:34:53 -04:00
David Sidrane	b1e0702657	px4_fmuv2:Save Flash CONFIG_LIBC_STRERROR=n	2021-05-07 11:50:38 -07:00
David Sidrane	8d82560308	NuttX Backports [BACKPORT] binnfmt:Fix return before close ELF fd stm32h7: serial: use dma tx semaphore as resource holder [BACKPORT] stm32h7:Serial Add RX and TX DMA [BACKPORT] drivers/serial: fix Rx interrupt enable for cdcacm [BACKPORT] stm32h7:Allow for reuse of the OTG_ID GPIO [BACKPORT] stm32f7:Allow for reuse of the OTG_ID GPIO	2021-05-07 11:50:38 -07:00
PX4 BuildBot	e265ebabc5	Update submodule ecl to latest Thu May 6 12:39:12 UTC 2021 - ecl in PX4/Firmware (a300d32523e24df3f366a0d564b764261e1c1909): https://github.com/PX4/PX4-ECL/commit/a7b8afe420f438554ad90bcba0f1f4872325e75b - ecl current upstream: https://github.com/PX4/PX4-ECL/commit/29243ac5cbb5d27ac71744e88afcd786df6f748d - Changes: https://github.com/PX4/PX4-ECL/compare/a7b8afe420f438554ad90bcba0f1f4872325e75b...29243ac5cbb5d27ac71744e88afcd786df6f748d `29243ac` 2021-05-05 bresch - yaw_reset: reduce minimum vector length to compute yaw error `aad4840` 2021-05-02 Kabir Mohammed - EKF: increase allowed difference between flow and gyro ODRs	2021-05-06 13:53:40 -04:00
Daniel Agar	9419f9c5e8	Update submodule mavlink v2.0 to latest Thu May 6 12:39:02 UTC 2021	2021-05-06 13:52:43 -04:00
Silvan Fuhrer	08dab18a8b	vtol_type: in FW, set min PWM to PWM_DEFAULT_MIN instead of PWM_MOTOR_OFF Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2021-05-06 10:20:24 +02:00
benjinne	099c2d13f6	dshot: add 3D capability	2021-05-06 08:12:07 +02:00
Daniel Agar	177ee4cbca	drivers/optical_flow/paw3902: properly discard samples after mode change - respect mode 2 shutter requirements from datasheet (should not operate with Shutter < 0x01F4 in Mode 2) - sensor reset is handled by mode change	2021-05-05 21:48:01 -04:00
Daniel Agar	b1ebd16c61	ekf2: improve selector reset handling - handle reset count rollover (uint8_t) - compute full reset delta if primary estimator instance has changed or if we missed a reset	2021-05-05 21:45:13 -04:00