gps: ublox sensor_gps_heading message

.github/workflows/deploy_all.yml

@@ -10,7 +10,6 @@ on:
 jobs:
   enumerate_targets:
     runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
     outputs:
       matrix: ${{ steps.set-matrix.outputs.matrix }}
     steps:

Kconfig

@@ -128,9 +128,6 @@ config BOARD_KEYSTORE
 
 menu "Serial ports"
 
-    config BOARD_SERIAL_URT6
-        string "URT6 tty port"
-
     config BOARD_SERIAL_GPS1
         string "GPS1 tty port"
 
@@ -161,14 +158,8 @@ menu "Serial ports"
     config BOARD_SERIAL_TEL5
         string "TEL5 tty port"
 
-    config BOARD_SERIAL_RC
-        string "RC tty port"
-
     config BOARD_SERIAL_WIFI
         string "WIFI tty port"
-
-    config BOARD_SERIAL_PPB
-        string "PPB (Pixhawk Payload Bus) tty port"
 endmenu
 
 menu "drivers"

ROMFS/px4fmu_common/init.d-posix/airframes/1050_standard_vtol_ctrlalloc

@@ -1,110 +0,0 @@
-#!/bin/sh
-#
-# @name Standard VTOL
-#
-# @type Standard VTOL
-#
-
-. ${R}etc/init.d/rc.vtol_defaults
-
-param set-default SYS_CTRL_ALLOC 1
-
-param set-default VM_MASS 1.5
-param set-default VM_INERTIA_XX 0.03
-param set-default VM_INERTIA_YY 0.03
-param set-default VM_INERTIA_ZZ 0.05
-
-param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
-param set-default FW_PR_FF 0.2
-param set-default FW_PR_P 0.9
-param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
-param set-default FW_P_LIM_MIN -15
-param set-default FW_RR_FF 0.1
-param set-default FW_RR_P 0.3
-param set-default FW_THR_CRUISE 0.25
-param set-default FW_THR_MAX 0.6
-param set-default FW_THR_MIN 0.05
-param set-default FW_T_ALT_TC 2
-param set-default FW_T_CLMB_MAX 8
-param set-default FW_T_HRATE_FF 0.5
-param set-default FW_T_SINK_MAX 2.7
-param set-default FW_T_SINK_MIN 2.2
-param set-default FW_T_TAS_TC 2
-
-param set-default MC_ROLLRATE_P 0.3
-param set-default MC_YAW_P 1.6
-
-param set-default MIS_TAKEOFF_ALT 10
-
-param set-default MPC_ACC_HOR_MAX 2
-param set-default MPC_XY_P 0.8
-param set-default MPC_XY_VEL_P_ACC 3
-param set-default MPC_XY_VEL_I_ACC 4
-param set-default MPC_XY_VEL_D_ACC 0.1
-
-param set-default NAV_ACC_RAD 5
-param set-default NAV_LOITER_RAD 80
-
-param set-default VT_FWD_THRUST_EN 4
-param set-default VT_F_TRANS_THR 0.75
-param set-default VT_MOT_ID 1234
-param set-default VT_FW_MOT_OFFID 1234
-param set-default VT_B_TRANS_DUR 8
-param set-default VT_TYPE 2
-
-param set-default CA_AIRFRAME 1
-param set-default CA_METHOD 1
-
-# VTOL Motors
-param set-default CA_ACT0_MIN 0.0
-param set-default CA_ACT1_MIN 0.0
-param set-default CA_ACT2_MIN 0.0
-param set-default CA_ACT3_MIN 0.0
-param set-default CA_ACT0_MAX 1.0
-param set-default CA_ACT1_MAX 1.0
-param set-default CA_ACT2_MAX 1.0
-param set-default CA_ACT3_MAX 1.0
-
-# Throttle
-param set-default CA_ACT4_MIN 0.0
-param set-default CA_ACT4_MAX 1.0
-
-# Ailerons/Ruddervator
-param set-default CA_ACT5_MIN -1.0
-param set-default CA_ACT5_MAX 1.0
-param set-default CA_ACT6_MIN -1.0
-param set-default CA_ACT6_MAX 1.0
-param set-default CA_ACT7_MIN -1.0
-param set-default CA_ACT7_MAX 1.0
-
-param set-default CA_MC_R0_PX 0.177
-param set-default CA_MC_R0_PY 0.177
-param set-default CA_MC_R0_CT 6.5
-param set-default CA_MC_R0_KM 0.05
-param set-default CA_MC_R1_PX -0.177
-param set-default CA_MC_R1_PY -0.177
-param set-default CA_MC_R1_CT 6.5
-param set-default CA_MC_R1_KM 0.05
-param set-default CA_MC_R2_PX 0.177
-param set-default CA_MC_R2_PY -0.177
-param set-default CA_MC_R2_CT 6.5
-param set-default CA_MC_R2_KM -0.05
-param set-default CA_MC_R3_PX -0.177
-param set-default CA_MC_R3_PY 0.177
-param set-default CA_MC_R3_CT 6.5
-param set-default CA_MC_R3_KM -0.05
-
-param set-default PWM_MAIN_FUNC1 101
-param set-default PWM_MAIN_FUNC2 102
-param set-default PWM_MAIN_FUNC3 103
-param set-default PWM_MAIN_FUNC4 104
-param set-default PWM_MAIN_FUNC5 105 # Setting pusher to Motor5 for now
-param set-default PWM_MAIN_FUNC6 201
-param set-default PWM_MAIN_FUNC7 202
-param set-default PWM_MAIN_FUNC8 203
-
-set MAV_TYPE 22
-
-set MIXER skip

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -67,7 +67,6 @@ px4_add_romfs_files(
 	1042_tiltrotor
 	1043_standard_vtol_drop
 	1043_standard_vtol_drop.post
-	1050_standard_vtol_ctrlalloc
 	1060_rover
 	1061_r1_rover
 	1062_tf-r1

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -15,22 +15,6 @@ ekf2 start &
 #                        End Estimator group selection                        #
 ###############################################################################
 
-if param compare SYS_CTRL_ALLOC 1
-then
-	#
-	# Start Control Allocator
-	#
-	control_allocator start
-
-	#
-	# Disable hover thrust estimator and prearming
-	# These features are currently incompatible with control allocation
-	#
-	# TODO: fix
-	#
-	param set MPC_USE_HTE 0
-fi
-
 airspeed_selector start
 
 vtol_att_control start

Tools/kconfig/merge_config.py

@@ -110,16 +110,10 @@ def main(kconfig_file, config1, config2):
 
     for line in f:
         match = unset_match(line)
-        #pprint.pprint(line)
+        #pprint.pprint(match)
         if match is not None:
             sym_name = match.group(1)
             kconf.syms[sym_name].unset_value()
-
-            for default, cond in kconf.syms[sym_name].orig_defaults:
-                if(cond.str_value == 'y'):
-                    # Default is y, our diff is unset thus we've set it to no
-                    kconf.syms[sym_name].set_value(0)
-
     f.close()
 
     # Print warnings for symbols whose actual value doesn't match the assigned

boards/holybro/kakutef7/default.px4board

@@ -5,7 +5,6 @@ CONFIG_BOARD_EXTERNAL_METADATA=y
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS3"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS0"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
-CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP280=y
 CONFIG_DRIVERS_DSHOT=y

boards/omnibus/f4sd/default.px4board

@@ -3,7 +3,6 @@ CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_CONSTRAINED_FLASH=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS1"
-CONFIG_BOARD_SERIAL_URT6="/dev/ttyS2"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP280=y
 CONFIG_DRIVERS_DSHOT=y
@@ -53,7 +52,6 @@ CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
 CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_TOP=y
-CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y

boards/omnibus/f4sd/src/timer_config.cpp

@@ -34,8 +34,8 @@
 #include <px4_arch/io_timer_hw_description.h>
 
 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
-	initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}),
 	initIOTimer(Timer::Timer2, DMA{DMA::Index1, DMA::Stream1, DMA::Channel3}),
+	initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}),
 };
 
 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {

boards/px4/fmu-v5x/default.px4board

@@ -7,7 +7,6 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
-CONFIG_BOARD_SERIAL_PPB="/dev/ttyS3"
 CONFIG_DRIVERS_ADC_ADS1115=y
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_COMMON_BAROMETERS=y

cmake/kconfig.cmake

@@ -210,9 +210,6 @@ if(EXISTS ${BOARD_DEFCONFIG})
 
 	# Serial ports
 	set(board_serial_ports)
-	if(SERIAL_URT6)
-		list(APPEND board_serial_ports URT6:${SERIAL_URT6})
-	endif()
 	if(SERIAL_GPS1)
         list(APPEND board_serial_ports GPS1:${SERIAL_GPS1})
 	endif()
@@ -243,15 +240,9 @@ if(EXISTS ${BOARD_DEFCONFIG})
 	if(SERIAL_TEL5)
         list(APPEND board_serial_ports TEL5:${SERIAL_TEL5})
 	endif()
-	if(SERIAL_RC)
-		list(APPEND board_serial_ports RC:${SERIAL_RC})
-	endif()
 	if(SERIAL_WIFI)
         list(APPEND board_serial_ports WIFI:${SERIAL_WIFI})
 	endif()
-	if(SERIAL_PPB)
-        list(APPEND board_serial_ports PPB:${SERIAL_PPB})
-	endif()
 
 
 	# ROMFS

msg/CMakeLists.txt

@@ -138,6 +138,7 @@ set(msg_files
 	sensor_combined.msg
 	sensor_correction.msg
 	sensor_gps.msg
+	sensor_gps_heading.msg
 	sensor_gyro.msg
 	sensor_gyro_fft.msg
 	sensor_gyro_fifo.msg

msg/sensor_gps_heading.msg

@@ -0,0 +1,10 @@
+uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp_sample
+
+uint32 device_id                # unique device ID for the sensor that does not change between power cycles
+
+float32 heading
+float32 heading_accuracy                 # [1e-5 deg] Accuracy of the heading of the relative position vector
+
+float32[3] relative_position    # relative position vector
+float32[3] relative_position_accuracy

platforms/posix/cmake/sitl_target.cmake

@@ -144,7 +144,6 @@ set(models
 	solo
 	standard_vtol
 	standard_vtol_drop
-	standard_vtol_ctrlalloc
 	tailsitter
 	techpod
 	tiltrotor

src/drivers/gps/definitions.h

@@ -43,6 +43,7 @@
 #include <px4_platform_common/defines.h>
 #include <uORB/topics/satellite_info.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_gps_heading.h>
 
 #define GPS_INFO(...) PX4_INFO(__VA_ARGS__)
 #define GPS_WARN(...) PX4_WARN(__VA_ARGS__)

src/drivers/gps/gps.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -63,6 +63,7 @@
 #include <uORB/topics/gps_dump.h>
 #include <uORB/topics/gps_inject_data.h>
 #include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_gps_heading.h>
 
 #ifndef CONSTRAINED_FLASH
 # include "devices/src/ashtech.h"
@@ -180,7 +181,11 @@ private:
 	sensor_gps_s			_report_gps_pos{};				///< uORB topic for gps position
 	satellite_info_s		*_p_report_sat_info{nullptr};			///< pointer to uORB topic for satellite info
 
+	sensor_gps_heading_s            _sensor_gps_heading{};
+	hrt_abstime                     _sensor_gps_heading_timestamp_last{0};
+
 	uORB::PublicationMulti<sensor_gps_s>	_report_gps_pos_pub{ORB_ID(sensor_gps)};	///< uORB pub for gps position
+	uORB::PublicationMulti<sensor_gps_heading_s> _sensor_gps_heading_pub{ORB_ID(sensor_gps_heading)};
 	uORB::PublicationMulti<satellite_info_s>	_report_sat_info_pub{ORB_ID(satellite_info)};		///< uORB pub for satellite info
 
 	float				_rate{0.0f};					///< position update rate
@@ -775,7 +780,7 @@ GPS::run()
 
 		/* FALLTHROUGH */
 		case gps_driver_mode_t::UBX:
-			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
+			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, &_sensor_gps_heading, _p_report_sat_info,
 						   gps_ubx_dynmodel, heading_offset, ubx_mode);
 			set_device_type(DRV_GPS_DEVTYPE_UBX);
 			break;
@@ -1099,6 +1104,15 @@ GPS::publish()
 		// The uORB message definition requires this data to be set to a NAN if no new valid data is available.
 		_report_gps_pos.heading = NAN;
 		_is_gps_main_advertised.store(true);
+
+		if (_sensor_gps_heading.timestamp_sample > _sensor_gps_heading_timestamp_last) {
+
+			_sensor_gps_heading.device_id = get_device_id();
+			_sensor_gps_heading.timestamp = hrt_absolute_time();
+			_sensor_gps_heading_pub.publish(_sensor_gps_heading);
+
+			_sensor_gps_heading_timestamp_last = _sensor_gps_heading.timestamp_sample;
+		}
 	}
 }
 

src/drivers/uavcan/uavcan_main.cpp

@@ -1205,7 +1205,7 @@ int uavcan_main(int argc, char *argv[])
 	}
 
 	if (!std::strcmp(argv[1], "hardpoint")) {
-		if (argc > 4 && !std::strcmp(argv[2], "set")) {
+		if (!std::strcmp(argv[2], "set") && argc > 4) {
 			const int hardpoint_id = atoi(argv[3]);
 			const int command = atoi(argv[4]);
 

src/drivers/uavcan_v1/NodeClient.hpp

@@ -1,92 +0,0 @@
-/****************************************************************************
- *
- *   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 NodeClient.hpp
- *
- * Defines basic implementation of UAVCAN PNP for dynamic Node ID
- *
- * @author Peter van der Perk <peter.vanderperk@nxp.com>
- */
-
-#pragma once
-
-
-#include <px4_platform_common/defines.h>
-#include <drivers/drv_hrt.h>
-
-#include "CanardInterface.hpp"
-
-#include <uavcan/node/ID_1_0.h>
-#include <uavcan/pnp/NodeIDAllocationData_1_0.h>
-#include <uavcan/pnp/NodeIDAllocationData_2_0.h>
-
-#include "Services/AccessRequest.hpp"
-#include "Services/ListRequest.hpp"
-
-#define PNP1_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_1_0_FIXED_PORT_ID_
-#define PNP1_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_1_0_SERIALIZATION_BUFFER_SIZE_BYTES_
-#define PNP2_PORT_ID                                 uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_
-#define PNP2_PAYLOAD_SIZE                            uavcan_pnp_NodeIDAllocationData_2_0_SERIALIZATION_BUFFER_SIZE_BYTES_
-
-class NodeClient : public UavcanBaseSubscriber
-{
-public:
-	NodeClient(CanardInstance &ins, UavcanParamManager &pmgr) : UavcanBaseSubscriber(ins, "NodeIDAllocationData", 0),
-		_canard_instance(ins) { };
-
-	void subscribe() override
-	{
-
-		canardRxSubscribe(&_canard_instance,
-				  CanardTransferKindMessage,
-				  (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PORT_ID : PNP1_PORT_ID),  // The fixed Subject-ID
-				  (_canard_instance.mtu_bytes == CANARD_MTU_CAN_FD ? PNP2_PAYLOAD_SIZE : PNP1_PAYLOAD_SIZE),
-				  CANARD_DEFAULT_TRANSFER_ID_TIMEOUT_USEC,
-				  &_subj_sub._canard_sub);
-	}
-
-	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg);
-	bool HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_2_0 &msg);
-
-	void callback(const CanardTransfer &receive); // NodeIDAllocation callback
-
-	void update();
-
-private:
-
-	CanardInstance &_canard_instance;
-	CanardTransferID _node_id_alloc_transfer_id{0};
-
-	hrt_abstime _nodealloc_request_last{0};
-};

src/lib/l1/ECL_L1_Pos_Controller.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2020 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
@@ -276,9 +276,7 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2d &vector_A, const Vector2d
 	float tangent_vel = xtrack_vel_center * loiter_direction;
 
 	/* prevent PD output from turning the wrong way when in circle mode */
-	const float l1_op_tan_vel = 2.f; // hard coded max tangential velocity in the opposite direction
-
-	if (tangent_vel < -l1_op_tan_vel && _circle_mode) {
+	if (tangent_vel < 0.0f && _circle_mode) {
 		lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd, 0.0f);
 	}
 

src/lib/world_magnetic_model/CMakeLists.txt

@@ -34,6 +34,7 @@
 add_custom_target(world_magnetic_model_update
 	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/fetch_noaa_table.py > ${CMAKE_CURRENT_SOURCE_DIR}/geo_magnetic_tables.hpp
 	DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/fetch_noaa_table.py
+	BYPRODUCTS ${CMAKE_CURRENT_SOURCE_DIR}/geo_magnetic_tables.hpp
 	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
 	USES_TERMINAL
 	COMMENT "Updating world magnetic model from NOAA (${CMAKE_CURRENT_SOURCE_DIR}/geo_magnetic_tables.hpp)"
@@ -42,6 +43,7 @@ add_custom_target(world_magnetic_model_update
 add_custom_target(world_magnetic_model_tests_update
 	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py > ${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp
 	DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/generate_gtest.py
+	BYPRODUCTS ${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp
 	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
 	USES_TERMINAL
 	COMMENT "Updating world magnetic model unit tests from NOAA (${CMAKE_CURRENT_SOURCE_DIR}/test_geo_lookup.cpp)"

src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp

@@ -49,8 +49,8 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 {
 	bool success = true; // start with a pass and change to a fail if any test fails
 
-	int32_t mag_strength_check = 1;
-	param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check);
+	int32_t mag_strength_check_enabled = 1;
+	param_get(param_find("COM_ARM_MAG_STR"), &mag_strength_check_enabled);
 
 	float hgt_test_ratio_limit = 1.f;
 	param_get(param_find("COM_ARM_EKF_HGT"), &hgt_test_ratio_limit);
@@ -107,20 +107,13 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 		goto out;
 	}
 
-	if ((mag_strength_check >= 1) && status.pre_flt_fail_mag_field_disturbed) {
-		const char *message = "Preflight%s: Strong magnetic interference detected";
-
-		if (mag_strength_check == 1) {
-			if (report_fail) {
-				mavlink_log_critical(mavlink_log_pub, message, " Fail");
-			}
-
-			success = false;
-			goto out;
-
-		} else if (report_fail) {
-			mavlink_log_warning(mavlink_log_pub, message, "");
+	if ((mag_strength_check_enabled == 1) && status.pre_flt_fail_mag_field_disturbed) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: strong magnetic interference detected");
 		}
+
+		success = false;
+		goto out;
 	}
 
 	// check vertical position innovation test ratio

src/modules/commander/commander_params.c

@@ -620,13 +620,10 @@ PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 45);
 /**
  * Enable mag strength preflight check
  *
- * Check if the estimator detects a strong magnetic
+ * Deny arming if the estimator detects a strong magnetic
  * disturbance (check enabled by EKF2_MAG_CHECK)
  *
- * @value 0 Disabled
- * @value 1 Deny arming
- * @value 2 Warning only
- *
+ * @boolean
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_ARM_MAG_STR, 1);

src/modules/commander/state_machine_helper.cpp

@@ -827,8 +827,6 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 			status.nav_state = vehicle_status_s::NAVIGATION_STATE_OFFBOARD;
 		}
 
-		break;
-
 	default:
 		break;
 	}

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp

@@ -44,7 +44,6 @@
 #include <ControlAllocation/ControlAllocation.hpp>
 
 #include <matrix/matrix/math.hpp>
-#include <mixer_module/output_functions.hpp>
 #include <uORB/topics/vehicle_actuator_setpoint.h>
 
 class ActuatorEffectiveness
@@ -105,11 +104,6 @@ public:
 	 */
 	virtual int numActuators() const = 0;
 
-	/**
-	 * Get the function of an actuator
-	 */
-	virtual int actuatorFunction(uint8_t idx) const = 0;
-
 protected:
 	matrix::Vector<float, NUM_ACTUATORS> _trim;			///< Actuator trim
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMultirotor.hpp

@@ -78,8 +78,6 @@ public:
 	bool getEffectivenessMatrix(matrix::Matrix<float, NUM_AXES, NUM_ACTUATORS> &matrix, bool force) override;
 
 	int numActuators() const override { return _num_actuators; }
-
-	int actuatorFunction(uint8_t idx) const override { return (int)OutputFunction::Motor1 + idx; }
 private:
 	bool _updated{true};
 	int _num_actuators{0};

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp

@@ -58,15 +58,7 @@ public:
 	 */
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
-	int numActuators() const override { return 8; }
-
-	int actuatorFunction(uint8_t idx) const override { return (int)_actuator_functions[idx]; }
-
+	int numActuators() const override { return 7; }
 protected:
-	OutputFunction _actuator_functions[NUM_ACTUATORS] = {
-		OutputFunction::Motor1, OutputFunction::Motor2, OutputFunction::Motor3, OutputFunction::Motor4,
-		OutputFunction::Motor5, OutputFunction::Servo1, OutputFunction::Servo2, OutputFunction::Servo3
-	};
-
 	bool _updated{true};
 };

src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp

@@ -59,8 +59,6 @@ public:
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 	int numActuators() const override { return 10; }
-
-	int actuatorFunction(uint8_t idx) const { return (int)OutputFunction::Motor1 + idx; }; /// TODO: This is wrong.  Someone familiar with the tiltrotor should fix this.
 protected:
 	bool _updated{true};
 };

src/modules/control_allocator/ControlAllocator.cpp

@@ -436,35 +436,21 @@ ControlAllocator::publish_control_allocator_status()
 void
 ControlAllocator::publish_legacy_actuator_controls()
 {
-	const hrt_abstime now = hrt_absolute_time();
-
-	actuator_motors_s actuator_motors {};
-	actuator_motors.timestamp = now;
+	actuator_motors_s actuator_motors;
+	actuator_motors.timestamp = hrt_absolute_time();
 	actuator_motors.timestamp_sample = _timestamp_sample;
 
-	actuator_servos_s actuator_servos {};
-	actuator_servos.timestamp = now;
-	actuator_servos.timestamp_sample = _timestamp_sample;
-
 	matrix::Vector<float, NUM_ACTUATORS> actuator_sp = _control_allocation->getActuatorSetpoint();
 	matrix::Vector<float, NUM_ACTUATORS> actuator_sp_normalized = _control_allocation->normalizeActuatorSetpoint(
 				actuator_sp);
 
-	for (size_t i = 0; i < NUM_ACTUATORS; i++) {
-		const int func = _actuator_effectiveness->actuatorFunction(i);
-		const float control = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN;
-
-		if (func >= (int)OutputFunction::Motor1 && func <= (int)OutputFunction::MotorMax) {
-			actuator_motors.control[func - (int)OutputFunction::Motor1] = control;
-		}
-
-		if (func >= (int)OutputFunction::Servo1 && func <= (int)OutputFunction::ServoMax) {
-			actuator_servos.control[func - (int)OutputFunction::Servo1] = control;
-		}
+	for (size_t i = 0; i < actuator_motors_s::NUM_CONTROLS; i++) {
+		actuator_motors.control[i] = PX4_ISFINITE(actuator_sp_normalized(i)) ? actuator_sp_normalized(i) : NAN;
 	}
 
 	_actuator_motors_pub.publish(actuator_motors);
-	_actuator_servos_pub.publish(actuator_servos);
+
+	// TODO: servos
 }
 
 int ControlAllocator::task_spawn(int argc, char *argv[])

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -134,7 +134,7 @@ FixedwingAttitudeControl::vehicle_control_mode_poll()
 }
 
 void
-FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
+FixedwingAttitudeControl::vehicle_manual_poll()
 {
 	const bool is_tailsitter_transition = _is_tailsitter && _vehicle_status.in_transition_mode;
 	const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
@@ -156,7 +156,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 					_att_sp.pitch_body = constrain(_att_sp.pitch_body,
 								       -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
 
-					_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
+					_att_sp.yaw_body = 0.0f;
 					_att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
 
 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
@@ -357,7 +357,7 @@ void FixedwingAttitudeControl::Run()
 		_vehicle_status_sub.update(&_vehicle_status);
 
 		vehicle_control_mode_poll();
-		vehicle_manual_poll(euler_angles.psi());
+		vehicle_manual_poll();
 		vehicle_land_detected_poll();
 
 		// the position controller will not emit attitude setpoints in some modes

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -233,7 +233,7 @@ private:
 	int		parameters_update();
 
 	void		vehicle_control_mode_poll();
-	void		vehicle_manual_poll(const float yaw_body);
+	void		vehicle_manual_poll();
 	void		vehicle_attitude_setpoint_poll();
 	void		vehicle_rates_setpoint_poll();
 	void		vehicle_land_detected_poll();

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -686,14 +686,14 @@ FixedwingPositionControl::set_control_mode_current(bool pos_sp_curr_valid)
 		if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
 			/* Need to init because last loop iteration was in a different mode */
 			_hold_alt = _current_altitude;
-			_hdg_hold_yaw = _yaw; // yaw is not controlled, so set setpoint to current yaw
+			_hdg_hold_yaw = _yaw;
 			_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
 			_yaw_lock_engaged = false;
 
 			/* reset setpoints from other modes (auto) otherwise we won't
 			 * level out without new manual input */
 			_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
-			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+			_att_sp.yaw_body = 0;
 		}
 
 		_control_mode_current = FW_POSCTRL_MODE_POSITION;
@@ -920,6 +920,7 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
 		} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
 			// LOITER: use SETPOINT_TYPE_POSITION to get to SETPOINT_TYPE_LOITER
 			if ((dist >= 0.f)
+			    && (dist_z > 2.f * _param_fw_clmbout_diff.get())
 			    && (dist_xy > 2.f * math::max(acc_rad, fabsf(pos_sp_curr.loiter_radius)))) {
 				// SETPOINT_TYPE_LOITER -> SETPOINT_TYPE_POSITION
 				position_sp_type = position_setpoint_s::SETPOINT_TYPE_POSITION;
@@ -1047,7 +1048,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const Ve
 
 	_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, get_nav_speed_2d(ground_speed));
 	_att_sp.roll_body = _l1_control.get_roll_setpoint();
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	_att_sp.yaw_body = _l1_control.nav_bearing();
 
 	tecs_update_pitch_throttle(now, position_sp_alt,
 				   calculate_target_airspeed(mission_airspeed, ground_speed),
@@ -1151,7 +1152,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const Vect
 	_l1_control.navigate_loiter(curr_wp, curr_pos, loiter_radius, loiter_direction, get_nav_speed_2d(ground_speed));
 
 	_att_sp.roll_body = _l1_control.get_roll_setpoint();
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	_att_sp.yaw_body = _l1_control.nav_bearing();
 
 	float alt_sp = pos_sp_curr.alt;
 
@@ -1266,7 +1267,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 		// assign values
 		_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
-		_att_sp.yaw_body = _runway_takeoff.getYaw(_yaw);
+		_att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing());
 		_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
 		_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
 
@@ -1307,7 +1308,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 			_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
 			_att_sp.roll_body = _l1_control.get_roll_setpoint();
-			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
 			/* Select throttle: only in LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS we want to use
 			 * full throttle, otherwise we use idle throttle */
@@ -1464,7 +1465,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
 	}
 
 	_att_sp.roll_body = _l1_control.get_roll_setpoint();
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	_att_sp.yaw_body = _l1_control.nav_bearing();
 
 	if (_land_noreturn_horizontal) {
 		/* limit roll motion to prevent wings from touching the ground first */
@@ -1681,7 +1682,7 @@ FixedwingPositionControl::control_altitude(const hrt_abstime &now, const Vector2
 				   _manual_height_rate_setpoint_m_s);
 
 	_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
-	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+	_att_sp.yaw_body = 0;
 
 	/* Copy thrust and pitch values from tecs */
 	if (_landed) {
@@ -1775,7 +1776,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 			_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
 
 			_att_sp.roll_body = _l1_control.get_roll_setpoint();
-			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 
 			if (in_takeoff_situation()) {
 				/* limit roll motion to ensure enough lift */
@@ -1800,7 +1801,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2
 		}
 
 		_att_sp.roll_body = roll_sp_new;
-		_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+		_att_sp.yaw_body = 0;
 	}
 
 	/* Copy thrust and pitch values from tecs */

src/modules/logger/logged_topics.cpp

@@ -154,6 +154,7 @@ void LoggedTopics::add_default_topics()
 	add_topic_multi("sensor_accel", 1000, 4);
 	add_topic_multi("sensor_baro", 1000, 4);
 	add_topic_multi("sensor_gps", 1000, 2);
+	add_topic_multi("sensor_gps_heading", 1000, 1);
 	add_topic_multi("sensor_gyro", 1000, 4);
 	add_topic_multi("sensor_mag", 1000, 4);
 	add_topic_multi("vehicle_imu", 500, 4);

src/modules/navigator/loiter.cpp

@@ -100,22 +100,11 @@ Loiter::set_loiter_position()
 
 	_loiter_pos_set = true;
 
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	if (_navigator->get_land_detected()->landed) {
-		_mission_item.nav_cmd = NAV_CMD_IDLE;
-
-	} else {
-		if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
-			setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
-
-		} else {
-			setLoiterItemFromCurrentPosition(&_mission_item);
-		}
-
-	}
+	// set current mission item to loiter
+	set_loiter_item(&_mission_item, _navigator->get_loiter_min_alt());
 
 	// 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.valid = false;
 	mission_apply_limitation(_mission_item);

src/modules/navigator/mission.cpp

@@ -683,22 +683,11 @@ Mission::set_mission_items()
 
 		_mission_type = MISSION_TYPE_NONE;
 
-		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-		if (_navigator->get_land_detected()->landed) {
-			_mission_item.nav_cmd = NAV_CMD_IDLE;
-
-		} else {
-			if (pos_sp_triplet->current.valid && pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
-				setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
-
-			} else {
-				setLoiterItemFromCurrentPosition(&_mission_item);
-			}
-
-		}
+		/* set loiter mission item and ensure that there is a minimum clearance from home */
+		set_loiter_item(&_mission_item, _navigator->get_takeoff_min_alt());
 
 		/* update position setpoint triplet  */
+		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		pos_sp_triplet->previous.valid = false;
 		mission_apply_limitation(_mission_item);
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);

src/modules/navigator/mission_block.cpp

@@ -375,9 +375,6 @@ MissionBlock::is_mission_item_reached()
 			struct position_setpoint_s *curr_sp_new = &_navigator->get_position_setpoint_triplet()->current;
 			const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
 
-			const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat,
-							next_sp.lon);
-
 			/* 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
@@ -386,10 +383,11 @@ MissionBlock::is_mission_item_reached()
 							  curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER &&
 							  (_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT);
 
-			// can only enforce exit heading if next waypoint is not within loiter radius of current waypoint
-			const bool exit_heading_is_reachable = dist_current_next > 1.2f * curr_sp_new->loiter_radius;
+			if (enforce_exit_heading) {
 
-			if (enforce_exit_heading && exit_heading_is_reachable) {
+
+				const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat,
+								next_sp.lon);
 
 				float yaw_err = 0.0f;
 
@@ -441,9 +439,7 @@ MissionBlock::is_mission_item_reached()
 					bearing += inner_angle;
 				}
 
-				// set typ to position, will get set to loiter in the fw position controller once close
-				// and replace current setpoint lat/lon with tangent coordinate
-				curr_sp.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
+				// Replace current setpoint lat/lon with tangent coordinate
 				waypoint_from_heading_and_distance(curr_sp.lat, curr_sp.lon,
 								   bearing, fabsf(curr_sp.loiter_radius),
 								   &curr_sp.lat, &curr_sp.lon);
@@ -644,40 +640,42 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 }
 
 void
-MissionBlock::setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item)
+MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
 {
-	setLoiterItemCommonFields(item);
+	if (_navigator->get_land_detected()->landed) {
+		/* landed, don't takeoff, but switch to IDLE mode */
+		item->nav_cmd = NAV_CMD_IDLE;
 
-	const position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	} else {
+		item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
 
-	item->lat = pos_sp_triplet->current.lat;
-	item->lon = pos_sp_triplet->current.lon;
-	item->altitude = pos_sp_triplet->current.alt;
-	item->loiter_radius = pos_sp_triplet->current.loiter_radius;
-}
+		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
-void
-MissionBlock::setLoiterItemFromCurrentPosition(struct mission_item_s *item)
-{
-	setLoiterItemCommonFields(item);
+		if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) {
+			/* use current position setpoint */
+			item->lat = pos_sp_triplet->current.lat;
+			item->lon = pos_sp_triplet->current.lon;
+			item->altitude = pos_sp_triplet->current.alt;
 
-	item->lat = _navigator->get_global_position()->lat;
-	item->lon = _navigator->get_global_position()->lon;
-	item->altitude = _navigator->get_global_position()->alt;
-	item->loiter_radius = _navigator->get_loiter_radius();
-}
+		} 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;
 
-void
-MissionBlock::setLoiterItemCommonFields(struct mission_item_s *item)
-{
-	item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+			if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
+				item->altitude = _navigator->get_home_position()->alt + min_clearance;
+			}
+		}
 
-	item->altitude_is_relative = false;
-	item->acceptance_radius = _navigator->get_acceptance_radius();
-	item->yaw = NAN;
-	item->time_inside = 0.0f;
-	item->autocontinue = false;
-	item->origin = ORIGIN_ONBOARD;
+		item->altitude_is_relative = false;
+		item->yaw = NAN;
+		item->loiter_radius = _navigator->get_loiter_radius();
+		item->acceptance_radius = _navigator->get_acceptance_radius();
+		item->time_inside = 0.0f;
+		item->autocontinue = false;
+		item->origin = ORIGIN_ONBOARD;
+	}
 }
 
 void

src/modules/navigator/mission_block.h

@@ -108,11 +108,10 @@ protected:
 	 */
 	bool mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp);
 
-	void setLoiterItemFromCurrentPositionSetpoint(struct mission_item_s *item);
-
-	void setLoiterItemFromCurrentPosition(struct mission_item_s *item);
-
-	void setLoiterItemCommonFields(struct mission_item_s *item);
+	/**
+	 * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
+	 */
+	void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f);
 
 	/**
 	 * Set a takeoff mission item

src/modules/navigator/navigator_main.cpp

@@ -306,8 +306,8 @@ Navigator::run()
 
 					} else if (PX4_ISFINITE(cmd.param7)) {
 						// Received only a request to change altitude, thus we keep the setpoint
-						rep->current.lat = PX4_ISFINITE(curr->current.lat) ? curr->current.lat : get_global_position()->lat;
-						rep->current.lon = PX4_ISFINITE(curr->current.lon) ? curr->current.lon : get_global_position()->lon;
+						rep->current.lat = curr->current.lat;
+						rep->current.lon = curr->current.lon;
 						rep->current.alt = cmd.param7;
 						only_alt_change_requested = true;
 
@@ -343,23 +343,16 @@ Navigator::run()
 						}
 					}
 
-					if (only_alt_change_requested) {
-						if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > 0) {
-							rep->current.loiter_radius = curr->current.loiter_radius;
+					if (only_alt_change_requested && PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > 0) {
+						rep->current.loiter_radius = curr->current.loiter_radius;
+						rep->current.loiter_direction = curr->current.loiter_direction;
 
-
-						} else {
-							rep->current.loiter_radius = get_loiter_radius();
-						}
-
-						if (curr->current.loiter_direction == 1 || curr->current.loiter_direction == -1) {
-							rep->current.loiter_direction = curr->current.loiter_direction;
-
-						} else {
-							rep->current.loiter_direction = 1;
-						}
+					} else {
+						rep->current.loiter_radius = get_loiter_radius();
+						rep->current.loiter_direction = 1;
 					}
 
+					rep->previous.valid = true;
 					rep->previous.timestamp = hrt_absolute_time();
 
 					rep->current.valid = true;

src/modules/navigator/takeoff.cpp

@@ -66,25 +66,11 @@ Takeoff::on_active()
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
 
-		position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
 		// set loiter item so position controllers stop doing takeoff logic
-		if (_navigator->get_land_detected()->landed) {
-			_mission_item.nav_cmd = NAV_CMD_IDLE;
-
-		} else {
-			if (pos_sp_triplet->current.valid) {
-				setLoiterItemFromCurrentPositionSetpoint(&_mission_item);
-
-			} else {
-				setLoiterItemFromCurrentPosition(&_mission_item);
-			}
-		}
-
+		set_loiter_item(&_mission_item);
+		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 		mission_apply_limitation(_mission_item);
-
 		mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
-
 		_navigator->set_position_setpoint_triplet_updated();
 	}
 }