WIP: allow selecting the RC override mode with a parameter

* Options are Position and Postion Slow.
2026-06-10 16:20:05 +08:00 · 2024-05-02 12:07:00 +02:00
84 changed files with 809 additions and 1431 deletions
@@ -15,7 +15,6 @@
 	"extensions": [
 		"chiehyu.vscode-astyle",
 		"dan-c-underwood.arm",
-		"editorconfig.editorconfig",
 		"fredericbonnet.cmake-test-adapter",
 		"github.vscode-pull-request-github",
 		"marus25.cortex-debug",
@@ -4,7 +4,6 @@
    "recommendations": [
        "chiehyu.vscode-astyle",
        "dan-c-underwood.arm",
-        "editorconfig.editorconfig",
        "fredericbonnet.cmake-test-adapter",
        "github.vscode-pull-request-github",
        "marus25.cortex-debug",
@@ -485,7 +485,7 @@ include(package)

 # install python requirements using configured python
 add_custom_target(install_python_requirements
-	COMMAND ${PYTHON_EXECUTABLE} -m pip install --break-system-packages --requirement ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt
+	COMMAND ${PYTHON_EXECUTABLE} -m pip install --requirement ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt
 	DEPENDS ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt
 	USES_TERMINAL
 )
@@ -34,23 +34,24 @@ def extract_timer(line):
    if search:
        return search.group(1), 'generic'

-    # NXP FlexPWM format format: initIOPWM(PWM::FlexPWM2),
-    search = re.search('PWM::Flex([0-9a-zA-Z_]+)..PWM::Submodule([0-9])[,\)]', line, re.IGNORECASE)
+    # nxp rt1062 format: initIOPWM(PWM::FlexPWM2),
+    search = re.search('PWM::Flex([0-9a-zA-Z_]+)[,\)]', line, re.IGNORECASE)
    if search:
-        return (search.group(1) + '_' +  search.group(2)), 'imxrt'
+        return search.group(1), 'imxrt'

    return None, 'unknown'

-def extract_timer_from_channel(line, timer_names):
+def extract_timer_from_channel(line, num_channels_already_found):
    # Try format: initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
    search = re.search('Timer::([0-9a-zA-Z_]+), ', line, re.IGNORECASE)
    if search:
        return search.group(1)

-    # NXP FlexPWM format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06),
-    search = re.search('PWM::(PWM[0-9]+).*PWM::Submodule([0-9])', line, re.IGNORECASE)
+    # nxp rt1062 format: initIOTimerChannel(io_timers, {PWM::PWM2_PWM_A, PWM::Submodule0}, IOMUX::Pad::GPIO_B0_06),
+    search = re.search('PWM::(PWM[0-9]+)[_,\)]', line, re.IGNORECASE)
    if search:
-        return str(timer_names.index((search.group(1) + '_' +  search.group(2))))
+        # imxrt uses a 1:1 timer group to channel association
+        return str(num_channels_already_found)

    return None

@@ -68,7 +69,6 @@ def get_timer_groups(timer_config_file, verbose=False):
    open_idx, close_idx = find_matching_brackets(('{', '}'), timer_config, verbose)
    timers_str = timer_config[open_idx:close_idx]
    timers = []
-    timer_names = []
    for line in timers_str.splitlines():
        line = line.strip()
        if len(line) == 0 or line.startswith('//'):
@@ -77,12 +77,14 @@ def get_timer_groups(timer_config_file, verbose=False):

        if timer_type == 'imxrt':
            if verbose: print('imxrt timer found')
-            timer_names.append(timer)
-            timers.append(str(len(timers)))
-            dshot_support = {str(i): False for i in range(16)}
+            max_num_channels = 16 # Just add a fixed number of timers
+            timers = [str(i) for i in range(max_num_channels)]
+            dshot_support = {str(i): False for i in range(max_num_channels)}
            for i in range(8): # First 8 channels support dshot
                dshot_support[str(i)] = True
-        elif timer:
+            break
+
+        if timer:
            if verbose: print('found timer def: {:}'.format(timer))
            dshot_support[timer] = 'DMA' in line
            timers.append(timer)
@@ -109,7 +111,7 @@ def get_timer_groups(timer_config_file, verbose=False):
            continue

        if verbose: print('--'+line+'--')
-        timer = extract_timer_from_channel(line, timer_names)
+        timer = extract_timer_from_channel(line, len(channel_timers))

        if timer:
            if verbose: print('Found timer: {:} in channel line {:}'.format(timer, line))
@@ -6,7 +6,6 @@
 param set-default CBRK_IO_SAFETY 0
 param set-default CANNODE_SUB_MBD 1
 param set-default CANNODE_SUB_RTCM 1
-param set-default GPS_1_GNSS 63
 param set-default MBE_ENABLE 1
 param set-default SENS_IMU_CLPNOTI 0

@@ -47,7 +47,7 @@
 #define GPIO_BTN_SAFETY       /* PB15 */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTB|GPIO_PIN15)

 /* Safety LED */
-#define GPIO_LED_SAFETY       /* PA1  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_LED_SAFETY       /* PA1  */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1)

 /* Tone alarm output. */
 #define TONE_ALARM_TIMER             2    /* timer 2 */
@@ -15,7 +15,6 @@ CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA=y
-CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y
 CONFIG_DRIVERS_OPTICAL_FLOW_PAW3902=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_PWM_OUT=y
@@ -32,17 +32,17 @@ then
 	param set-default SENS_TEMP_ID 2490378
 fi

-param set-default EKF2_BARO_DELAY 39
+param set-default EKF2_BARO_DELAY 13
 param set-default EKF2_BARO_NOISE 0.9
-param set-default EKF2_HGT_REF 0
-param set-default EKF2_MAG_DELAY 60
+param set-default EKF2_HGT_REF 2
+param set-default EKF2_MAG_DELAY 100
 param set-default EKF2_MAG_NOISE 0.06
 param set-default EKF2_MULTI_IMU 2
 param set-default EKF2_OF_CTRL 1
-param set-default EKF2_OF_DELAY 28
+param set-default EKF2_OF_DELAY 48
 param set-default EKF2_OF_N_MIN 0.05
 param set-default EKF2_RNG_A_HMAX 25
-param set-default EKF2_RNG_DELAY 105
+param set-default EKF2_RNG_DELAY 48
 param set-default EKF2_RNG_NOISE 0.03
 param set-default EKF2_RNG_QLTY_T 0.1
 param set-default EKF2_RNG_SFE 0.03
@@ -21,10 +21,8 @@ then
 fi

 # Internal magnetometer on I2C
-if ! iis2mdc -R 4 -I -b 4 start
-then
-	mmc5983ma -I -b 4 start
-fi
+# TODO: Write a driver for the MMC5983MA
+mmc5983ma -I -b 4 start

 # Internal Baro on I2C
 bmp388 -I -b 4 start
@@ -185,6 +185,7 @@ CONFIG_LPUART8_TXDMA=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
+CONFIG_MMCSD_MULTIBLOCK_LIMIT=1
 CONFIG_MMCSD_SDIO=y
 CONFIG_MTD=y
 CONFIG_MTD_BYTE_WRITE=y
@@ -86,6 +86,7 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_REFLECT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
 CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_SERIAL_TEST=y
@@ -1,5 +1,5 @@
 uint64 timestamp				# time since system start (microseconds)
-uint64 last_at_ok_timestamp			# timestamp of the last "OK" received after the "AT" command
+uint64 last_heartbeat				# timestamp of the last successful sbd session
 uint16 tx_buf_write_index			# current size of the tx buffer
 uint16 rx_buf_read_index			# the rx buffer is parsed up to that index
 uint16 rx_buf_end_index				# current size of the rx buffer
@@ -1,4 +1,4 @@
-# A logging message, output with PX4_WARN, PX4_ERR, PX4_INFO
+# A logging message, output with PX4_{WARN,ERR,INFO}

 uint64 timestamp		# time since system start (microseconds)

@@ -165,10 +165,7 @@ int8 ARMING_ACTION_ARM = 1
 uint8 GRIPPER_ACTION_RELEASE = 0
 uint8 GRIPPER_ACTION_GRAB = 1

-uint8 frame # The coordinate system of the Command
-uint8 FRAME_UNKNOWN = 0 # Not specified
-uint8 FRAME_GLOBAL = 1 # Global WGS84 coordinate frame + altitude relative to mean sea level
-uint8 FRAME_GLOBAL_RELATIVE_ALTITUDE = 2 # Global WGS84 coordinate frame + altitude relative to the home position
+uint8 ORB_QUEUE_LENGTH = 8

 float32 param1			# Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.
 float32 param2			# Parameter 2, as defined by MAVLink uint16 VEHICLE_CMD enum.
@@ -187,6 +184,4 @@ bool from_external

 uint16 COMPONENT_MODE_EXECUTOR_START = 1000

-uint8 ORB_QUEUE_LENGTH = 8
-
 # TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor
@@ -5,16 +5,13 @@
 uint64 timestamp		# time since system start (microseconds)

 # Result cases. This follows the MAVLink MAV_RESULT enum definition
-uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command is valid (is supported and has valid parameters), and was executed
-uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command is valid, but cannot be executed at this time
-uint8 VEHICLE_CMD_RESULT_DENIED = 2 # Command is invalid (is supported but has invalid parameters)
-uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3 # Command is not supported (unknown)
-uint8 VEHICLE_CMD_RESULT_FAILED = 4 # Command is valid, but execution has failed
-uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5 # Command is valid and is being executed
-uint8 VEHICLE_CMD_RESULT_CANCELLED = 6 # Command has been cancelled (as a result of receiving a COMMAND_CANCEL message)
-uint8 VEHICLE_CMD_RESULT_COMMAND_LONG_ONLY = 7 # Command is only accepted when sent as a COMMAND_LONG
-uint8 VEHICLE_CMD_RESULT_COMMAND_INT_ONLY = 8 # Command is only accepted when sent as a COMMAND_INT
-uint8 VEHICLE_CMD_RESULT_COMMAND_UNSUPPORTED_MAV_FRAME = 9 # Command is invalid because a frame is required and the specified frame is not supported
+uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
+uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |
+uint8 VEHICLE_CMD_RESULT_DENIED = 2			# Command PERMANENTLY DENIED |
+uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3		# Command UNKNOWN/UNSUPPORTED |
+uint8 VEHICLE_CMD_RESULT_FAILED = 4			# Command executed, but failed |
+uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5		# Command being executed |
+uint8 VEHICLE_CMD_RESULT_CANCELLED = 6			# Command Canceled

 # Arming denied specific cases
 uint16 ARM_AUTH_DENIED_REASON_GENERIC = 0
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 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
@@ -31,67 +31,18 @@
 *
 ****************************************************************************/

-#include "iis2mdc.h"
-#include <drivers/device/i2c.h>
+/*
+ * This header defines the events::EventType type.
+ */

-class IIS2MDC_I2C : public device::I2C
+#pragma once
+
+#include <uORB/topics/event.h>
+
+namespace events
 {
-public:
-	IIS2MDC_I2C(const I2CSPIDriverConfig &config);
-	virtual ~IIS2MDC_I2C() = default;
+using EventType = event_s;
+} // namespace events

-	virtual int read(unsigned address, void *data, unsigned count) override;
-	virtual int write(unsigned address, void *data, unsigned count) override;

-protected:
-	virtual int probe();
-};

-IIS2MDC_I2C::IIS2MDC_I2C(const I2CSPIDriverConfig &config) :
-	I2C(config)
-{
-}
-
-int IIS2MDC_I2C::probe()
-{
-	uint8_t data = 0;
-
-	if (read(IIS2MDC_ADDR_WHO_AM_I, &data, 1)) {
-		DEVICE_DEBUG("read_reg fail");
-		return -EIO;
-	}
-
-	if (data != IIS2MDC_WHO_AM_I) {
-		DEVICE_DEBUG("IIS2MDC bad ID: %02x", data);
-		return -EIO;
-	}
-
-	_retries = 1;
-
-	return OK;
-}
-
-int IIS2MDC_I2C::read(unsigned address, void *data, unsigned count)
-{
-	uint8_t cmd = address;
-	return transfer(&cmd, 1, (uint8_t *)data, count);
-}
-
-int IIS2MDC_I2C::write(unsigned address, void *data, unsigned count)
-{
-	uint8_t buf[32];
-
-	if (sizeof(buf) < (count + 1)) {
-		return -EIO;
-	}
-
-	buf[0] = address;
-	memcpy(&buf[1], data, count);
-
-	return transfer(&buf[0], count + 1, nullptr, 0);
-}
-
-device::Device *IIS2MDC_I2C_interface(const I2CSPIDriverConfig &config)
-{
-	return new IIS2MDC_I2C(config);
-}
@@ -50,15 +50,3 @@
 * @return 0 on success, -errno otherwise
 */
 int px4_get_parameter_value(const char *option, int &value);
-
-/**
- * Parse a CLI argument to a float. There are 2 valid formats:
- * - 'p:<param_name>'
- *   in this case the parameter is loaded from an integer parameter
- * - <float>
- *   a floating-point value, so just a string to float conversion is done
- * @param option CLI argument
- * @param value returned value
- * @return 0 on success, -errno otherwise
- */
-int px4_get_parameter_value(const char *option, float &value);
@@ -45,7 +45,7 @@
 #endif

 #include <events/events_generated.h>
-#include <uORB/topics/event.h>
+#include <libevents_definitions.h>

 #include <stdint.h>

@@ -93,7 +93,7 @@ constexpr unsigned sizeofArguments(const T &t, const Args &... args)
 /**
 * publish/send an event
 */
-void send(event_s &event);
+void send(EventType &event);

 /**
 * Generate event ID from an event name
@@ -109,7 +109,7 @@ constexpr uint32_t ID(const char (&name)[N])
 template<typename... Args>
 inline void send(uint32_t id, const LogLevels &log_levels, const char *message, Args... args)
 {
-	event_s e{};
+	EventType e{};
 	e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external;
 	e.id = id;
 	static_assert(util::sizeofArguments(args...) <= sizeof(e.arguments), "Too many arguments");
@@ -120,7 +120,7 @@ inline void send(uint32_t id, const LogLevels &log_levels, const char *message,

 inline void send(uint32_t id, const LogLevels &log_levels, const char *message)
 {
-	event_s e{};
+	EventType e{};
 	e.log_levels = ((uint8_t)log_levels.internal << 4) | (uint8_t)log_levels.external;
 	e.id = id;
 	CONSOLE_PRINT_EVENT(e.log_level_external, e.id, message);
@@ -59,7 +59,6 @@ int px4_get_parameter_value(const char *option, int &value)
 		if (param_handle != PARAM_INVALID) {

 			if (param_type(param_handle) != PARAM_TYPE_INT32) {
-				PX4_ERR("Type of param '%s' is different from INT32", param_name);
 				return -EINVAL;
 			}

@@ -88,48 +87,3 @@ int px4_get_parameter_value(const char *option, int &value)

 	return 0;
 }
-
-int px4_get_parameter_value(const char *option, float &value)
-{
-	value = 0;
-
-	/* check if this is a param name */
-	if (strncmp("p:", option, 2) == 0) {
-
-		const char *param_name = option + 2;
-
-		/* user wants to use a param name */
-		param_t param_handle = param_find(param_name);
-
-		if (param_handle != PARAM_INVALID) {
-
-			if (param_type(param_handle) != PARAM_TYPE_FLOAT) {
-				PX4_ERR("Type of param '%s' is different from FLOAT", param_name);
-				return -EINVAL;
-			}
-
-			float pwm_parm;
-			int ret = param_get(param_handle, &pwm_parm);
-
-			if (ret != 0) {
-				return ret;
-			}
-
-			value = pwm_parm;
-
-		} else {
-			PX4_ERR("param name '%s' not found", param_name);
-			return -ENXIO;
-		}
-
-	} else {
-		char *ep;
-		value = strtof(option, &ep);
-
-		if (*ep != '\0') {
-			return -EINVAL;
-		}
-	}
-
-	return 0;
-}
@@ -255,34 +255,6 @@ static inline int channels_timer(unsigned channel)
 	return timer_io_channels[channel].timer_index;
 }

-static uint32_t get_timer_channels(unsigned timer)
-{
-	uint32_t channels = 0;
-	static uint32_t channels_cache[MAX_IO_TIMERS] = {0};
-
-	if (validate_timer_index(timer) < 0) {
-		return channels;
-
-	} else {
-		if (channels_cache[timer] == 0) {
-			/* Gather the channel bits that belong to the timer */
-
-			uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index;
-			uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count;
-
-			for (unsigned chan_index = first_channel_index; chan_index < last_channel_index; chan_index++) {
-				channels |= 1 << chan_index;
-			}
-
-			/* cache them */
-
-			channels_cache[timer] = channels;
-		}
-	}
-
-	return channels_cache[timer];
-}
-
 static uint32_t get_channel_mask(unsigned channel)
 {
 	return io_timer_validate_channel_index(channel) == 0 ? 1 << channel : 0;
@@ -419,66 +391,41 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
 	return rv;
 }

-static int timer_set_rate(unsigned timer, unsigned rate)
+static int timer_set_rate(unsigned channel, unsigned rate)
 {
-	int channels = get_timer_channels(timer);
-
 	irqstate_t flags = px4_enter_critical_section();
-
-	for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) {
-		if ((1 << channel) & channels) {
-			rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
-							   ;
-			rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1;
-			rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
-
-		}
-	}
-
+	rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
+					   ;
+	rVAL1(channels_timer(channel), timer_io_channels[channel].sub_module) = (BOARD_PWM_FREQ / rate) - 1;
+	rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
 	px4_leave_critical_section(flags);
 	return 0;
 }

-static inline void io_timer_set_oneshot_mode(unsigned timer)
+static inline void io_timer_set_oneshot_mode(unsigned channel)
 {
-	int channels = get_timer_channels(timer);
-
 	irqstate_t flags = px4_enter_critical_section();
-
-	for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) {
-		if ((1 << channel) & channels) {
-			uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
-			rvalue &= ~SMCTRL_PRSC_MASK;
-			rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD;
-			rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
-							   ;
-			rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module)  = rvalue;
-			rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
-		}
-	}
-
+	uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
+	rvalue &= ~SMCTRL_PRSC_MASK;
+	rvalue |= SMCTRL_PRSC_DIV2 | SMCTRL_LDMOD;
+	rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
+					   ;
+	rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module)  = rvalue;
+	rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
 	px4_leave_critical_section(flags);

 }

-static inline void io_timer_set_PWM_mode(unsigned timer)
+static inline void io_timer_set_PWM_mode(unsigned channel)
 {
-	int channels = get_timer_channels(timer);
-
 	irqstate_t flags = px4_enter_critical_section();
-
-	for (uint32_t channel = 0; channel < DIRECT_PWM_OUTPUT_CHANNELS; ++channel) {
-		if ((1 << channel) & channels) {
-			uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
-			rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD);
-			rvalue |= SMCTRL_PRSC_DIV16;
-			rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
-							   ;
-			rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module)  = rvalue;
-			rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
-		}
-	}
-
+	uint16_t rvalue = rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module);
+	rvalue &= ~(SMCTRL_PRSC_MASK | SMCTRL_LDMOD);
+	rvalue |= SMCTRL_PRSC_DIV16;
+	rMCTRL(channels_timer(channel)) |= (timer_io_channels[channel].sub_module_bits >> MCTRL_LDOK_SHIFT) << MCTRL_CLDOK_SHIFT
+					   ;
+	rCTRL(channels_timer(channel), timer_io_channels[channel].sub_module)  = rvalue;
+	rMCTRL(channels_timer(channel)) |= timer_io_channels[channel].sub_module_bits;
 	px4_leave_critical_section(flags);
 }

@@ -583,35 +530,33 @@ int io_timer_init_timer(unsigned timer, io_timer_channel_mode_t mode)
 			break;
 		}

-		int channels = get_timer_channels(timer);
+		uint32_t first_channel_index = io_timers_channel_mapping.element[timer].first_channel_index;
+		uint32_t last_channel_index = first_channel_index + io_timers_channel_mapping.element[timer].channel_count;

-		for (uint32_t chan = 0; chan < DIRECT_PWM_OUTPUT_CHANNELS; ++chan) {
-			if ((1 << chan) & channels) {
+		for (uint32_t chan = first_channel_index; chan < last_channel_index; chan++) {

-				/* Clear all Faults */
-				rFSTS0(timer) = FSTS_FFLAG_MASK;
+			/* Clear all Faults */
+			rFSTS0(timer) = FSTS_FFLAG_MASK;

-				rCTRL2(timer, timer_io_channels[chan].sub_module) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP;
-				rCTRL(timer, timer_io_channels[chan].sub_module)  = SMCTRL_PRSC_DIV16 | SMCTRL_FULL;
-				/* Edge aligned at 0 */
-				rINIT(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
-				rVAL0(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
-				rVAL2(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
-				rVAL4(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
-				rFFILT0(timer) &= ~FFILT_FILT_PER_MASK;
-				rDISMAP0(timer, timer_io_channels[chan].sub_module) = 0xf000;
-				rDISMAP1(timer, timer_io_channels[chan].sub_module) = 0xf000;
-				rOUTEN(timer) |= timer_io_channels[chan].val_offset == PWMA_VAL ? OUTEN_PWMA_EN(1 << timer_io_channels[chan].sub_module)
-						 : OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module);
-				rDTSRCSEL(timer) = 0;
-				rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module);
-				rMCTRL(timer) = timer_io_channels[chan].sub_module_bits;
-			}
+			rCTRL2(timer, timer_io_channels[chan].sub_module) = SMCTRL2_CLK_SEL_EXT_CLK | SMCTRL2_DBGEN | SMCTRL2_INDEP;
+			rCTRL(timer, timer_io_channels[chan].sub_module)  = SMCTRL_PRSC_DIV16 | SMCTRL_FULL;
+			/* Edge aligned at 0 */
+			rINIT(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
+			rVAL0(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
+			rVAL2(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
+			rVAL4(channels_timer(chan), timer_io_channels[chan].sub_module) = 0;
+			rFFILT0(timer) &= ~FFILT_FILT_PER_MASK;
+			rDISMAP0(timer, timer_io_channels[chan].sub_module) = 0xf000;
+			rDISMAP1(timer, timer_io_channels[chan].sub_module) = 0xf000;
+			rOUTEN(timer) |= timer_io_channels[chan].val_offset == PWMA_VAL ? OUTEN_PWMA_EN(1 << timer_io_channels[chan].sub_module)
+					 : OUTEN_PWMB_EN(1 << timer_io_channels[chan].sub_module);
+			rDTSRCSEL(timer) = 0;
+			rMCTRL(timer) = MCTRL_LDOK(1 << timer_io_channels[chan].sub_module);
+			rMCTRL(timer) = timer_io_channels[chan].sub_module_bits;
+			io_timer_set_PWM_mode(chan);
+			timer_set_rate(chan, 50);
 		}

-		io_timer_set_PWM_mode(timer);
-		timer_set_rate(timer, 50);
-
 		px4_leave_critical_section(flags);
 	}

@@ -873,7 +818,8 @@ uint16_t io_channel_get_ccr(unsigned channel)
 	return value;
 }

-uint32_t io_timer_get_group(unsigned timer)
+// The rt has 1:1 group to channel
+uint32_t io_timer_get_group(unsigned group)
 {
-	return get_timer_channels(timer);
+	return get_channel_mask(group);
 }
@@ -68,10 +68,7 @@ BMP388::init()
 		return -EIO;
 	}

-	if (_interface->get_reg(BMP3_CHIP_ID_ADDR, &_chip_id) != OK) {
-		PX4_WARN("failed to get chip id");
-		return -EIO;
-	}
+	_chip_id = _interface->get_reg(BMP3_CHIP_ID_ADDR);

 	if (_chip_id != BMP388_CHIP_ID && _chip_id != BMP390_CHIP_ID) {
 		PX4_WARN("id of your baro is not: 0x%02x or 0x%02x", BMP388_CHIP_ID, BMP390_CHIP_ID);
@@ -83,10 +80,7 @@ BMP388::init()
 		this->_item_name = "bmp390";
 	}

-	if (_interface->get_reg(BMP3_REV_ID_ADDR, &_chip_rev_id) != OK) {
-		PX4_WARN("failed to get chip rev id");
-		return -EIO;
-	}
+	_chip_rev_id = _interface->get_reg(BMP3_REV_ID_ADDR);

 	_cal = _interface->get_calibration(BMP3_CALIB_DATA_ADDR);

@@ -210,22 +204,14 @@ BMP388::soft_reset()
 	uint8_t status;
 	int     ret;

-	ret = _interface->get_reg(BMP3_SENS_STATUS_REG_ADDR, &status);
-
-	if (ret != OK) {
-		return false;
-	}
+	status = _interface->get_reg(BMP3_SENS_STATUS_REG_ADDR);

 	if (status & BMP3_CMD_RDY) {
 		ret = _interface->set_reg(BPM3_CMD_SOFT_RESET, BMP3_CMD_ADDR);

 		if (ret == OK) {
 			usleep(BMP3_POST_RESET_WAIT_TIME);
-			ret = _interface->get_reg(BMP3_ERR_REG_ADDR, &status);
-
-			if (ret != OK) {
-				return false;
-			}
+			status = _interface->get_reg(BMP3_ERR_REG_ADDR);

 			if ((status & BMP3_CMD_ERR) == 0) {
 				result = true;
@@ -283,9 +269,7 @@ BMP388::validate_trimming_param()

 	crc = (crc ^ 0xFF);

-	if (_interface->get_reg(BMP3_TRIM_CRC_DATA_ADDR, &stored_crc) != OK) {
-		return false;
-	}
+	stored_crc = _interface->get_reg(BMP3_TRIM_CRC_DATA_ADDR);

 	return stored_crc == crc;
 }
@@ -420,12 +404,7 @@ BMP388::set_op_mode(uint8_t op_mode)
 	uint8_t op_mode_reg_val;
 	int     ret = OK;

-	ret = _interface->get_reg(BMP3_PWR_CTRL_ADDR, &op_mode_reg_val);
-
-	if (ret != OK) {
-		return false;
-	}
-
+	op_mode_reg_val = _interface->get_reg(BMP3_PWR_CTRL_ADDR);
 	last_set_mode = BMP3_GET_BITS(op_mode_reg_val, BMP3_OP_MODE);

 	/* Device needs to be put in sleep mode to transition */
@@ -441,12 +420,7 @@ BMP388::set_op_mode(uint8_t op_mode)
 	}

 	if (ret == OK) {
-		ret = _interface->get_reg(BMP3_PWR_CTRL_ADDR, &op_mode_reg_val);
-
-		if (ret != OK) {
-			return false;
-		}
-
+		op_mode_reg_val = _interface->get_reg(BMP3_PWR_CTRL_ADDR);
 		op_mode_reg_val = BMP3_SET_BITS(op_mode_reg_val, BMP3_OP_MODE, op_mode);
 		ret = _interface->set_reg(op_mode_reg_val, BMP3_PWR_CTRL_ADDR);

@@ -292,7 +292,7 @@ public:
 	virtual int init() = 0;

 	// read reg value
-	virtual int get_reg(uint8_t addr, uint8_t *value) = 0;
+	virtual uint8_t get_reg(uint8_t addr) = 0;

 	// bulk read reg value
 	virtual int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len) = 0;
@@ -49,7 +49,7 @@ public:

 	int init();

-	int get_reg(uint8_t addr, uint8_t *value);
+	uint8_t get_reg(uint8_t addr);
 	int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len);
 	int set_reg(uint8_t value, uint8_t addr);
 	calibration_s *get_calibration(uint8_t addr);
@@ -78,10 +78,12 @@ int BMP388_I2C::init()
 	return I2C::init();
 }

-int BMP388_I2C::get_reg(uint8_t addr, uint8_t *value)
+uint8_t BMP388_I2C::get_reg(uint8_t addr)
 {
-	const uint8_t cmd = (uint8_t)(addr);
-	return transfer(&cmd, sizeof(cmd), value, 1);
+	uint8_t cmd[2] = { (uint8_t)(addr), 0};
+	transfer(&cmd[0], 1, &cmd[1], 1);
+
+	return cmd[1];
 }

 int BMP388_I2C::get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len)
@@ -66,7 +66,7 @@ public:

 	int init();

-	int get_reg(uint8_t addr, uint8_t *value);
+	uint8_t get_reg(uint8_t addr);
 	int get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len);
 	int set_reg(uint8_t value, uint8_t addr);
 	calibration_s *get_calibration(uint8_t addr);
@@ -95,16 +95,12 @@ int BMP388_SPI::init()
 	return SPI::init();
 };

-int BMP388_SPI::get_reg(uint8_t addr, uint8_t *value)
+uint8_t BMP388_SPI::get_reg(uint8_t addr)
 {
 	uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; //set MSB bit
-	int ret = transfer(&cmd[0], &cmd[0], 2);
+	transfer(&cmd[0], &cmd[0], 2);

-	if (ret == OK) {
-		*value = cmd[1];
-	}
-
-	return ret;
+	return cmd[1];
 }

 int BMP388_SPI::get_reg_buf(uint8_t addr, uint8_t *buf, uint8_t len)
@@ -75,7 +75,7 @@ SF45LaserSerial::SF45LaserSerial(const char *port, uint8_t rotation) :
 	// populate obstacle map members
 	_obstacle_map_msg.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
 	_obstacle_map_msg.increment = 5;
-	_obstacle_map_msg.angle_offset = 2.5;
+	_obstacle_map_msg.angle_offset = -2.5;
 	_obstacle_map_msg.min_distance = UINT16_MAX;
 	_obstacle_map_msg.max_distance = 5000;

@@ -157,6 +157,7 @@ int SF45LaserSerial::collect()
 	int64_t read_elapsed = hrt_elapsed_time(&_last_read);
 	int ret;
 	/* the buffer for read chars is buflen minus null termination */
+	param_get(param_find("SF45_CP_LIMIT"), &_collision_constraint);
 	uint8_t readbuf[SF45_MAX_PAYLOAD];

 	float distance_m = -1.0f;
@@ -668,35 +669,34 @@ void SF45LaserSerial::sf45_process_replies(float *distance_m)
 				raw_yaw = raw_yaw * -1;
 			}

-			// SF45/B product guide {Data output bit: 8 Description: "Yaw angle [1/100 deg] size: int16}"
-			scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
-
 			switch (_yaw_cfg) {
 			case 0:
 				break;

 			case 1:
-				if (scaled_yaw > 180) {
-					scaled_yaw = scaled_yaw - 180;
+				if (raw_yaw > 180) {
+					raw_yaw = raw_yaw - 180;

 				} else {
-					scaled_yaw = scaled_yaw + 180; // rotation facing aft
+					raw_yaw = raw_yaw + 180; // rotation facing aft
 				}

 				break;

 			case 2:
-				scaled_yaw = scaled_yaw + 90; // rotation facing right
+				raw_yaw = raw_yaw + 90; // rotation facing right
 				break;

 			case 3:
-				scaled_yaw = scaled_yaw - 90; // rotation facing left
+				raw_yaw = raw_yaw - 90; // rotation facing left
 				break;

 			default:
 				break;
 			}

+			scaled_yaw = raw_yaw * SF45_SCALE_FACTOR;
+
 			// Convert to meters for rangefinder update
 			*distance_m = raw_distance * SF45_SCALE_FACTOR;
 			obstacle_dist_cm = (uint16_t)raw_distance;
@@ -64,7 +64,6 @@
 #define DRV_MAG_DEVTYPE_IST8308  0x0B
 #define DRV_MAG_DEVTYPE_LIS2MDL  0x0C
 #define DRV_MAG_DEVTYPE_MMC5983MA  0x0D
-#define DRV_MAG_DEVTYPE_IIS2MDC  0x0E

 #define DRV_IMU_DEVTYPE_LSM303D  0x11

@@ -939,8 +939,7 @@ GPS::run()
 						set_device_type(DRV_GPS_DEVTYPE_UBX_9);
 						break;

-					case GPSDriverUBX::Board::u_blox9_F9P_L1L2:
-					case GPSDriverUBX::Board::u_blox9_F9P_L1L5:
+					case GPSDriverUBX::Board::u_blox9_F9P:
 						set_device_type(DRV_GPS_DEVTYPE_UBX_F9P);
 						break;

@@ -247,16 +247,14 @@ PARAM_DEFINE_INT32(GPS_2_PROTOCOL, 1);
 * 2 : Use Galileo
 * 3 : Use BeiDou
 * 4 : Use GLONASS
- * 5 : Use NAVIC
 *
 * @min 0
- * @max 63
+ * @max 31
 * @bit 0 GPS (with QZSS)
 * @bit 1 SBAS
 * @bit 2 Galileo
 * @bit 3 BeiDou
 * @bit 4 GLONASS
- * @bit 5 NAVIC
 *
 * @reboot_required true
 * @group GPS
@@ -279,16 +277,14 @@ PARAM_DEFINE_INT32(GPS_1_GNSS, 0);
 * 2 : Use Galileo
 * 3 : Use BeiDou
 * 4 : Use GLONASS
- * 5 : Use NAVIC
 *
 * @min 0
- * @max 63
+ * @max 31
 * @bit 0 GPS (with QZSS)
 * @bit 1 SBAS
 * @bit 2 Galileo
 * @bit 3 BeiDou
 * @bit 4 GLONASS
- * @bit 5 NAVIC
 *
 * @reboot_required true
 * @group GPS
@@ -441,11 +441,10 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 			sensor_gps.altitude_msl_m = positionGpsLla.c[2];
 			sensor_gps.altitude_ellipsoid_m = sensor_gps.altitude_msl_m;

-			sensor_gps.vel_m_s = matrix::Vector3f(velocityGpsNed.c).length();
+			sensor_gps.vel_ned_valid = true;
 			sensor_gps.vel_n_m_s = velocityGpsNed.c[0];
 			sensor_gps.vel_e_m_s = velocityGpsNed.c[1];
 			sensor_gps.vel_d_m_s = velocityGpsNed.c[2];
-			sensor_gps.vel_ned_valid = true;

 			sensor_gps.hdop = dop.hDOP;
 			sensor_gps.vdop = dop.vDOP;
@@ -41,5 +41,4 @@ add_subdirectory(lis3mdl)
 add_subdirectory(lsm303agr)
 add_subdirectory(memsic)
 add_subdirectory(rm3100)
-add_subdirectory(st)
 add_subdirectory(vtrantech)
@@ -14,7 +14,6 @@ menu "Magnetometer"
        select DRIVERS_MAGNETOMETER_RM3100
        select DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L
        select DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA
-        select DRIVERS_MAGNETOMETER_ST_IIS2MDC
        ---help---
            Enable default set of magnetometer drivers
    rsource "*/Kconfig"
@@ -1,3 +0,0 @@
-menu "ST"
-    rsource "*/Kconfig"
-endmenu
@@ -1,45 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-px4_add_module(
-	MODULE drivers__magnetometer__st__iis2mdc
-	MAIN iis2mdc
-	COMPILE_FLAGS
-		# -DDEBUG_BUILD
-	SRCS
-		iis2mdc_i2c.cpp
-		iis2mdc_main.cpp
-		iis2mdc.cpp
-	DEPENDS
-		drivers_magnetometer
-		px4_work_queue
-	)
@@ -1,5 +0,0 @@
-menuconfig DRIVERS_MAGNETOMETER_ST_IIS2MDC
-	bool "iis2mdc"
-	default n
-	---help---
-		Enable support for iis2mdc
@@ -1,137 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 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 "iis2mdc.h"
-
-using namespace time_literals;
-
-IIS2MDC::IIS2MDC(device::Device *interface, const I2CSPIDriverConfig &config) :
-	I2CSPIDriver(config),
-	_interface(interface),
-	_px4_mag(interface->get_device_id(), config.rotation),
-	_sample_count(perf_alloc(PC_COUNT, "iis2mdc_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "iis2mdc_comms_errors"))
-{}
-
-IIS2MDC::~IIS2MDC()
-{
-	perf_free(_sample_count);
-	perf_free(_comms_errors);
-	delete _interface;
-}
-
-int IIS2MDC::init()
-{
-	if (hrt_absolute_time() < 20_ms) {
-		px4_usleep(20_ms); // ~10ms power-on time
-	}
-
-	write_register(IIS2MDC_ADDR_CFG_REG_A, MD_CONTINUOUS | ODR_100 | COMP_TEMP_EN);
-	write_register(IIS2MDC_ADDR_CFG_REG_B, OFF_CANC);
-	write_register(IIS2MDC_ADDR_CFG_REG_C, BDU);
-
-	_px4_mag.set_scale(100.f / 65535.f); // +/- 50 Gauss, 16bit
-
-	ScheduleDelayed(20_ms);
-
-	return PX4_OK;
-}
-
-void IIS2MDC::RunImpl()
-{
-	uint8_t status = read_register(IIS2MDC_ADDR_STATUS_REG);
-
-	if (status & IIS2MDC_STATUS_REG_READY) {
-		SensorData data = {};
-
-		if (read_register_block(&data) == PX4_OK) {
-			int16_t x = int16_t((data.xout1 << 8) | data.xout0);
-			int16_t y = int16_t((data.yout1 << 8) | data.yout0);
-			int16_t z = -int16_t((data.zout1 << 8) | data.zout0);
-			int16_t t = int16_t((data.tout1 << 8) | data.tout0);
-			// 16 bits twos complement with a sensitivity of 8 LSB/°C. Typically, the output zero level corresponds to 25 °C.
-			_px4_mag.set_temperature(float(t) / 8.f + 25.f);
-			_px4_mag.update(hrt_absolute_time(), x, y, z);
-			_px4_mag.set_error_count(perf_event_count(_comms_errors));
-			perf_count(_sample_count);
-
-		} else {
-			PX4_DEBUG("read failed");
-			perf_count(_comms_errors);
-		}
-
-	} else {
-		PX4_DEBUG("not ready: %u", status);
-		perf_count(_comms_errors);
-	}
-
-	ScheduleDelayed(10_ms);
-}
-
-uint8_t IIS2MDC::read_register_block(SensorData *data)
-{
-	uint8_t reg = IIS2MDC_ADDR_OUTX_L_REG;
-
-	if (_interface->read(reg, data, sizeof(SensorData)) != PX4_OK) {
-		perf_count(_comms_errors);
-
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
-}
-
-uint8_t IIS2MDC::read_register(uint8_t reg)
-{
-	uint8_t value = 0;
-
-	if (_interface->read(reg, &value, sizeof(value)) != PX4_OK) {
-		perf_count(_comms_errors);
-	}
-
-	return value;
-}
-
-void IIS2MDC::write_register(uint8_t reg, uint8_t value)
-{
-	if (_interface->write(reg, &value, sizeof(value)) != PX4_OK) {
-		perf_count(_comms_errors);
-	}
-}
-
-void IIS2MDC::print_status()
-{
-	I2CSPIDriverBase::print_status();
-	perf_print_counter(_sample_count);
-	perf_print_counter(_comms_errors);
-}
@@ -1,95 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 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.
- *
- ****************************************************************************/
-
-#pragma once
-
-#include <px4_platform_common/i2c_spi_buses.h>
-#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
-
-// IIS2MDC Registers
-#define IIS2MDC_ADDR_CFG_REG_A  0x60
-#define IIS2MDC_ADDR_CFG_REG_B  0x61
-#define IIS2MDC_ADDR_CFG_REG_C  0x62
-#define IIS2MDC_ADDR_STATUS_REG 0x67
-#define IIS2MDC_ADDR_OUTX_L_REG 0x68
-#define IIS2MDC_ADDR_WHO_AM_I   0x4F
-
-// IIS2MDC Definitions
-#define IIS2MDC_WHO_AM_I         0b01000000
-#define IIS2MDC_STATUS_REG_READY 0b00001111
-// CFG_REG_A
-#define COMP_TEMP_EN    (1 << 7)
-#define MD_CONTINUOUS   (0 << 0)
-#define ODR_100         ((1 << 3) | (1 << 2))
-// CFG_REG_B
-#define OFF_CANC        (1 << 1)
-// CFG_REG_C
-#define BDU             (1 << 4)
-
-extern device::Device *IIS2MDC_I2C_interface(const I2CSPIDriverConfig &config);
-
-class IIS2MDC : public I2CSPIDriver<IIS2MDC>
-{
-public:
-	IIS2MDC(device::Device *interface, const I2CSPIDriverConfig &config);
-	virtual ~IIS2MDC();
-
-	struct SensorData {
-		uint8_t xout0;
-		uint8_t xout1;
-		uint8_t yout0;
-		uint8_t yout1;
-		uint8_t zout0;
-		uint8_t zout1;
-		uint8_t tout0;
-		uint8_t tout1;
-	};
-
-	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
-	static void print_usage();
-
-	int init();
-	void print_status() override;
-
-	void RunImpl();
-
-private:
-	uint8_t read_register_block(SensorData *data);
-	uint8_t read_register(uint8_t reg);
-	void write_register(uint8_t reg, uint8_t value);
-
-	device::Device *_interface;
-	PX4Magnetometer _px4_mag;
-	perf_counter_t _sample_count;
-	perf_counter_t _comms_errors;
-};
@@ -1,116 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2024 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 "iis2mdc.h"
-#include <px4_platform_common/module.h>
-
-I2CSPIDriverBase *IIS2MDC::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
-{
-	device::Device *interface = IIS2MDC_I2C_interface(config);
-
-	if (interface == nullptr) {
-		PX4_ERR("alloc failed");
-		return nullptr;
-	}
-
-	if (interface->init() != OK) {
-		delete interface;
-		PX4_DEBUG("no device on bus %i (devid 0x%lx)", config.bus, config.spi_devid);
-		return nullptr;
-	}
-
-	IIS2MDC *dev = new IIS2MDC(interface, config);
-
-	if (dev == nullptr) {
-		delete interface;
-		return nullptr;
-	}
-
-	if (OK != dev->init()) {
-		delete dev;
-		return nullptr;
-	}
-
-	return dev;
-}
-
-void IIS2MDC::print_usage()
-{
-	PRINT_MODULE_USAGE_NAME("iis2mdc", "driver");
-	PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
-	PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
-	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x30);
-	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
-}
-
-extern "C" int iis2mdc_main(int argc, char *argv[])
-{
-	using ThisDriver = IIS2MDC;
-	int ch;
-	BusCLIArguments cli{true, false};
-	cli.i2c_address = 0x1E;
-	cli.default_i2c_frequency = 400000;
-
-	while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
-		switch (ch) {
-		case 'R':
-			cli.rotation = (enum Rotation)atoi(cli.optArg());
-			break;
-		}
-	}
-
-	const char *verb = cli.optArg();
-
-	if (!verb) {
-		ThisDriver::print_usage();
-		return -1;
-	}
-
-	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_IIS2MDC);
-
-	if (!strcmp(verb, "start")) {
-		return ThisDriver::module_start(cli, iterator);
-	}
-
-	if (!strcmp(verb, "stop")) {
-		return ThisDriver::module_stop(iterator);
-	}
-
-	if (!strcmp(verb, "status")) {
-		return ThisDriver::module_status(iterator);
-	}
-
-	ThisDriver::print_usage();
-	return -1;
-}
@@ -87,6 +87,14 @@ void PowerMonitorSelectorAuterion::Run()
 			int ret_val = ina226_probe(i);

 			if (ret_val == PX4_OK) {
+
+				float current_shunt_value = 0.0f;
+				param_get(param_find("INA226_SHUNT"), &current_shunt_value);
+
+				if (fabsf(current_shunt_value - _sensors[i].shunt_value) > FLT_EPSILON) {
+					param_set(param_find("INA226_SHUNT"), &(_sensors[i].shunt_value));
+				}
+
 				char bus_number[4] = {0};
 				itoa(_sensors[i].bus_number, bus_number, 10);
 				const char *start_argv[] {
@@ -34,4 +34,4 @@
 add_subdirectory(bst)
 add_subdirectory(frsky_telemetry)
 add_subdirectory(hott)
-add_subdirectory(iridiumsbd)
+#add_subdirectory(iridiumsbd)
@@ -235,7 +235,7 @@ int IridiumSBD::print_status()
 	PX4_INFO("RX session pending:           %d", _rx_session_pending);
 	PX4_INFO("RX read pending:              %d", _rx_read_pending);
 	PX4_INFO("Time since last signal check: %" PRId64, hrt_absolute_time() - _last_signal_check);
-	PX4_INFO("Last heartbeat:               %" PRId64, _last_at_ok_timestamp);
+	PX4_INFO("Last heartbeat:               %" PRId64, _last_heartbeat);
 	return 0;
 }

@@ -333,11 +333,6 @@ void IridiumSBD::standby_loop(void)
 		}
 	}

-	if (!is_modem_responsive()) {
-		VERBOSE_INFO("MODEM IS NOT RENSPONSIVE");
-		return;
-	}
-
 	// check for incoming SBDRING, handled inside read_at_command()
 	read_at_command();

@@ -482,6 +477,7 @@ void IridiumSBD::sbdsession_loop(void)
 		_ring_pending = false;
 		_tx_session_pending = false;
 		_last_read_time = hrt_absolute_time();
+		_last_heartbeat = _last_read_time;
 		++_successful_sbd_sessions;

 		if (mt_queued > 0) {
@@ -502,6 +498,8 @@ void IridiumSBD::sbdsession_loop(void)

 	case 1:
 		VERBOSE_INFO("SBD SESSION: MO SUCCESS, MT FAIL");
+		_last_heartbeat = hrt_absolute_time();
+
 		// after a successful session reset the tx buffer
 		_tx_buf_write_idx = 0;
 		++_successful_sbd_sessions;
@@ -554,6 +552,11 @@ void IridiumSBD::start_csq(void)

 	_last_signal_check = hrt_absolute_time();

+	if (!is_modem_ready()) {
+		VERBOSE_INFO("UPDATE SIGNAL QUALITY: MODEM NOT READY!");
+		return;
+	}
+
 	write_at("AT+CSQ");
 	_new_state = SATCOM_STATE_CSQ;
 }
@@ -568,6 +571,11 @@ void IridiumSBD::start_sbd_session(void)
 		VERBOSE_INFO("STARTING SBD SESSION");
 	}

+	if (!is_modem_ready()) {
+		VERBOSE_INFO("SBD SESSION: MODEM NOT READY!");
+		return;
+	}
+
 	if (_ring_pending) {
 		write_at("AT+SBDIXA");

@@ -602,8 +610,8 @@ void IridiumSBD::start_test(void)
 		printf("\n");
 	}

-	if (!is_modem_responsive()) {
-		PX4_WARN("MODEM NOT RENSPONSIVE!");
+	if (!is_modem_ready()) {
+		PX4_WARN("MODEM NOT READY!");
 		return;
 	}

@@ -710,6 +718,11 @@ ssize_t IridiumSBD::read(struct file *filp, char *buffer, size_t buflen)

 void IridiumSBD::write_tx_buf()
 {
+	if (!is_modem_ready()) {
+		VERBOSE_INFO("WRITE SBD: MODEM NOT READY!");
+		return;
+	}
+
 	pthread_mutex_lock(&_tx_buf_mutex);

 	char command[13];
@@ -766,6 +779,11 @@ void IridiumSBD::write_tx_buf()

 void IridiumSBD::read_rx_buf(void)
 {
+	if (!is_modem_ready()) {
+		VERBOSE_INFO("READ SBD: MODEM NOT READY!");
+		return;
+	}
+
 	pthread_mutex_lock(&_rx_buf_mutex);


@@ -931,12 +949,11 @@ satcom_uart_status IridiumSBD::open_uart(char *uart_name)
 	return SATCOM_UART_OK;
 }

-bool IridiumSBD::is_modem_responsive(void)
+bool IridiumSBD::is_modem_ready(void)
 {
 	write_at("AT");

 	if (read_at_command() == SATCOM_RESULT_OK) {
-		_last_at_ok_timestamp = hrt_absolute_time();
 		return true;

 	} else {
@@ -963,9 +980,9 @@ void IridiumSBD::publish_iridium_status()
 {
 	bool need_to_publish = false;

-	if (_status.last_at_ok_timestamp != _last_at_ok_timestamp) {
+	if (_status.last_heartbeat != _last_heartbeat) {
 		need_to_publish = true;
-		_status.last_at_ok_timestamp = _last_at_ok_timestamp;
+		_status.last_heartbeat = _last_heartbeat;
 	}

 	if (_status.tx_buf_write_index != _tx_buf_write_idx) {
@@ -245,7 +245,7 @@ private:
 	/*
 	 * Checks if the modem responds to the "AT" command
 	 */
-	bool is_modem_responsive(void);
+	bool is_modem_ready(void);

 	/*
 	 * Get the poll state
@@ -321,7 +321,7 @@ private:

 	hrt_abstime _last_write_time = 0;
 	hrt_abstime _last_read_time = 0;
-	hrt_abstime _last_at_ok_timestamp = 0;
+	hrt_abstime _last_heartbeat = 0;
 	hrt_abstime _session_start_time = 0;

 	satcom_state _state = SATCOM_STATE_STANDBY;
@@ -108,14 +108,12 @@ void AdsbConflict::remove_icao_address_from_conflict_list(int traffic_index)
 {
 	_traffic_buffer.icao_address.remove(traffic_index);
 	_traffic_buffer.timestamp.remove(traffic_index);
-	PX4_INFO("icao_address removed. Buffer Size: %d", (int)_traffic_buffer.timestamp.size());
 }

 void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address)
 {
 	_traffic_buffer.timestamp.push_back(hrt_absolute_time());
 	_traffic_buffer.icao_address.push_back(icao_address);
-	PX4_INFO("icao_address added. Buffer Size: %d", (int)_traffic_buffer.timestamp.size());
 }

 void AdsbConflict::get_traffic_state()
@@ -131,6 +129,7 @@ void AdsbConflict::get_traffic_state()

 	if (old_conflict && _conflict_detected) {
 		old_conflict_warning_expired = (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > CONFLICT_WARNING_TIMEOUT);
+
 	}

 	if (new_traffic && _conflict_detected && !_traffic_buffer_full) {
@@ -155,56 +154,65 @@ void AdsbConflict::get_traffic_state()

 }

-void AdsbConflict::remove_expired_conflicts()
-{
-	for (uint8_t traffic_index = 0; traffic_index < _traffic_buffer.timestamp.size();) {
-		if (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > TRAFFIC_CONFLICT_LIFETIME) {
-			events::send<uint32_t>(events::ID("navigator_traffic_expired"), events::Log::Notice,
-					       "Traffic Conflict {1} Expired and removed from buffer",
-					       _traffic_buffer.icao_address[traffic_index]);
-			remove_icao_address_from_conflict_list(traffic_index);
-
-		} else {
-			traffic_index++;
-		}
-	}
-}

 bool AdsbConflict::handle_traffic_conflict()
 {

 	get_traffic_state();

+	char uas_id[UTM_GUID_MSG_LENGTH]; //GUID of incoming UTM messages
+
+	//convert UAS_id byte array to char array for User Warning
+	for (int i = 0; i < 5; i++) {
+		snprintf(&uas_id[i * 2], sizeof(uas_id) - i * 2, "%02x", _transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - 5 + i]);
+	}
+
+	uint64_t uas_id_int = 0;
+
+	for (int i = 0; i < 8; i++) {
+		uas_id_int |= (uint64_t)(_transponder_report.uas_id[PX4_GUID_BYTE_LENGTH - i - 1]) << (i * 8);
+	}
+
+
 	bool take_action = false;

 	switch (_traffic_state) {

 	case TRAFFIC_STATE::ADD_CONFLICT:
 	case TRAFFIC_STATE::REMIND_CONFLICT: {
-			take_action = send_traffic_warning((int)(math::degrees(_transponder_report.heading) + 180.f),
-							   (int)fabsf(_crosstrack_error.distance), _transponder_report.flags,
+
+			take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180,
+							   (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id,
 							   _transponder_report.callsign,
-							   _transponder_report.icao_address);
+							   uas_id_int);
+
 		}
 		break;

 	case TRAFFIC_STATE::REMOVE_OLD_CONFLICT: {
-			events::send<uint32_t>(events::ID("navigator_traffic_resolved"), events::Log::Notice,
-					       "Traffic Conflict Resolved {1}!",
-					       _transponder_report.icao_address);
-			_last_traffic_warning_time = hrt_absolute_time();
+
+			mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC UPDATE: %s is no longer in conflict!",
+					     _transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ?
+					     _transponder_report.callsign : uas_id);
+
+			events::send<uint64_t>(events::ID("navigator_traffic_resolved"), events::Log::Critical, "Traffic Conflict Resolved",
+					       uas_id_int);
+
 		}
 		break;

 	case TRAFFIC_STATE::BUFFER_FULL: {

-			if ((_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL)
-			    && (hrt_elapsed_time(&_last_buffer_full_warning_time) > TRAFFIC_WARNING_TIMESTEP)) {
-				events::send(events::ID("buffer_full"), events::Log::Notice, "Too much traffic! Showing all messages from now on");
-				_last_buffer_full_warning_time = hrt_absolute_time();
+			if (_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL) {
+				PX4_WARN("Too much traffic! Showing all messages from now on");
 			}

-			//disable conflict warnings when buffer is full
+			//stop buffering incoming conflicts
+			take_action = send_traffic_warning(math::degrees(_transponder_report.heading) + 180,
+							   (int)fabsf(_crosstrack_error.distance), _transponder_report.flags, uas_id,
+							   _transponder_report.callsign,
+							   uas_id_int);
+
 		}
 		break;

@@ -234,67 +242,49 @@ void AdsbConflict::set_conflict_detection_params(float crosstrack_separation, fl


 bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags,
-					char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address)
+					char uas_id[UTM_GUID_MSG_LENGTH], char tr_callsign[UTM_CALLSIGN_LENGTH], uint64_t uas_id_int)
 {

 	switch (_conflict_detection_params.traffic_avoidance_mode) {

 	case 0: {
-
-			if (tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) {
-
-				PX4_WARN("Traffic alert - UTM callsign %s! Separation Distance %d, Heading %d, ICAO Address %d",
-					 tr_callsign,
-					 traffic_seperation,
-					 traffic_direction, (int)icao_address);
-
-			}
-
-
-			_last_traffic_warning_time = hrt_absolute_time();
-
+			PX4_WARN("TRAFFIC %s! dst %d, hdg %d",
+				 tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
+				 traffic_seperation,
+				 traffic_direction);
 			break;
 		}

 	case 1: {
-
-
-			if (tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) {
-
-				PX4_WARN("Traffic alert - UTM callsign %s! Separation Distance %d, Heading %d, ICAO Address %d",
-					 tr_callsign,
-					 traffic_seperation,
-					 traffic_direction, (int)icao_address);
-
-			}
-
+			mavlink_log_critical(&_mavlink_log_pub, "Warning TRAFFIC %s! dst %d, hdg %d\t",
+					     tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
+					     traffic_seperation,
+					     traffic_direction);
 			/* EVENT
 			 * @description
-			 * - ICAO Address: {1}
-			 * - Traffic Separation Distance: {2m}
-			 * - Heading: {3} degrees
+			 * - ID: {1}
+			 * - Distance: {2m}
+			 * - Direction: {3} degrees
 			 */
-			events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic"), events::Log::Notice,
-					"Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}",
-					icao_address, traffic_seperation, traffic_direction);
-
-			_last_traffic_warning_time = hrt_absolute_time();
-
+			events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic"), events::Log::Critical, "Traffic alert",
+					uas_id_int, traffic_seperation, traffic_direction);
 			break;
 		}

 	case 2: {
+			mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Returning home! dst %d, hdg %d\t",
+					     tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
+					     traffic_seperation,
+					     traffic_direction);
 			/* EVENT
 			 * @description
-			 * - ICAO Address: {1}
-			 * - Traffic Separation Distance: {2m}
-			 * - Heading: {3} degrees
+			 * - ID: {1}
+			 * - Distance: {2m}
+			 * - Direction: {3} degrees
 			 */
-			events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic_rtl"), events::Log::Notice,
-					"Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, returning home",
-					icao_address, traffic_seperation, traffic_direction);
-
-			_last_traffic_warning_time = hrt_absolute_time();
+			events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_rtl"), events::Log::Critical,
+					"Traffic alert, returning home",
+					uas_id_int, traffic_seperation, traffic_direction);

 			return true;

@@ -302,17 +292,19 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper
 		}

 	case 3: {
+			mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Landing! dst %d, hdg % d\t",
+					     tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
+					     traffic_seperation,
+					     traffic_direction);
 			/* EVENT
 			 * @description
-			 * - ICAO Address: {1}
-			 * - Traffic Separation Distance: {2m}
-			 * - Heading: {3} degrees
+			 * - ID: {1}
+			 * - Distance: {2m}
+			 * - Direction: {3} degrees
 			 */
-			events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic_land"), events::Log::Notice,
-					"Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, landing",
-					icao_address, traffic_seperation, traffic_direction);
-
-			_last_traffic_warning_time = hrt_absolute_time();
+			events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_land"), events::Log::Critical,
+					"Traffic alert, landing",
+					uas_id_int, traffic_seperation, traffic_direction);

 			return true;

@@ -321,18 +313,19 @@ bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seper
 		}

 	case 4: {
+			mavlink_log_critical(&_mavlink_log_pub, "TRAFFIC: %s Holding position! dst %d, hdg %d\t",
+					     tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN ? tr_callsign : uas_id,
+					     traffic_seperation,
+					     traffic_direction);
 			/* EVENT
 			 * @description
-			 * - ICAO Address: {1}
-			 * - Traffic Separation Distance: {2m}
-			 * - Heading: {3} degrees
+			 * - ID: {1}
+			 * - Distance: {2m}
+			 * - Direction: {3} degrees
 			 */
-			events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic_hold"), events::Log::Notice,
-					"Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, holding position",
-					icao_address, traffic_seperation, traffic_direction);
-
-			_last_traffic_warning_time = hrt_absolute_time();
-
+			events::send<uint64_t, int32_t, int16_t>(events::ID("navigator_traffic_hold"), events::Log::Critical,
+					"Traffic alert, holding position",
+					uas_id_int, traffic_seperation, traffic_direction);

 			return true;

@@ -398,7 +391,8 @@ void AdsbConflict::fake_traffic(const char *callsign, float distance, float dire
 void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
 				    float &alt_uav)
 {
-	//Test with buffer size of 10
+
+	/*
 	//first conflict
 	fake_traffic("LX001", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav,
@@ -408,20 +402,19 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
 	fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
 		     alt_uav);
-	fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
+	fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
 		     alt_uav);
-	fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
+	fake_traffic("LX0002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
 		     alt_uav);

 	//stop spamming
-	//new conflicts
+
 	fake_traffic("LX003", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 3, lat_uav, lon_uav,
 		     alt_uav);

-
 	fake_traffic("LX004", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav,
 		     alt_uav);
@@ -452,9 +445,6 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
 		     alt_uav);

 	//buffer full
-
-	//buffer full conflicts
-
 	fake_traffic("LX011", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 11, lat_uav, lon_uav,
 		     alt_uav);
@@ -468,7 +458,7 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
 		     alt_uav);


-	//end conflicts
+	//end conflict
 	fake_traffic("LX001", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav,
 		     alt_uav);
@@ -484,29 +474,30 @@ void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav,
 		     alt_uav);

+
 	//new conflicts with space in buffer
 	fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
 		     alt_uav);
-	//spam
+
 	fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
 		     alt_uav);
-	//new conflict
+
 	fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav,
 		     alt_uav);
-	//spam
+
 	fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav,
 		     alt_uav);
-	//new conflict
+
 	fake_traffic("LX015", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
 		     transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 15, lat_uav, lon_uav,
 		     alt_uav);

-
 	for (size_t i = 0; i < _traffic_buffer.icao_address.size(); i++) {
 		PX4_INFO("%u ", static_cast<unsigned int>(_traffic_buffer.icao_address[i]));
 	}
+	*/
 }
@@ -46,6 +46,7 @@
 #include <uORB/topics/transponder_report.h>
 #include <uORB/topics/vehicle_command.h>

+#include <systemlib/mavlink_log.h>
 #include <px4_platform_common/events.h>

 #include <px4_platform_common/board_common.h>
@@ -56,15 +57,14 @@ using namespace time_literals;

 static constexpr uint8_t NAVIGATOR_MAX_TRAFFIC{10};

+static constexpr uint8_t UTM_GUID_MSG_LENGTH{11};
+
 static constexpr uint8_t UTM_CALLSIGN_LENGTH{9};

 static constexpr uint64_t CONFLICT_WARNING_TIMEOUT{60_s};

 static constexpr float TRAFFIC_TO_UAV_DISTANCE_EXTENSION{1000.0f};

-static constexpr uint64_t TRAFFIC_WARNING_TIMESTEP{60_s}; //limits the max warning rate when traffic conflict buffer is full
-
-static constexpr uint64_t TRAFFIC_CONFLICT_LIFETIME{120_s}; //limits the time a conflict can be in the buffer without being seen (as a conflict)

 struct traffic_data_s {
 	double lat_traffic;
@@ -118,7 +118,7 @@ public:


 	bool send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags,
-				  char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address);
+				  char uas_id[UTM_GUID_MSG_LENGTH], char tr_callsign[UTM_CALLSIGN_LENGTH], uint64_t uas_id_int);

 	transponder_report_s _transponder_report{};

@@ -132,8 +132,6 @@ public:
 	void run_fake_traffic(double &lat_uav, double &lon_uav,
 			      float &alt_uav);

-	void remove_expired_conflicts();
-
 	bool _conflict_detected{false};

 	TRAFFIC_STATE _traffic_state{TRAFFIC_STATE::NO_CONFLICT};
@@ -146,15 +144,15 @@ protected:

 private:

+	orb_advert_t _mavlink_log_pub{nullptr};
+
 	crosstrack_error_s _crosstrack_error{};

+
 	transponder_report_s tr{};

 	orb_advert_t fake_traffic_report_publisher = orb_advertise(ORB_ID(transponder_report), &tr);

 	TRAFFIC_STATE _traffic_state_previous{TRAFFIC_STATE::NO_CONFLICT};

-	hrt_abstime _last_traffic_warning_time{0};
-
-	hrt_abstime _last_buffer_full_warning_time{0};
 };
@@ -50,7 +50,7 @@ TEST_F(AdsbConflictTest, detectTrafficConflict)
 	for (uint32_t i = 0; i < traffic_dataset_size; i++) {


-		//printf("---------------%d--------------\n", i);
+		printf("---------------%d--------------\n", i);

 		struct traffic_data_s traffic = traffic_dataset[i];

@@ -64,10 +64,10 @@ TEST_F(AdsbConflictTest, detectTrafficConflict)

 		adsb_conflict.detect_traffic_conflict(lat_now, lon_now, alt_now, vx_now, vy_now, vz_now);

-		//printf("conflict_detected %d \n", adsb_conflict._conflict_detected);
+		printf("conflict_detected %d \n", adsb_conflict._conflict_detected);

-		//printf("------------------------------\n");
-		//printf("------------------------------\n \n");
+		printf("------------------------------\n");
+		printf("------------------------------\n \n");

 		EXPECT_TRUE(adsb_conflict._conflict_detected == traffic.in_conflict);
 	}
@@ -191,6 +191,7 @@ TEST_F(AdsbConflictTest, trafficAlerts)

 TEST_F(AdsbConflictTest, trafficReminder)
 {
+
 	struct	traffic_buffer_s used_buffer;
 	used_buffer.icao_address.push_back(2345);
 	used_buffer.icao_address.push_back(1234);
@@ -200,11 +201,11 @@ TEST_F(AdsbConflictTest, trafficReminder)
 	used_buffer.icao_address.push_back(5000);

 	used_buffer.timestamp.push_back(3_s);
-	used_buffer.timestamp.push_back(80_s);
-	used_buffer.timestamp.push_back(10_s);
-	used_buffer.timestamp.push_back(1000_s);
+	used_buffer.timestamp.push_back(800_s);
 	used_buffer.timestamp.push_back(100_s);
-	used_buffer.timestamp.push_back(187_s);
+	used_buffer.timestamp.push_back(20000_s);
+	used_buffer.timestamp.push_back(6000_s);
+	used_buffer.timestamp.push_back(6587_s);

 	struct	traffic_buffer_s full_buffer;
 	full_buffer.icao_address.push_back(2345);
@@ -219,15 +220,15 @@ TEST_F(AdsbConflictTest, trafficReminder)
 	full_buffer.icao_address.push_back(99999);

 	full_buffer.timestamp.push_back(1_s);
-	full_buffer.timestamp.push_back(80_s);
-	full_buffer.timestamp.push_back(10_s);
-	full_buffer.timestamp.push_back(1000_s);
+	full_buffer.timestamp.push_back(800_s);
 	full_buffer.timestamp.push_back(100_s);
-	full_buffer.timestamp.push_back(900_s);
-	full_buffer.timestamp.push_back(500_s);
+	full_buffer.timestamp.push_back(20000_s);
+	full_buffer.timestamp.push_back(6000_s);
+	full_buffer.timestamp.push_back(19000_s);
+	full_buffer.timestamp.push_back(5000_s);
 	full_buffer.timestamp.push_back(2_s);
-	full_buffer.timestamp.push_back(100_s);
-	full_buffer.timestamp.push_back(5843_s);
+	full_buffer.timestamp.push_back(1000_s);
+	full_buffer.timestamp.push_back(58943_s);


 	TestAdsbConflict 	adsb_conflict;
@@ -45,7 +45,7 @@ static uint16_t event_sequence{events::initial_event_sequence};
 namespace events
 {

-void send(event_s &event)
+void send(EventType &event)
 {
 	event.timestamp = hrt_absolute_time();

@@ -47,5 +47,5 @@

 #define EVENTSIOCSEND _EVENTSIOC(1)
 typedef struct eventiocsend {
-	event_s &event;
+	events::EventType &event;
 } eventiocsend_t;
@@ -44,7 +44,7 @@
 namespace events
 {

-void send(event_s &event)
+void send(EventType &event)
 {
 	eventiocsend_t data = {event};
 	boardctl(EVENTSIOCSEND, reinterpret_cast<unsigned long>(&data));
@@ -1109,7 +1109,7 @@ Commander::handle_command(const vehicle_command_s &cmd)

 	case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: {
 			// if no high latency telemetry exists send a failed acknowledge
-			if (_high_latency_datalink_timestamp < _boot_timestamp) {
+			if (_high_latency_datalink_heartbeat > _boot_timestamp) {
 				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
 				mavlink_log_critical(&_mavlink_log_pub, "Control high latency failed! Telemetry unavailable\t");
 				events::send(events::ID("commander_ctrl_high_latency_failed"), {events::Log::Critical, events::LogInternal::Info},
@@ -2634,7 +2634,7 @@ int Commander::task_spawn(int argc, char *argv[])
 	_task_id = px4_task_spawn_cmd("commander",
 				      SCHED_DEFAULT,
 				      SCHED_PRIORITY_DEFAULT + 40,
-				      PX4_STACK_ADJUSTED(3250),
+				      3250,
 				      (px4_main_t)&run_trampoline,
 				      (char *const *)argv);

@@ -2672,28 +2672,6 @@ void Commander::enable_hil()

 void Commander::dataLinkCheck()
 {
-	// high latency data link
-	iridiumsbd_status_s iridium_status;
-
-	if (_iridiumsbd_status_sub.update(&iridium_status)) {
-		_high_latency_datalink_timestamp = iridium_status.last_at_ok_timestamp;
-
-		if (_vehicle_status.high_latency_data_link_lost &&
-		    (_high_latency_datalink_timestamp > _high_latency_datalink_lost) &&
-		    (_high_latency_datalink_regained == 0)
-		   ) {
-			_high_latency_datalink_regained = _high_latency_datalink_timestamp;
-		}
-
-		if (_vehicle_status.high_latency_data_link_lost &&
-		    (_high_latency_datalink_regained != 0) &&
-		    (hrt_elapsed_time(&_high_latency_datalink_regained) > (_param_com_hldl_reg_t.get() * 1_s))
-		   ) {
-			_vehicle_status.high_latency_data_link_lost = false;
-			_status_changed = true;
-		}
-	}
-
 	for (auto &telemetry_status :  _telemetry_status_subs) {
 		telemetry_status_s telemetry;

@@ -2707,18 +2685,16 @@ void Commander::dataLinkCheck()
 				break;

 			case telemetry_status_s::LINK_TYPE_IRIDIUM: {
+					iridiumsbd_status_s iridium_status;

-					if ((_high_latency_datalink_timestamp > 0) &&
-					    (hrt_elapsed_time(&_high_latency_datalink_timestamp) > (_param_com_hldl_loss_t.get() * 1_s))) {
+					if (_iridiumsbd_status_sub.update(&iridium_status)) {
+						_high_latency_datalink_heartbeat = iridium_status.last_heartbeat;

-						_high_latency_datalink_lost = _high_latency_datalink_timestamp;
-						_high_latency_datalink_regained = 0;
-
-						if (!_vehicle_status.high_latency_data_link_lost) {
-							_vehicle_status.high_latency_data_link_lost = true;
-							mavlink_log_critical(&_mavlink_log_pub, "High latency data link lost\t");
-							events::send(events::ID("commander_high_latency_lost"), events::Log::Critical, "High latency data link lost");
-							_status_changed = true;
+						if (_vehicle_status.high_latency_data_link_lost) {
+							if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) {
+								_vehicle_status.high_latency_data_link_lost = false;
+								_status_changed = true;
+							}
 						}
 					}

@@ -2860,6 +2836,19 @@ void Commander::dataLinkCheck()
 			_vehicle_status.avoidance_system_valid = false;
 		}
 	}
+
+	// high latency data link loss failsafe
+	if (_high_latency_datalink_heartbeat > 0
+	    && hrt_elapsed_time(&_high_latency_datalink_heartbeat) > (_param_com_hldl_loss_t.get() * 1_s)) {
+		_high_latency_datalink_lost = hrt_absolute_time();
+
+		if (!_vehicle_status.high_latency_data_link_lost) {
+			_vehicle_status.high_latency_data_link_lost = true;
+			mavlink_log_critical(&_mavlink_log_pub, "High latency data link lost\t");
+			events::send(events::ID("commander_high_latency_lost"), events::Log::Critical, "High latency data link lost");
+			_status_changed = true;
+		}
+	}
 }

 void Commander::battery_status_check()
@@ -2927,7 +2916,11 @@ void Commander::manualControlCheck()
 				if (override_enabled) {
 					// If no failsafe is active, directly change the mode, otherwise pass the request to the failsafe state machine
 					if (_failsafe.selectedAction() <= FailsafeBase::Action::Warn) {
-						if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_POSCTL, ModeChangeSource::User, true)) {
+						// TODO: determine the mode to use
+						const int32_t rc_override_mode = getRcOverrideMode();
+						PX4_INFO("RC override to mode %d", rc_override_mode);
+
+						if (_user_mode_intention.change(rc_override_mode, ModeChangeSource::User, true)) {
 							tune_positive(true);
 							mavlink_log_info(&_mavlink_log_pub, "Pilot took over using sticks\t");
 							events::send(events::ID("commander_rc_override"), events::Log::Info, "Pilot took over using sticks");
@@ -2978,6 +2971,24 @@ void Commander::send_parachute_command()
 	set_tune_override(tune_control_s::TUNE_ID_PARACHUTE_RELEASE);
 }

+uint8_t Commander::getRcOverrideMode() const
+{
+	const int32_t rc_override_mode = _param_com_rc_ov_mode.get();
+
+	switch (rc_override_mode) {
+	case static_cast<int32_t>(RcOverrideModes::POSITION):
+		return vehicle_status_s::NAVIGATION_STATE_POSCTL;
+
+	case static_cast<int32_t>(RcOverrideModes::POSITION_SLOW):
+		return vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW;
+
+	default:
+		PX4_WARN("Unexpected value for COM_RC_OV_MODE: %d. Using Position mode.", rc_override_mode);
+	}
+
+	return vehicle_status_s::NAVIGATION_STATE_POSCTL;
+}
+
 int Commander::print_usage(const char *reason)
 {
 	if (reason) {
@@ -200,6 +200,8 @@ private:

 	void modeManagementUpdate();

+	uint8_t getRcOverrideMode() const;
+
 	enum class PrearmedMode {
 		DISABLED = 0,
 		SAFETY_BUTTON = 1,
@@ -211,6 +213,11 @@ private:
 		OFFBOARD_MODE_BIT = (1 << 1),
 	};

+	enum class RcOverrideModes : int32_t {
+		POSITION = 0,
+		POSITION_SLOW = 1
+	};
+
 	/* Decouple update interval and hysteresis counters, all depends on intervals */
 	static constexpr uint64_t COMMANDER_MONITORING_INTERVAL{10_ms};

@@ -246,9 +253,8 @@ private:

 	hrt_abstime _last_print_mode_reject_time{0};	///< To remember when last notification was sent

-	hrt_abstime _high_latency_datalink_timestamp{0};
+	hrt_abstime _high_latency_datalink_heartbeat{0};
 	hrt_abstime _high_latency_datalink_lost{0};
-	hrt_abstime _high_latency_datalink_regained{0};

 	hrt_abstime _boot_timestamp{0};
 	hrt_abstime _last_disarmed_timestamp{0};
@@ -347,6 +353,7 @@ private:
 		(ParamFloat<px4::params::COM_OBC_LOSS_T>)   _param_com_obc_loss_t,
 		(ParamInt<px4::params::COM_PREARM_MODE>)    _param_com_prearm_mode,
 		(ParamInt<px4::params::COM_RC_OVERRIDE>)    _param_com_rc_override,
+		(ParamInt<px4::params::COM_RC_OV_MODE>)     _param_com_rc_ov_mode,
 		(ParamInt<px4::params::COM_FLIGHT_UUID>)    _param_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>)    _param_takeoff_finished_action,
 		(ParamFloat<px4::params::COM_CPU_MAX>)      _param_com_cpu_max
@@ -284,7 +284,7 @@ bool Report::report(bool is_armed, bool force)

 	// send all events
 	int offset = 0;
-	event_s event;
+	events::EventType event;

 	for (int i = 0; i < max_num_events && offset < _next_buffer_idx; ++i) {
 		EventBufferHeader *header = (EventBufferHeader *)(_event_buffer + offset);
@@ -376,7 +376,7 @@ bool Report::addEvent(uint32_t event_id, const events::LogLevels &log_levels, co
 		      Args... args)
 {
 	constexpr unsigned args_size = events::util::sizeofArguments(modes, args...);
-	static_assert(args_size <= sizeof(event_s::arguments), "Too many arguments");
+	static_assert(args_size <= sizeof(events::EventType::arguments), "Too many arguments");
 	unsigned total_size = sizeof(EventBufferHeader) + args_size;

 	if (total_size > sizeof(_event_buffer) - _next_buffer_idx) {
@@ -99,12 +99,8 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
 		}
 	}

-	param_t param_ekf2_en_handle = param_find_no_notification("EKF2_EN");
 	int32_t param_ekf2_en = 0;
-
-	if (param_ekf2_en_handle != PARAM_INVALID) {
-		param_get(param_ekf2_en_handle, &param_ekf2_en);
-	}
+	param_get(param_find_no_notification("EKF2_EN"), &param_ekf2_en);

 	if (missing_data && (param_ekf2_en == 1)) {
 		/* EVENT
@@ -48,7 +48,13 @@ void ImuConsistencyChecks::checkAndReport(const Context &context, Report &report

 			const float accel_inconsistency_m_s_s = imu.accel_inconsistency_m_s_s[i];

+			NavModes required_groups = NavModes::None;
+
 			if (accel_inconsistency_m_s_s > _param_com_arm_imu_acc.get()) {
+				required_groups = NavModes::All;
+			}
+
+			if (accel_inconsistency_m_s_s > _param_com_arm_imu_acc.get() * 0.8f) {
 				/* EVENT
 				 * @description
 				 * Check the calibration.
@@ -60,7 +66,7 @@ void ImuConsistencyChecks::checkAndReport(const Context &context, Report &report
 				 * This check can be configured via <param>COM_ARM_IMU_ACC</param> parameter.
 				 * </profile>
 				 */
-				reporter.armingCheckFailure<uint8_t, float, float>(NavModes::All, health_component_t::accel,
+				reporter.armingCheckFailure<uint8_t, float, float>(required_groups, health_component_t::accel,
 						events::ID("check_imu_accel_inconsistent"),
 						events::Log::Warning, "Accel {1} inconsistent", i, accel_inconsistency_m_s_s, _param_com_arm_imu_acc.get());

@@ -79,7 +85,13 @@ void ImuConsistencyChecks::checkAndReport(const Context &context, Report &report

 			const float gyro_inconsistency_rad_s = imu.gyro_inconsistency_rad_s[i];

+			NavModes required_groups = NavModes::None;
+
 			if (gyro_inconsistency_rad_s > _param_com_arm_imu_gyr.get()) {
+				required_groups = NavModes::All;
+			}
+
+			if (gyro_inconsistency_rad_s > _param_com_arm_imu_gyr.get() * 0.5f) {
 				/* EVENT
 				 * @description
 				 * Check the calibration.
@@ -91,7 +103,7 @@ void ImuConsistencyChecks::checkAndReport(const Context &context, Report &report
 				 * This check can be configured via <param>COM_ARM_IMU_GYR</param> parameter.
 				 * </profile>
 				 */
-				reporter.armingCheckFailure<uint8_t, float, float>(NavModes::All, health_component_t::gyro,
+				reporter.armingCheckFailure<uint8_t, float, float>(required_groups, health_component_t::gyro,
 						events::ID("check_imu_gyro_inconsistent"),
 						events::Log::Warning, "Gyro {1} inconsistent", i, gyro_inconsistency_rad_s, _param_com_arm_imu_gyr.get());

@@ -58,7 +58,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 		 * @description
 		 * Wait until the estimator initialized
 		 */
-		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_attitude, health_component_t::attitude_estimate,
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_attitude, health_component_t::system,
 					    events::ID("check_modes_attitude"),
 					    events::Log::Critical, "No valid attitude estimate");
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_attitude);
@@ -78,7 +78,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 	if (local_position_modes != NavModes::None) {
 		/* EVENT
 		 */
-		reporter.armingCheckFailure(local_position_modes, health_component_t::local_position_estimate,
+		reporter.armingCheckFailure(local_position_modes, health_component_t::system,
 					    events::ID("check_modes_local_pos"),
 					    events::Log::Error, "No valid local position estimate");
 		reporter.clearCanRunBits(local_position_modes);
@@ -87,8 +87,7 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
 	if (reporter.failsafeFlags().global_position_invalid && reporter.failsafeFlags().mode_req_global_position != 0) {
 		/* EVENT
 		 */
-		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position,
-					    health_component_t::global_position_estimate,
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position, health_component_t::system,
 					    events::ID("check_modes_global_pos"),
 					    events::Log::Error, "No valid global position estimate");
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position);
@@ -483,6 +483,18 @@ PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1);
 */
 PARAM_DEFINE_FLOAT(COM_RC_STICK_OV, 30.0f);

+/**
+ * RC stick override mode
+ *
+ * When an RC override occurs (see COM_RC_OVERRIDE), switch to the selected mode, if possible.
+ *
+ * @value 0 Position
+ * @value 1 Position Slow
+ *
+ * @group Commander
+ */
+PARAM_DEFINE_INT32(COM_RC_OV_MODE, 0);
+
 /**
 * Require valid mission to arm
 *
@@ -485,6 +485,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 	if (actionAllowsUserTakeover(selected_action) && takeover_allowed) {
 		if (!_user_takeover_active && rc_sticks_takeover_request) {
 			// TODO: if the user intended mode is a stick-controlled mode, switch back to that instead
+			PX4_WARN("Setting POSCTL");
+			// TODO: get configured mode here
 			returned_state.updated_user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL;
 		}

@@ -493,7 +495,7 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s
 		returned_state.delayed_action = Action::None;

 		if (!_user_takeover_active) {
-			PX4_DEBUG("Activating user takeover");
+			PX4_WARN("Activating user takeover");
 #ifndef EMSCRIPTEN_BUILD
 			events::send(events::ID("failsafe_rc_override"), events::Log::Info, "Pilot took over using sticks");
 #endif // EMSCRIPTEN_BUILD
@@ -672,6 +672,7 @@ private:

 # if defined(CONFIG_EKF2_GNSS_YAW)
 	estimator_aid_source1d_s _aid_src_gnss_yaw{};
+	uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source
 # endif // CONFIG_EKF2_GNSS_YAW
 #endif // CONFIG_EKF2_GNSS

@@ -358,6 +358,7 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
 				&& !_gps_intermittent;

 		if (_control_status.flags.gps_yaw) {
+
 			if (continuing_conditions_passing) {

 				fuseGpsYaw(gps_sample.yaw_offset);
@@ -365,14 +366,27 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
 				const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);

 				if (is_fusion_failing) {
-					stopGpsYawFusion();
+					if (_nb_gps_yaw_reset_available > 0) {
+						// Data seems good, attempt a reset
+						resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset);

-					// Before takeoff, we do not want to continue to rely on the current heading
-					// if we had to stop the fusion
-					if (!_control_status.flags.in_air) {
-						ECL_INFO("clearing yaw alignment");
-						_control_status.flags.yaw_align = false;
+						if (_control_status.flags.in_air) {
+							_nb_gps_yaw_reset_available--;
+						}
+
+					} else if (starting_conditions_passing) {
+						// Data seems good, but previous reset did not fix the issue
+						// something else must be wrong, declare the sensor faulty and stop the fusion
+						_control_status.flags.gps_yaw_fault = true;
+						stopGpsYawFusion();
+
+					} else {
+						// A reset did not fix the issue but all the starting checks are not passing
+						// This could be a temporary issue, stop the fusion without declaring the sensor faulty
+						stopGpsYawFusion();
 					}
+
+					// TODO: should we give a new reset credit when the fusion does not fail for some time?
 				}

 			} else {
@@ -383,27 +397,14 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
 		} else {
 			if (starting_conditions_passing) {
 				// Try to activate GPS yaw fusion
-				const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;
-
-				if (!_control_status.flags.in_air
-				    || !_control_status.flags.yaw_align
-				    || not_using_ne_aiding) {
-
-					// Reset before starting the fusion
-					if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) {
-						_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
-						_control_status.flags.gps_yaw = true;
-						_control_status.flags.yaw_align = true;
-					}
-
-				} else if (!_aid_src_gnss_yaw.innovation_rejected) {
-					// Do not force a reset but wait for the consistency check to pass
-					_control_status.flags.gps_yaw = true;
-					fuseGpsYaw(gps_sample.yaw_offset);
-				}
-
-				if (_control_status.flags.gps_yaw) {
+				if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) {
 					ECL_INFO("starting GPS yaw fusion");
+
+					_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
+					_control_status.flags.gps_yaw = true;
+					_control_status.flags.yaw_align = true;
+
+					_nb_gps_yaw_reset_available = 1;
 				}
 			}
 		}
@@ -423,7 +424,15 @@ void Ekf::stopGpsYawFusion()
 		_control_status.flags.gps_yaw = false;
 		resetEstimatorAidStatus(_aid_src_gnss_yaw);

-		ECL_INFO("stopping GPS yaw fusion");
+		// Before takeoff, we do not want to continue to rely on the current heading
+		// if we had to stop the fusion
+		if (!_control_status.flags.in_air) {
+			ECL_INFO("stopping GPS yaw fusion, clearing yaw alignment");
+			_control_status.flags.yaw_align = false;
+
+		} else {
+			ECL_INFO("stopping GPS yaw fusion");
+		}
 	}
 }
 #endif // CONFIG_EKF2_GNSS_YAW
@@ -150,7 +150,7 @@ def baroCorrection(x, v_body):

    return correction

-def run(logfile, w_hpf):
+def run(logfile):
    (t, v_body, baro, v_local_z, gnss_h) = getAllData(logfile)

    # x[0]: pcoef_xn / g
@@ -162,10 +162,8 @@ def run(logfile, w_hpf):

    # Remove low ferquency part of the signal as we're only interested in the short-term errors
    baro_error -= baro_error[0]
-
-    if (w_hpf > 0):
-        sos = butter(4, w_hpf, 'hp', fs=1/(t[1]-t[0]), output='sos')
-        baro_error = sosfilt(sos, baro_error)
+    sos = butter(4, 0.01, 'hp', fs=1/(t[1]-t[0]), output='sos')
+    baro_error = sosfilt(sos, baro_error)

    J = lambda x: np.sum(np.power(baro_error - baroCorrection(x, v_body), 2.0)) # cost function

@@ -226,10 +224,8 @@ if __name__ == '__main__':

    # Provide parameter file path and name
    parser.add_argument('logfile', help='Full ulog file path, name and extension', type=str)
-    parser.add_argument('--hpf', help='Cuttoff frequency of high-pass filter on baro error (Hz)', type=float)
    args = parser.parse_args()

    logfile = os.path.abspath(args.logfile) # Convert to absolute path
-    w_hpf = 2 * np.pi * args.hpf

-    run(logfile, w_hpf)
+    run(logfile)
@@ -114,7 +114,8 @@ parameters:
      decimal: 1
    EKF2_AVEL_DELAY:
      description:
-        short: Auxiliary Velocity Estimate delay relative to IMU measurements
+        short: Auxiliary Velocity Estimate (e.g from a landing target) delay relative
+          to IMU measurements
      type: float
      default: 5
      min: 0
@@ -581,8 +582,7 @@ parameters:
      max: 3
    EKF2_NOAID_TOUT:
      description:
-        short: Maximum inertial dead-reckoning time
-        long: Maximum lapsed time from last fusion of measurements that constrain
+        short: Maximum lapsed time from last fusion of measurements that constrain
          velocity drift before the EKF will report the horizontal nav solution as
          invalid
      type: int32
@@ -655,8 +655,8 @@ parameters:
      decimal: 1
    EKF2_EVP_NOISE:
      description:
-        short: Measurement noise for vision position measurements
-        long:  Used to lower bound or replace the uncertainty included in the message
+        short: Measurement noise for vision position observations used to lower bound
+          or replace the uncertainty included in the message
      type: float
      default: 0.1
      min: 0.01
@@ -664,8 +664,8 @@ parameters:
      decimal: 2
    EKF2_EVV_NOISE:
      description:
-        short: Measurement noise for vision velocity measurements
-        long:  Used to lower bound or replace the uncertainty included in the message
+        short: Measurement noise for vision velocity observations used to lower bound
+          or replace the uncertainty included in the message
      type: float
      default: 0.1
      min: 0.01
@@ -673,8 +673,8 @@ parameters:
      decimal: 2
    EKF2_EVA_NOISE:
      description:
-        short: Measurement noise for vision angle measurements
-        long:  Used to lower bound or replace the uncertainty included in the message
+        short: Measurement noise for vision angle observations used to lower bound
+          or replace the uncertainty included in the message
      type: float
      default: 0.1
      min: 0.05
@@ -697,8 +697,7 @@ parameters:
      default: 0
    EKF2_OF_N_MIN:
      description:
-        short: Optical flow minimum noise
-        long: Measurement noise for the optical flow sensor when it's reported quality
+        short: Measurement noise for the optical flow sensor when it's reported quality
          metric is at the maximum
      type: float
      default: 0.15
@@ -707,9 +706,9 @@ parameters:
      decimal: 2
    EKF2_OF_N_MAX:
      description:
-        short: Optical flow maximum noise
-        long: Measurement noise for the optical flow sensor when it's reported quality
-          metric is at the minimum
+        short: Measurement noise for the optical flow sensor
+        long: '(when it''s reported quality metric is at the minimum set by EKF2_OF_QMIN).
+          The following condition must be met: EKF2_OF_N_MAXN >= EKF2_OF_N_MIN'
      type: float
      default: 0.5
      min: 0.05
@@ -717,8 +716,7 @@ parameters:
      decimal: 2
    EKF2_OF_QMIN:
      description:
-        short: In air optical flow minimum quality
-        long: Optical Flow data will only be used in air if the sensor reports a
+        short: Optical Flow data will only be used in air if the sensor reports a
          quality metric >= EKF2_OF_QMIN
      type: int32
      default: 1
@@ -726,8 +724,7 @@ parameters:
      max: 255
    EKF2_OF_QMIN_GND:
      description:
-        short: On ground optical flow minimum quality
-        long: Optical Flow data will only be used on the ground if the sensor reports
+        short: Optical Flow data will only be used on the ground if the sensor reports
          a quality metric >= EKF2_OF_QMIN_GND
      type: int32
      default: 0
@@ -745,7 +742,8 @@ parameters:
      decimal: 1
    EKF2_TERR_NOISE:
      description:
-        short: Terrain altitude process noise
+        short: Terrain altitude process noise - accounts for instability in vehicle
+          height estimate
      type: float
      default: 5.0
      min: 0.5
@@ -761,120 +759,120 @@ parameters:
      decimal: 2
    EKF2_IMU_POS_X:
      description:
-        short: X position of IMU in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: X position of IMU in body frame (forward axis with origin relative
+          to vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_IMU_POS_Y:
      description:
-        short: Y position of IMU in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: Y position of IMU in body frame (right axis with origin relative to
+          vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_IMU_POS_Z:
      description:
-        short: Z position of IMU in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: Z position of IMU in body frame (down axis with origin relative to
+          vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_GPS_POS_X:
      description:
-        short: X position of GPS antenna in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: X position of GPS antenna in body frame (forward axis with origin relative
+          to vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_GPS_POS_Y:
      description:
-        short: Y position of GPS antenna in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: Y position of GPS antenna in body frame (right axis with origin relative
+          to vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_GPS_POS_Z:
      description:
-        short: Z position of GPS antenna in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: Z position of GPS antenna in body frame (down axis with origin relative
+          to vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_RNG_POS_X:
      description:
-        short: X position of range finder origin in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: X position of range finder origin in body frame (forward axis with
+          origin relative to vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_RNG_POS_Y:
      description:
-        short: Y position of range finder origin in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: Y position of range finder origin in body frame (right axis with origin
+          relative to vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_RNG_POS_Z:
      description:
-        short: Z position of range finder origin in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: Z position of range finder origin in body frame (down axis with origin
+          relative to vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_OF_POS_X:
      description:
-        short: X position of optical flow focal point in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: X position of optical flow focal point in body frame (forward axis
+          with origin relative to vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_OF_POS_Y:
      description:
-        short: Y position of optical flow focal point in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: Y position of optical flow focal point in body frame (right axis with
+          origin relative to vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_OF_POS_Z:
      description:
-        short: Z position of optical flow focal point in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: Z position of optical flow focal point in body frame (down axis with
+          origin relative to vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_EV_POS_X:
      description:
-        short: X position of VI sensor focal point in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: X position of VI sensor focal point in body frame (forward axis with
+          origin relative to vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_EV_POS_Y:
      description:
-        short: Y position of VI sensor focal point in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: Y position of VI sensor focal point in body frame (right axis with
+          origin relative to vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
      decimal: 3
    EKF2_EV_POS_Z:
      description:
-        short: Z position of VI sensor focal point in body frame
-        long: Forward axis with origin relative to vehicle centre of gravity
+        short: Z position of VI sensor focal point in body frame (down axis with origin
+          relative to vehicle centre of gravity)
      type: float
      default: 0.0
      unit: m
@@ -910,8 +908,8 @@ parameters:
      decimal: 2
    EKF2_TAU_POS:
      description:
-        short: Output predictor position time constant
-        long: Controls how tightly the output track the EKF states
+        short: Time constant of the position output prediction and smoothing filter.
+          Controls how tightly the output track the EKF states
      type: float
      default: 0.25
      min: 0.1
@@ -970,7 +968,8 @@ parameters:
      unit: m/s
    EKF2_RNG_A_HMAX:
      description:
-        short: Maximum height above ground allowed for conditional range aid mode
+        short: Maximum absolute altitude (height above ground level) allowed for conditional
+          range aid mode
        long: If the vehicle absolute altitude exceeds this value then the estimator
          will not fuse range measurements to estimate its height. This only applies
          when conditional range aid mode is activated (EKF2_RNG_CTRL = 1).
@@ -991,8 +990,7 @@ parameters:
      max: 5.0
    EKF2_RNG_QLTY_T:
      description:
-        short: Minumum range validity period
-        long: Minimum duration during which the reported range finder signal quality
+        short: Minimum duration during which the reported range finder signal quality
          needs to be non-zero in order to be declared valid (s)
      type: float
      default: 1.0
@@ -1043,9 +1041,9 @@ parameters:
      default: 0
    EKF2_DRAG_NOISE:
      description:
-        short: Specific drag force observation noise variance
-        long: Used by the multi-rotor specific drag force model.
-          Increasing this makes the multi-rotor wind estimates adjust more slowly.
+        short: Specific drag force observation noise variance used by the multi-rotor
+          specific drag force model
+        long: Increasing this makes the multi-rotor wind estimates adjust more slowly.
      type: float
      default: 2.5
      min: 0.5
@@ -1084,7 +1082,7 @@ parameters:
      decimal: 1
    EKF2_MCOEF:
      description:
-        short: Propeller momentum drag coefficient for multi-rotor wind estimation
+        short: Propeller momentum drag coefficient used for multi-rotor wind estimation
        long: This parameter controls the prediction of drag produced by the propellers
          when flying a multi-copter, which enables estimation of wind drift when
          enabled by the EKF2_DRAG_CTRL parameter. The drag produced by this effect
@@ -1106,7 +1104,8 @@ parameters:
      decimal: 2
    EKF2_ASPD_MAX:
      description:
-        short: Maximum airspeed used for baro static pressure compensation
+        short: Upper limit on airspeed along individual axes used to correct baro
+          for position error effects
      type: float
      default: 20.0
      min: 5.0
@@ -1210,7 +1209,8 @@ parameters:
      decimal: 1
    EKF2_ABL_TAU:
      description:
-        short: Accel bias learning inhibit time constant
+        short: Time constant used by acceleration and angular rate magnitude checks
+          used to inhibit accel bias learning
        long: The vector magnitude of angular rate and acceleration used to check
          if learning should be inhibited has a peak hold filter applied to it with
          an exponential decay. This parameter controls the time constant of the decay.
@@ -1333,8 +1333,8 @@ parameters:
      decimal: 1
    EKF2_AGP_NOISE:
      description:
-        short: Measurement noise for aux global position measurements
-        long:  Used to lower bound or replace the uncertainty included in the message
+        short: Measurement noise for aux global position observations used to lower
+          bound or replace the uncertainty included in the message
      type: float
      default: 0.9
      min: 0.01
@@ -204,7 +204,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag)
 	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
 	EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());

-	//const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();
+	const int initial_quat_reset_counter = _ekf_wrapper.getQuaternionResetCounter();

 	// BUT WHEN: the GPS yaw is suddenly invalid
 	gps_heading = NAN;
@@ -215,7 +215,7 @@ TEST_F(EkfGpsHeadingTest, fallBackToMag)
 	// the estimator should fall back to mag fusion
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
-	//EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
 }

 TEST_F(EkfGpsHeadingTest, fallBackToYawEmergencyEstimator)
@@ -275,15 +275,9 @@ TEST_F(EkfGpsHeadingTest, yawJmpOnGround)
 	_sensor_simulator._gps.setYaw(gps_heading);
 	_sensor_simulator.runSeconds(8);

-	// THEN: the fusion should stop, reset to mag
-	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
-	EXPECT_TRUE(_ekf_wrapper.isIntendingMagHeadingFusion());
-	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
-
-	// AND THEN: restart GNSS yaw fusion
-	_sensor_simulator.runSeconds(5);
+	// THEN: the fusion should reset
 	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
-	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2);
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
 	EXPECT_LT(fabsf(matrix::wrap_pi(_ekf_wrapper.getYawAngle() - gps_heading)), math::radians(1.f));
 }

@@ -291,7 +285,7 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir)
 {
 	// GIVEN: the GPS yaw fusion activated
 	float gps_heading = _ekf_wrapper.getYawAngle();
-	_sensor_simulator._gps.setYaw(gps_heading + math::radians(90.f));
+	_sensor_simulator._gps.setYaw(gps_heading);
 	_sensor_simulator.runSeconds(5);
 	_ekf->set_in_air_status(true);

@@ -301,14 +295,20 @@ TEST_F(EkfGpsHeadingTest, yawJumpInAir)
 	_sensor_simulator._gps.setYaw(gps_heading);
 	_sensor_simulator.runSeconds(7.5);

-	// THEN: the fusion should not reset as heading is still observable through GNSS vel/pos fusion
-	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter);
+	// THEN: the fusion should reset
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 1);
+
+	// BUT WHEN: the measurement jumps a 2nd time
+	gps_heading = matrix::wrap_pi(_ekf_wrapper.getYawAngle() + math::radians(180.f));
+	_sensor_simulator._gps.setYaw(gps_heading);
+	_sensor_simulator.runSeconds(7.5);

 	// THEN: after a few seconds, the fusion should stop and
 	// the estimator doesn't fall back to mag fusion because it has
 	// been declared inconsistent with the filter states
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
 	EXPECT_FALSE(_ekf_wrapper.isMagHeadingConsistent());
+	//TODO: should we force a reset to mag if the GNSS yaw fusion was forced to stop?
 	EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
 }

@@ -328,7 +328,7 @@ TEST_F(EkfGpsHeadingTest, stopOnGround)
 	// THEN: the fusion should stop and the GPS pos/vel aiding
 	// should stop as well because the yaw is not aligned anymore
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeadingFusion());
-	//EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
+	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());

 	// AND IF: the mag fusion type is set to NONE
 	_ekf_wrapper.setMagFuseTypeNone();
@@ -170,8 +170,7 @@ bool FlightTaskAuto::update()
 		waypoints[2] = _position_setpoint;
 	}

-	const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == int32_t(yaw_mode::towards_waypoint_yaw_first)
-					       && !_yaw_sp_aligned;
+	const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned;
 	const bool force_zero_velocity_setpoint = should_wait_for_yaw_align || _is_emergency_braking_active;
 	_updateTrajConstraints();
 	PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints;
@@ -533,37 +532,32 @@ void FlightTaskAuto::_set_heading_from_mode()

 	Vector2f v; // Vector that points towards desired location

-	switch (yaw_mode(_param_mpc_yaw_mode.get())) {
+	switch (_param_mpc_yaw_mode.get()) {

-	case yaw_mode::towards_waypoint: // Heading points towards the current waypoint.
-	case yaw_mode::towards_waypoint_yaw_first: // Same as 0 but yaw first and then go
+	case 0: // Heading points towards the current waypoint.
+	case 4: // Same as 0 but yaw first and then go
 		v = Vector2f(_target) - Vector2f(_position);
 		break;

-	case yaw_mode::towards_home: // Heading points towards home.
+	case 1: // Heading points towards home.
 		if (_sub_home_position.get().valid_lpos) {
 			v = Vector2f(&_sub_home_position.get().x) - Vector2f(_position);
 		}

 		break;

-	case yaw_mode::away_from_home: // Heading point away from home.
+	case 2: // Heading point away from home.
 		if (_sub_home_position.get().valid_lpos) {
 			v = Vector2f(_position) - Vector2f(&_sub_home_position.get().x);
 		}

 		break;

-	case yaw_mode::along_trajectory: // Along trajectory.
+	case 3: // Along trajectory.
 		// The heading depends on the kind of setpoint generation. This needs to be implemented
 		// in the subclasses where the velocity setpoints are generated.
 		v.setAll(NAN);
 		break;
-
-	case yaw_mode::yaw_fixed: // Yaw fixed.
-		// Yaw is operated via manual control or MAVLINK messages.
-		break;
-
 	}

 	if (v.isAllFinite()) {
@@ -81,15 +81,6 @@ enum class State {
 	none /**< Vehicle is in normal tracking mode from triplet previous to triplet target */
 };

-enum class yaw_mode : int32_t {
-	towards_waypoint = 0,
-	towards_home = 1,
-	away_from_home = 2,
-	along_trajectory = 3,
-	towards_waypoint_yaw_first = 4,
-	yaw_fixed = 5,
-};
-
 class FlightTaskAuto : public FlightTask
 {
 public:
@@ -1404,9 +1404,6 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const
 				   tecs_fw_thr_max,
 				   _param_sinkrate_target.get(),
 				   _param_climbrate_target.get());
-
-	_att_sp.thrust_body[0] = min(get_tecs_thrust(), tecs_fw_thr_max);
-	_att_sp.pitch_body = get_tecs_pitch();
 }

 void
@@ -2304,7 +2301,7 @@ FixedwingPositionControl::Run()
 			_reference_altitude = 0.f;
 		}

-		_current_altitude = -_local_pos.z + _reference_altitude; // Altitude AMSL in meters
+		_current_altitude = -_local_pos.z + _local_pos.ref_alt; // Altitude AMSL in meters

 		// handle estimator reset events. we only adjust setpoins for manual modes
 		if (_control_mode.flag_control_manual_enabled) {
@@ -208,18 +208,17 @@ void SendProtocol::handle_request_event(const mavlink_message_t &msg) const

 void SendProtocol::send_event(const Event &event) const
 {
-	if (_mavlink.get_mode() != Mavlink::MAVLINK_MODE_IRIDIUM) {
-		mavlink_event_t event_msg{};
-		event_msg.event_time_boot_ms = event.timestamp_ms;
-		event_msg.destination_component = MAV_COMP_ID_ALL;
-		event_msg.destination_system = 0;
-		event_msg.id = event.id;
-		event_msg.sequence = event.sequence;
-		event_msg.log_levels = event.log_levels;
-		static_assert(sizeof(event_msg.arguments) >= sizeof(event.arguments), "MAVLink message arguments buffer too small");
-		memcpy(&event_msg.arguments, event.arguments, sizeof(event.arguments));
-		mavlink_msg_event_send_struct(_mavlink.get_channel(), &event_msg);
-	}
+	mavlink_event_t event_msg{};
+	event_msg.event_time_boot_ms = event.timestamp_ms;
+	event_msg.destination_component = MAV_COMP_ID_ALL;
+	event_msg.destination_system = 0;
+	event_msg.id = event.id;
+	event_msg.sequence = event.sequence;
+	event_msg.log_levels = event.log_levels;
+	static_assert(sizeof(event_msg.arguments) >= sizeof(event.arguments), "MAVLink message arguments buffer too small");
+	memcpy(&event_msg.arguments, event.arguments, sizeof(event.arguments));
+	mavlink_msg_event_send_struct(_mavlink.get_channel(), &event_msg);
+
 }

 void SendProtocol::on_gcs_connected()
@@ -229,9 +228,8 @@ void SendProtocol::on_gcs_connected()

 void SendProtocol::send_current_sequence(const hrt_abstime &now, bool force_reset)
 {
-	// only send if enough tx buffer space available or not MAVLINK_MODE_IRIDIUM
-	if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES ||
-	    _mavlink.get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
+	// only send if enough tx buffer space available
+	if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
 		return;
 	}

@@ -1057,9 +1057,10 @@ Mavlink::send_statustext_emergency(const char *string)
 bool
 Mavlink::send_autopilot_capabilities()
 {
+	uORB::Subscription status_sub{ORB_ID(vehicle_status)};
 	vehicle_status_s status;

-	if (_vehicle_status_sub.copy(&status)) {
+	if (status_sub.copy(&status)) {
 		mavlink_autopilot_version_t msg{};

 		msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT;
@@ -1439,6 +1440,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
 		configure_stream_local("SYS_STATUS", 1.0f);
 		configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f);
+		configure_stream_local("UTM_GLOBAL_POSITION", 0.5f);
 		configure_stream_local("VFR_HUD", 4.0f);
 		configure_stream_local("VIBRATION", 0.1f);
 		configure_stream_local("WIND_COV", 0.5f);
@@ -1508,6 +1510,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("SYSTEM_TIME", 1.0f);
 		configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f);
 		configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
+		configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
 		configure_stream_local("VFR_HUD", 10.0f);
 		configure_stream_local("VIBRATION", 0.5f);
 		configure_stream_local("WIND_COV", 10.0f);
@@ -1573,6 +1576,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
 		configure_stream_local("SYS_STATUS", 5.0f);
 		configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
+		configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
 		configure_stream_local("VFR_HUD", 4.0f);
 		configure_stream_local("VIBRATION", 0.5f);
 		configure_stream_local("WIND_COV", 1.0f);
@@ -1671,6 +1675,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("SYS_STATUS", 1.0f);
 		configure_stream_local("SYSTEM_TIME", 1.0f);
 		configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f);
+		configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
 		configure_stream_local("VFR_HUD", 20.0f);
 		configure_stream_local("VIBRATION", 2.5f);
 		configure_stream_local("WIND_COV", 10.0f);
@@ -1689,7 +1694,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		break;

 	case MAVLINK_MODE_IRIDIUM:
-		configure_stream_local("HIGH_LATENCY2", _high_latency_freq);
+		configure_stream_local("HIGH_LATENCY2", 0.015f);
 		break;

 	case MAVLINK_MODE_MINIMAL:
@@ -1755,6 +1760,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("SYSTEM_TIME", 2.0f);
 		configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f);
 		configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
+		configure_stream_local("UTM_GLOBAL_POSITION", 1.0f);
 		configure_stream_local("VFR_HUD", 4.0f);
 		configure_stream_local("VIBRATION", 0.5f);
 		configure_stream_local("WIND_COV", 1.0f);
@@ -1843,7 +1849,7 @@ Mavlink::task_main(int argc, char *argv[])
 	int temp_int_arg;
 #endif

-	while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:F:fswxzZp", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fswxzZp", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'b':
 			if (px4_get_parameter_value(myoptarg, _baudrate) != 0) {
@@ -2031,26 +2037,6 @@ Mavlink::task_main(int argc, char *argv[])
 				break;
 			}

-		case 'F': {
-				float freq;
-
-				if (px4_get_parameter_value(myoptarg, freq) != 0) {
-					PX4_ERR("iridium mode frequency parsing failed");
-					err_flag = true;
-
-				} else {
-					if (freq >= 0.f) {
-						_high_latency_freq = freq;
-
-					} else {
-						PX4_ERR("Invalid value for iridium mode frequency.");
-						err_flag = true;
-					}
-				}
-
-				break;
-			}
-
 		case 'f':
 			_forwarding_on = true;
 			break;
@@ -2255,18 +2241,12 @@ Mavlink::task_main(int argc, char *argv[])

 	_task_running.store(true);

-	bool cmd_logging_start_acknowledgement = false;
-	bool cmd_logging_stop_acknowledgement = false;
-
 	while (!should_exit()) {
 		/* main loop */
 		px4_usleep(_main_loop_delay);

 		if (!should_transmit()) {
 			check_requested_subscriptions();
-			handleStatus();
-			handleCommands();
-			handleAndGetCurrentCommandAck(cmd_logging_start_acknowledgement, cmd_logging_stop_acknowledgement);
 			continue;
 		}

@@ -2289,9 +2269,157 @@ Mavlink::task_main(int argc, char *argv[])

 		configure_sik_radio();

-		handleStatus();
-		handleCommands();
-		handleAndGetCurrentCommandAck(cmd_logging_start_acknowledgement, cmd_logging_stop_acknowledgement);
+		if (_vehicle_status_sub.updated()) {
+			vehicle_status_s vehicle_status;
+
+			if (_vehicle_status_sub.copy(&vehicle_status)) {
+				/* switch HIL mode if required */
+				set_hil_enabled(vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON);
+
+				if (_mode == MAVLINK_MODE_IRIDIUM) {
+
+					if (_transmitting_enabled && vehicle_status.high_latency_data_link_lost &&
+					    !_transmitting_enabled_commanded && _first_heartbeat_sent) {
+
+						_transmitting_enabled = false;
+						mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s\t", _device_name);
+						events::send<int8_t>(events::ID("mavlink_iridium_disable"), events::Log::Info,
+								     "Disabling transmitting with IRIDIUM mavlink on instance {1}", _instance_id);
+
+					} else if (!_transmitting_enabled && !vehicle_status.high_latency_data_link_lost) {
+						_transmitting_enabled = true;
+						mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s\t", _device_name);
+						events::send<int8_t>(events::ID("mavlink_iridium_enable"), events::Log::Info,
+								     "Enabling transmitting with IRIDIUM mavlink on instance {1}", _instance_id);
+					}
+				}
+			}
+		}
+
+
+		// MAVLINK_MODE_IRIDIUM: handle VEHICLE_CMD_CONTROL_HIGH_LATENCY
+		if (_mode == MAVLINK_MODE_IRIDIUM) {
+			int vehicle_command_updates = 0;
+
+			while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) {
+				vehicle_command_updates++;
+				const unsigned last_generation = _vehicle_command_sub.get_last_generation();
+				vehicle_command_s vehicle_cmd;
+
+				if (_vehicle_command_sub.update(&vehicle_cmd)) {
+					if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
+						PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation());
+					}
+
+					if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
+					    _mode == MAVLINK_MODE_IRIDIUM) {
+
+						if (vehicle_cmd.param1 > 0.5f) {
+							if (!_transmitting_enabled) {
+								mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command\t",
+										 _device_name);
+								events::send<int8_t>(events::ID("mavlink_iridium_enable_cmd"), events::Log::Info,
+										     "Enabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id);
+							}
+
+							_transmitting_enabled = true;
+							_transmitting_enabled_commanded = true;
+
+						} else {
+							if (_transmitting_enabled) {
+								mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command\t",
+										 _device_name);
+								events::send<int8_t>(events::ID("mavlink_iridium_disable_cmd"), events::Log::Info,
+										     "Disabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id);
+							}
+
+							_transmitting_enabled = false;
+							_transmitting_enabled_commanded = false;
+						}
+
+						// send positive command ack
+						vehicle_command_ack_s command_ack{};
+						command_ack.command = vehicle_cmd.command;
+						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+						command_ack.from_external = !vehicle_cmd.from_external;
+						command_ack.target_system = vehicle_cmd.source_system;
+						command_ack.target_component = vehicle_cmd.source_component;
+						command_ack.timestamp = vehicle_cmd.timestamp;
+						_vehicle_command_ack_pub.publish(command_ack);
+					}
+				}
+			}
+		}
+
+		/* send command ACK */
+		bool cmd_logging_start_acknowledgement = false;
+		bool cmd_logging_stop_acknowledgement = false;
+
+		if (_vehicle_command_ack_sub.updated()) {
+			static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+
+			while ((get_free_tx_buf() >= COMMAND_ACK_TOTAL_LEN) && _vehicle_command_ack_sub.updated()) {
+				vehicle_command_ack_s command_ack;
+				const unsigned last_generation = _vehicle_command_ack_sub.get_last_generation();
+
+				if (_vehicle_command_ack_sub.update(&command_ack)) {
+					if (_vehicle_command_ack_sub.get_last_generation() != last_generation + 1) {
+						PX4_ERR("vehicle_command_ack lost, generation %u -> %u", last_generation,
+							_vehicle_command_ack_sub.get_last_generation());
+					}
+
+					const bool is_target_known = _receiver.component_was_seen(command_ack.target_system, command_ack.target_component);
+
+					if (!command_ack.from_external
+					    && command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START
+					    && is_target_known
+					    && command_ack.target_component < vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) {
+
+						mavlink_command_ack_t msg{};
+						msg.result = command_ack.result;
+						msg.command = command_ack.command;
+						msg.progress = command_ack.result_param1;
+						msg.result_param2 = command_ack.result_param2;
+						msg.target_system = command_ack.target_system;
+						msg.target_component = command_ack.target_component;
+
+						mavlink_msg_command_ack_send_struct(get_channel(), &msg);
+
+						if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
+							cmd_logging_start_acknowledgement = true;
+
+						} else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP
+							   && command_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
+							cmd_logging_stop_acknowledgement = true;
+						}
+					}
+				}
+			}
+		}
+
+		// For legacy gimbals using the mavlink gimbal v1 protocol, we need to send out commands.
+		// We don't care about acks for these though.
+		if (_gimbal_v1_command_sub.updated()) {
+			vehicle_command_s cmd;
+			_gimbal_v1_command_sub.copy(&cmd);
+
+			// FIXME: filter for target system/component
+
+			mavlink_command_long_t msg{};
+			msg.param1 = cmd.param1;
+			msg.param2 = cmd.param2;
+			msg.param3 = cmd.param3;
+			msg.param4 = cmd.param4;
+			msg.param5 = cmd.param5;
+			msg.param6 = cmd.param6;
+			msg.param7 = cmd.param7;
+			msg.command = cmd.command;
+			msg.target_system = cmd.target_system;
+			msg.target_component = cmd.target_component;
+			msg.confirmation = 0;
+
+			mavlink_msg_command_long_send_struct(get_channel(), &msg);
+		}

 		/* check for shell output */
 		if (_mavlink_shell && _mavlink_shell->available() > 0) {
@@ -2460,170 +2588,6 @@ Mavlink::task_main(int argc, char *argv[])
 	return OK;
 }

-void Mavlink::handleStatus()
-{
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-
-		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			/* switch HIL mode if required */
-			set_hil_enabled(vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON);
-
-			if (_mode == MAVLINK_MODE_IRIDIUM) {
-
-				if (_transmitting_enabled && (!vehicle_status.gcs_connection_lost || (vehicle_status.high_latency_data_link_lost &&
-							      !_transmitting_enabled_commanded && _first_heartbeat_sent))) {
-
-					_transmitting_enabled = false;
-					mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s\t", _device_name);
-					events::send<int8_t>(events::ID("mavlink_iridium_disable"), events::Log::Info,
-							     "Disabling transmitting with IRIDIUM mavlink on instance {1}", _instance_id);
-
-				} else if (!_transmitting_enabled && vehicle_status.gcs_connection_lost
-					   && !vehicle_status.high_latency_data_link_lost) {
-					_transmitting_enabled = true;
-					mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s\t", _device_name);
-					events::send<int8_t>(events::ID("mavlink_iridium_enable"), events::Log::Info,
-							     "Enabling transmitting with IRIDIUM mavlink on instance {1}", _instance_id);
-				}
-			}
-		}
-	}
-}
-
-void Mavlink::handleCommands()
-{
-	if (_mode == MAVLINK_MODE_IRIDIUM) {
-		int vehicle_command_updates = 0;
-
-		while (_vehicle_command_sub.updated() && (vehicle_command_updates < vehicle_command_s::ORB_QUEUE_LENGTH)) {
-			vehicle_command_updates++;
-			const unsigned last_generation = _vehicle_command_sub.get_last_generation();
-			vehicle_command_s vehicle_cmd;
-
-			if (_vehicle_command_sub.update(&vehicle_cmd)) {
-				if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
-					PX4_ERR("vehicle_command lost, generation %u -> %u", last_generation, _vehicle_command_sub.get_last_generation());
-				}
-
-				if ((vehicle_cmd.command == vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY) &&
-				    _mode == MAVLINK_MODE_IRIDIUM) {
-
-					if (vehicle_cmd.param1 > 0.5f) {
-						if (!_transmitting_enabled) {
-							mavlink_log_info(&_mavlink_log_pub, "Enable transmitting with IRIDIUM mavlink on device %s by command\t",
-									 _device_name);
-							events::send<int8_t>(events::ID("mavlink_iridium_enable_cmd"), events::Log::Info,
-									     "Enabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id);
-						}
-
-						_transmitting_enabled = true;
-						_transmitting_enabled_commanded = true;
-
-					} else {
-						if (_transmitting_enabled) {
-							mavlink_log_info(&_mavlink_log_pub, "Disable transmitting with IRIDIUM mavlink on device %s by command\t",
-									 _device_name);
-							events::send<int8_t>(events::ID("mavlink_iridium_disable_cmd"), events::Log::Info,
-									     "Disabling transmitting with IRIDIUM mavlink on instance {1} by command", _instance_id);
-						}
-
-						_transmitting_enabled = false;
-						_transmitting_enabled_commanded = false;
-					}
-
-					// send positive command ack
-					vehicle_command_ack_s command_ack{};
-					command_ack.command = vehicle_cmd.command;
-					command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
-					command_ack.from_external = !vehicle_cmd.from_external;
-					command_ack.target_system = vehicle_cmd.source_system;
-					command_ack.target_component = vehicle_cmd.source_component;
-					command_ack.timestamp = vehicle_cmd.timestamp;
-					_vehicle_command_ack_pub.publish(command_ack);
-				}
-			}
-		}
-	}
-
-	// For legacy gimbals using the mavlink gimbal v1 protocol, we need to send out commands.
-	// We don't care about acks for these though.
-	if (_gimbal_v1_command_sub.updated()) {
-		vehicle_command_s cmd;
-		_gimbal_v1_command_sub.copy(&cmd);
-
-		// FIXME: filter for target system/component
-
-		mavlink_command_long_t msg{};
-		msg.param1 = cmd.param1;
-		msg.param2 = cmd.param2;
-		msg.param3 = cmd.param3;
-		msg.param4 = cmd.param4;
-		msg.param5 = cmd.param5;
-		msg.param6 = cmd.param6;
-		msg.param7 = cmd.param7;
-		msg.command = cmd.command;
-		msg.target_system = cmd.target_system;
-		msg.target_component = cmd.target_component;
-		msg.confirmation = 0;
-
-		mavlink_msg_command_long_send_struct(get_channel(), &msg);
-	}
-}
-
-void Mavlink::handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &logging_stop_ack)
-{
-	if (_vehicle_command_ack_sub.updated()) {
-		static constexpr size_t COMMAND_ACK_TOTAL_LEN = MAVLINK_MSG_ID_COMMAND_ACK_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-
-		while ((get_free_tx_buf() >= COMMAND_ACK_TOTAL_LEN) && _vehicle_command_ack_sub.updated()) {
-			vehicle_command_ack_s command_ack;
-			const unsigned last_generation = _vehicle_command_ack_sub.get_last_generation();
-
-			if (_vehicle_command_ack_sub.update(&command_ack)) {
-				if (_vehicle_command_ack_sub.get_last_generation() != last_generation + 1) {
-					PX4_ERR("vehicle_command_ack lost, generation %u -> %u", last_generation,
-						_vehicle_command_ack_sub.get_last_generation());
-				}
-
-				const bool is_target_known = _receiver.component_was_seen(command_ack.target_system, command_ack.target_component);
-
-				if (!command_ack.from_external
-				    && command_ack.command < vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START
-				    && is_target_known
-				    && command_ack.target_component < vehicle_command_s::COMPONENT_MODE_EXECUTOR_START) {
-
-					mavlink_command_ack_t msg{};
-					msg.result = command_ack.result;
-					msg.command = command_ack.command;
-					msg.progress = command_ack.result_param1;
-					msg.result_param2 = command_ack.result_param2;
-					msg.target_system = command_ack.target_system;
-					msg.target_component = command_ack.target_component;
-
-					if (_mode == MAVLINK_MODE_IRIDIUM) {
-						if (command_ack.from_external) {
-							// for MAVLINK_MODE_IRIDIUM send only if external
-							mavlink_msg_command_ack_send_struct(get_channel(), &msg);
-						}
-
-					} else {
-						mavlink_msg_command_ack_send_struct(get_channel(), &msg);
-					}
-
-					if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
-						logging_start_ack = true;
-
-					} else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP
-						   && command_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
-						logging_stop_ack = true;
-					}
-				}
-			}
-		}
-	}
-}
-
 void Mavlink::check_requested_subscriptions()
 {
 	if (_subscribe_to_stream != nullptr) {
@@ -2924,11 +2888,6 @@ Mavlink::display_status()
 	       _ftp_on ? "YES" : "NO",
 	       _transmitting_enabled ? "YES" : "NO");
 	printf("\tmode: %s\n", mavlink_mode_str(_mode));
-
-	if (_mode == MAVLINK_MODE_IRIDIUM) {
-		printf("\t    iridium tx freq: %.3f\n", (double)(_high_latency_freq));
-	}
-
 	printf("\tForwarding: %s\n", get_forwarding_on() ? "On" : "Off");
 	printf("\tMAVLink version: %" PRId32 "\n", _protocol_version);

@@ -3309,7 +3268,6 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 #if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE)
 	PRINT_MODULE_USAGE_PARAM_STRING('c', nullptr, "Multicast address in the range [239.0.0.0,239.255.255.255]", "Multicast address (multicasting can be enabled via MAV_{i}_BROADCAST param)", true);
 #endif
-	PRINT_MODULE_USAGE_PARAM_FLOAT('F', 0.015, 0.0, 50.0, "Sets the transmission frequency for iridium mode", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('f', "Enable message forwarding to other Mavlink instances", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('w', "Wait to send, until first message received", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('x', "Enable FTP", true);
@@ -584,7 +584,6 @@ private:
 	int			_baudrate{57600};
 	int			_datarate{1000};		///< data rate for normal streams (attitude, position, etc.)
 	float			_rate_mult{1.0f};
-	float			_high_latency_freq{0.015f};	///< frequency of HIGH_LATENCY2 stream

 	bool			_radio_status_available{false};
 	bool			_radio_status_critical{false};
@@ -708,12 +707,6 @@ private:

 	void check_requested_subscriptions();

-	void handleCommands();
-
-	void handleAndGetCurrentCommandAck(bool &logging_start_ack, bool &logging_stop_ack);
-
-	void handleStatus();
-
 	/**
 	 * Reconfigure a SiK radio if requested by MAV_SIK_RADIO_ID
 	 *
@@ -150,6 +150,7 @@
 # include "streams/SCALED_PRESSURE3.hpp"
 # include "streams/UAVIONIX_ADSB_OUT_CFG.hpp"
 # include "streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp"
+# include "streams/UTM_GLOBAL_POSITION.hpp"
 #endif // !CONSTRAINED_FLASH

 // ensure PX4 rotation enum and MAV_SENSOR_ROTATION align
@@ -418,6 +419,9 @@ static const StreamListItem streams_list[] = {
 #if defined(ADSB_VEHICLE_HPP)
 	create_stream_list_item<MavlinkStreamADSBVehicle>(),
 #endif // ADSB_VEHICLE_HPP
+#if defined(UTM_GLOBAL_POSITION_HPP)
+	create_stream_list_item<MavlinkStreamUTMGlobalPosition>(),
+#endif // UTM_GLOBAL_POSITION_HPP
 #if defined(COLLISION_HPP)
 	create_stream_list_item<MavlinkStreamCollision>(),
 #endif // COLLISION_HPP
@@ -224,6 +224,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_adsb_vehicle(msg);
 		break;

+	case MAVLINK_MSG_ID_UTM_GLOBAL_POSITION:
+		handle_message_utm_global_position(msg);
+		break;
+
 	case MAVLINK_MSG_ID_COLLISION:
 		handle_message_collision(msg);
 		break;
@@ -469,25 +473,6 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 	vcmd.param3 = cmd_mavlink.param3;
 	vcmd.param4 = cmd_mavlink.param4;

-	switch (cmd_mavlink.frame) {
-	case MAV_FRAME_GLOBAL:
-	case MAV_FRAME_GLOBAL_INT:
-		vcmd.frame = vehicle_command_s::FRAME_GLOBAL;
-		break;
-
-	case MAV_FRAME_GLOBAL_RELATIVE_ALT:
-	case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
-		vcmd.frame = vehicle_command_s::FRAME_GLOBAL_RELATIVE_ALTITUDE;
-		break;
-
-	default:
-		vcmd.frame = vehicle_command_s::FRAME_UNKNOWN;
-		PX4_ERR("invalid frame %" PRIu8 " for command %" PRIu16, cmd_mavlink.frame, cmd_mavlink.command);
-		acknowledge(msg->sysid, msg->compid, cmd_mavlink.command,
-			    vehicle_command_ack_s::VEHICLE_CMD_RESULT_COMMAND_UNSUPPORTED_MAV_FRAME);
-		return;
-	}
-
 	if (cmd_mavlink.x == INT32_MAX && cmd_mavlink.y == INT32_MAX) {
 		// INT32_MAX for x and y means to ignore it.
 		vcmd.param5 = (double)NAN;
@@ -2555,6 +2540,91 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
 	_transponder_report_pub.publish(t);
 }

+void
+MavlinkReceiver::handle_message_utm_global_position(mavlink_message_t *msg)
+{
+	mavlink_utm_global_position_t utm_pos;
+	mavlink_msg_utm_global_position_decode(msg, &utm_pos);
+
+	bool is_self_published = false;
+
+
+#ifndef BOARD_HAS_NO_UUID
+	px4_guid_t px4_guid;
+	board_get_px4_guid(px4_guid);
+	is_self_published = sizeof(px4_guid) == sizeof(utm_pos.uas_id)
+			    && memcmp(px4_guid, utm_pos.uas_id, sizeof(px4_guid_t)) == 0;
+#else
+
+	is_self_published = msg->sysid == _mavlink->get_system_id();
+#endif /* BOARD_HAS_NO_UUID */
+
+
+	//Ignore selfpublished UTM messages
+	if (is_self_published) {
+		return;
+	}
+
+	// Convert cm/s to m/s
+	float vx = utm_pos.vx / 100.0f;
+	float vy = utm_pos.vy / 100.0f;
+	float vz = utm_pos.vz / 100.0f;
+
+	transponder_report_s t{};
+	t.timestamp = hrt_absolute_time();
+	mav_array_memcpy(t.uas_id, utm_pos.uas_id, PX4_GUID_BYTE_LENGTH);
+	t.icao_address = msg->sysid;
+	t.lat = utm_pos.lat * 1e-7;
+	t.lon = utm_pos.lon * 1e-7;
+	t.altitude = utm_pos.alt / 1000.0f;
+	t.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC;
+	// UTM_GLOBAL_POSIION uses NED (north, east, down) coordinates for velocity, in cm / s.
+	t.heading = atan2f(vy, vx);
+	t.hor_velocity = sqrtf(vy * vy + vx * vx);
+	t.ver_velocity = -vz;
+	// TODO: Callsign
+	// For now, set it to all 0s. This is a null-terminated string, so not explicitly giving it a null
+	// terminator could cause problems.
+	memset(&t.callsign[0], 0, sizeof(t.callsign));
+	t.emitter_type = ADSB_EMITTER_TYPE_UAV;  // TODO: Is this correct?x2?
+
+	// The Mavlink docs do not specify what to do if tslc (time since last communication) is out of range of
+	// an 8-bit int, or if this is the first communication.
+	// Here, I assume that if this is the first communication, tslc = 0.
+	// If tslc > 255, then tslc = 255.
+	unsigned long time_passed = (t.timestamp - _last_utm_global_pos_com) / 1000000;
+
+	if (_last_utm_global_pos_com == 0) {
+		time_passed = 0;
+
+	} else if (time_passed > UINT8_MAX) {
+		time_passed = UINT8_MAX;
+	}
+
+	t.tslc = (uint8_t) time_passed;
+
+	t.flags = 0;
+
+	if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE) {
+		t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS;
+	}
+
+	if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE) {
+		t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;
+	}
+
+	if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE) {
+		t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING;
+		t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
+	}
+
+	// Note: t.flags has deliberately NOT set VALID_CALLSIGN or VALID_SQUAWK, because UTM_GLOBAL_POSITION does not
+	// provide these.
+	_transponder_report_pub.publish(t);
+
+	_last_utm_global_pos_com = t.timestamp;
+}
+
 void
 MavlinkReceiver::handle_message_collision(mavlink_message_t *msg)
 {
@@ -32,10 +32,6 @@ serial_config:
        then
            set MAV_ARGS "${MAV_ARGS} -z"
        fi
-        if param compare MAV_${i}_MODE 6
-        then
-            set MAV_ARGS "${MAV_ARGS} -F p:MAV_${i}_HL_FREQ"
-        fi
        mavlink start ${MAV_ARGS} -x
      port_config_param:
        name: MAV_${i}_CONFIG
@@ -182,22 +178,3 @@ parameters:
            num_instances: *max_num_config_instances
            default: [2, 2, 2]
            reboot_required: true
-
-        MAV_${i}_HL_FREQ:
-            description:
-                short: Configures the frequency of HIGH_LATENCY2 stream for instance ${i}
-                long: |
-                    Positive real value that configures the transmission frequency of the
-                    HIGH_LATENCY2 stream for instance ${i}, configured in iridium mode.
-                    This parameter has no effect if the instance mode is different from iridium.
-
-            type: float
-            decimal: 3
-            increment: 0.001
-            unit: Hz
-            min: 0.0
-            max: 50.0
-            num_instances: *max_num_config_instances
-            default: [0.015, 0.015, 0.015]
-            reboot_required: true
-
@@ -53,7 +53,6 @@
 #include <uORB/topics/vehicle_thrust_setpoint.h>
 #include <uORB/topics/sensor_gps.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/failsafe_flags.h>
 #include <uORB/topics/health_report.h>
@@ -126,21 +125,18 @@ private:
 			updated |= _temperature.valid();
 			updated |= _throttle.valid();
 			updated |= _windspeed.valid();
-			updated |= write_attitude_setpoint_if_updated(&msg);
-			updated |= write_estimator_status_if_updated(&msg);
-			updated |= write_fw_ctrl_status_if_updated(&msg);
-			updated |= write_geofence_result_if_updated(&msg);
-			updated |= write_global_position_if_updated(&msg);
-			updated |= write_heading_if_updated(&msg);
-			updated |= write_mission_result_if_updated(&msg);
+			updated |= write_airspeed(&msg);
+			updated |= write_attitude_sp(&msg);
+			updated |= write_battery_status(&msg);
+			updated |= write_estimator_status(&msg);
+			updated |= write_fw_ctrl_status(&msg);
+			updated |= write_geofence_result(&msg);
+			updated |= write_global_position(&msg);
+			updated |= write_mission_result(&msg);
+			updated |= write_tecs_status(&msg);
+			updated |= write_vehicle_status(&msg);
 			updated |= write_failsafe_flags(&msg);
-
-			// these topics are already updated in update_data() and thus we just copy them here
-			write_airspeed(&msg);
-			write_battery_status(&msg);
-			write_tecs_status(&msg);
-			write_vehicle_status(&msg);
-			write_wind(&msg);
+			updated |= write_wind(&msg);

 			if (updated) {
 				msg.timestamp = t / 1000;
@@ -248,7 +244,7 @@ private:
 	{
 		airspeed_s airspeed;

-		if (_airspeed_sub.copy(&airspeed)) {
+		if (_airspeed_sub.update(&airspeed)) {
 			if (airspeed.confidence < 0.95f) { // the same threshold as for the commander
 				msg->failure_flags |= HL_FAILURE_FLAG_DIFFERENTIAL_PRESSURE;
 			}
@@ -259,7 +255,7 @@ private:
 		return false;
 	}

-	bool write_attitude_setpoint_if_updated(mavlink_high_latency2_t *msg)
+	bool write_attitude_sp(mavlink_high_latency2_t *msg)
 	{
 		vehicle_attitude_setpoint_s attitude_sp;

@@ -278,7 +274,7 @@ private:
 		for (int i = 0; i < battery_status_s::MAX_INSTANCES; i++) {
 			battery_status_s battery;

-			if (_batteries[i].subscription.copy(&battery)) {
+			if (_batteries[i].subscription.update(&battery)) {
 				updated = true;
 				_batteries[i].connected = battery.connected;

@@ -291,12 +287,11 @@ private:
 		return updated;
 	}

-	bool write_estimator_status_if_updated(mavlink_high_latency2_t *msg)
+	bool write_estimator_status(mavlink_high_latency2_t *msg)
 	{
 		// use primary estimator_status
-		estimator_selector_status_s estimator_selector_status;
-
-		if (_estimator_selector_status_sub.update(&estimator_selector_status)) {
+		if (_estimator_selector_status_sub.updated()) {
+			estimator_selector_status_s estimator_selector_status;

 			if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
 				if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) {
@@ -325,7 +320,7 @@ private:
 		return false;
 	}

-	bool write_fw_ctrl_status_if_updated(mavlink_high_latency2_t *msg)
+	bool write_fw_ctrl_status(mavlink_high_latency2_t *msg)
 	{
 		position_controller_status_s pos_ctrl_status;

@@ -339,7 +334,7 @@ private:
 		return false;
 	}

-	bool write_geofence_result_if_updated(mavlink_high_latency2_t *msg)
+	bool write_geofence_result(mavlink_high_latency2_t *msg)
 	{
 		geofence_result_s geofence;

@@ -355,12 +350,12 @@ private:
 		return false;
 	}

-	bool write_global_position_if_updated(mavlink_high_latency2_t *msg)
+	bool write_global_position(mavlink_high_latency2_t *msg)
 	{
 		vehicle_global_position_s global_pos;
 		vehicle_local_position_s local_pos;

-		if (_global_pos_sub.update(&global_pos) && _local_pos_sub.copy(&local_pos)) {
+		if (_global_pos_sub.update(&global_pos) && _local_pos_sub.update(&local_pos)) {
 			msg->latitude = global_pos.lat * 1e7;
 			msg->longitude = global_pos.lon * 1e7;

@@ -375,20 +370,7 @@ private:

 			msg->altitude = altitude;

-			return true;
-		}
-
-		return false;
-	}
-
-	bool write_heading_if_updated(mavlink_high_latency2_t *msg)
-	{
-		vehicle_attitude_s attitude;
-
-		if (_attitude_sub.update(&attitude)) {
-
-			const matrix::Eulerf euler = matrix::Quatf(attitude.q);
-			msg->heading = static_cast<uint8_t>(math::degrees(matrix::wrap_2pi(euler.psi())) * 0.5f);
+			msg->heading = static_cast<uint8_t>(math::degrees(matrix::wrap_2pi(local_pos.heading)) * 0.5f);

 			return true;
 		}
@@ -396,7 +378,7 @@ private:
 		return false;
 	}

-	bool write_mission_result_if_updated(mavlink_high_latency2_t *msg)
+	bool write_mission_result(mavlink_high_latency2_t *msg)
 	{
 		mission_result_s mission_result;

@@ -412,7 +394,7 @@ private:
 	{
 		tecs_status_s tecs_status;

-		if (_tecs_status_sub.copy(&tecs_status)) {
+		if (_tecs_status_sub.update(&tecs_status)) {
 			int16_t target_altitude;
 			convert_limit_safe(tecs_status.altitude_sp, target_altitude);
 			msg->target_altitude = target_altitude;
@@ -427,7 +409,7 @@ private:
 	{
 		vehicle_status_s status;

-		if (_status_sub.copy(&status)) {
+		if (_status_sub.update(&status)) {
 			health_report_s health_report;

 			if (_health_report_sub.copy(&health_report)) {
@@ -494,7 +476,7 @@ private:
 	{
 		wind_s wind;

-		if (_wind_sub.copy(&wind)) {
+		if (_wind_sub.update(&wind)) {
 			msg->wind_heading = static_cast<uint8_t>(math::degrees(matrix::wrap_2pi(atan2f(wind.windspeed_east,
 					    wind.windspeed_north))) * 0.5f);
 			return true;
@@ -643,7 +625,6 @@ private:
 	uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0};
 	uORB::Subscription _vehicle_thrust_setpoint_1_sub{ORB_ID(vehicle_thrust_setpoint), 1};
 	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
-	uORB::Subscription _attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _attitude_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
 	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
@@ -172,7 +172,6 @@ PARAM_DEFINE_FLOAT(MPC_YAWRAUTO_ACC, 60.f);
 * @value 2 away from home
 * @value 3 along trajectory
 * @value 4 towards waypoint (yaw first)
- * @value 5 yaw fixed
 * @group Mission
 */
 PARAM_DEFINE_INT32(MPC_YAW_MODE, 0);
@@ -108,7 +108,6 @@ Land::on_active()
 		vehicle_command_s vcmd = {};

 		vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
-		vcmd.frame = vehicle_command_s::FRAME_GLOBAL;
 		vcmd.param1 = -1;
 		vcmd.param2 = 1;
 		vcmd.param5 = _navigator->get_global_position()->lat;
@@ -855,7 +855,6 @@ MissionBase::do_abort_landing()
 	vehicle_command_s vcmd = {};

 	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_REPOSITION;
-	vcmd.frame = vehicle_command_s::FRAME_GLOBAL;
 	vcmd.param1 = -1;
 	vcmd.param2 = 1;
 	vcmd.param5 = _mission_item.lat;
@@ -267,7 +267,6 @@ void Navigator::run()

 				vehicle_global_position_s position_setpoint{};

-				// latitude/longitude
 				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
 					position_setpoint.lat = cmd.param5;
 					position_setpoint.lon = cmd.param6;
@@ -277,22 +276,7 @@ void Navigator::run()
 					position_setpoint.lon = get_global_position()->lon;
 				}

-				// altitude
-				if (PX4_ISFINITE(cmd.param7)) {
-					switch (cmd.frame) {
-					default:
-					case vehicle_command_s::FRAME_GLOBAL:
-						position_setpoint.alt = cmd.param7;
-						break;
-
-					case vehicle_command_s::FRAME_GLOBAL_RELATIVE_ALTITUDE:
-						position_setpoint.alt = cmd.param7 + get_home_position()->alt;
-						break;
-					}
-
-				} else {
-					position_setpoint.alt = get_global_position()->alt;
-				}
+				position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt;

 				if (geofence_allows_position(position_setpoint)) {
 					position_setpoint_triplet_s *rep = get_reposition_triplet();
@@ -1289,9 +1273,6 @@ void Navigator::check_traffic()
 			}
 		}
 	}
-
-	_adsb_conflict.remove_expired_conflicts();
-
 }

 bool Navigator::abort_landing()
@@ -1563,7 +1544,7 @@ controller.
 	PRINT_MODULE_USAGE_NAME("navigator", "controller");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("fencefile", "load a geofence file from SD card, stored at etc/geofence.txt");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("fake_traffic", "publishes 24 fake transponder_report_s uORB messages");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("fake_traffic", "publishes 4 fake transponder_report_s uORB messages");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();

 	return 0;
@@ -78,8 +78,8 @@ bool GZMixingInterfaceWheel::updateOutputs(bool stop_wheels, uint16_t outputs[MA

 		for (unsigned i = 0; i < active_output_count; i++) {
 			// Offsetting the output allows for negative values despite unsigned integer to reverse the wheels
-			static constexpr double output_offset = 100.0;
-			double scaled_output = (double)outputs[i] - output_offset;
+			static constexpr float output_offset = 100.0f;
+			float scaled_output = (float)outputs[i] - output_offset;
 			wheel_velocity_message.set_velocity(i, scaled_output);
 		}

@@ -271,16 +271,9 @@ void AutopilotTester::execute_mission()
 	REQUIRE(poll_condition_with_timeout(
 	[this]() { return _mission->start_mission() == Mission::Result::Success; }, std::chrono::seconds(3)));

-	float speed_factor = 1.0f;
+	// TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc.

-	if (_info != nullptr) {
-		speed_factor = _info->get_speed_factor().second;
-	}
-
-	const float mission_finish_waiting_time_in_simulation_s = 500.f;
-	float mission_finish_waiting_time_in_real_s = mission_finish_waiting_time_in_simulation_s / speed_factor;
-
-	wait_for_mission_finished(std::chrono::seconds(static_cast<int>(mission_finish_waiting_time_in_real_s)));
+	wait_for_mission_finished(std::chrono::seconds(90));
 }

 void AutopilotTester::execute_mission_and_lose_gps()
@@ -395,16 +388,9 @@ void AutopilotTester::execute_mission_raw()
 {
 	REQUIRE(_mission->start_mission() == Mission::Result::Success);

-	float speed_factor = 1.0f;
+	// TODO: Adapt time limit based on mission size, flight speed, sim speed factor, etc.

-	if (_info != nullptr) {
-		speed_factor = _info->get_speed_factor().second;
-	}
-
-	const float waiting_time_simulation_time_s = 300.f; // currently this is tuned for the VTOL wind test
-	float waiting_time_absolute_s = waiting_time_simulation_time_s / speed_factor;
-
-	wait_for_mission_raw_finished(std::chrono::seconds(static_cast<int>(waiting_time_absolute_s)));
+	wait_for_mission_raw_finished(std::chrono::seconds(120));
 }

 void AutopilotTester::execute_rtl()
@@ -47,7 +47,7 @@ TEST_CASE("Fly VTOL Loiter with airspeed failure", "[vtol_airspeed_fail]")
 	const float takeoff_altitude = 10.f;
 	tester.set_takeoff_altitude(takeoff_altitude);

-	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission.plan");
+	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission_straight_south.plan");
 	tester.arm();

 	tester.takeoff();
@@ -49,7 +49,7 @@
                    0,
                    0,
                    null,
-                    47.392,
+                    47.38497919751799,
                    8.54578066404548,
                    30
                ],
@@ -142,13 +142,13 @@ parameters:
                            # (Extend this list as needed)
                            type: string
                            allowed: [
-                                '%', 'Hz', '1/s', 'mAh',
+                                '%', 'Hz', 'mAh',
                                'rad', '%/rad', 'rad/s', 'rad/s^2', '%/rad/s',  'rad s^2/m','rad s/m',
                                'bit/s', 'B/s',
                                'deg', 'deg*1e7', 'deg/s', 'deg/s^2',
                                'celcius', 'gauss', 'gauss/s', 'mgauss', 'mgauss^2',
                                'hPa', 'kg', 'kg/m^2', 'kg m^2', 'kg/m^3',
-                                'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad', 'g0',
+                                'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad',
                                'Ohm', 'V', 'A',
                                'us', 'ms', 's',
                                'S', 'A/%', '(m/s^2)^2',  'm/m',  'tan(rad)^2', '(m/s)^2', 'm/rad',