mavlink: return unsupported frame result when that's the case

mavlink: spacing, naming of command frame handling
Update MAVLink to contain _INT frame deprecation warning
2026-05-28 19:40:05 +08:00 · 2024-05-02 17:32:37 +02:00 · 2024-05-02 17:30:59 +02:00 · 2024-05-02 17:19:31 +02:00 · 2024-05-02 16:49:37 +02:00 · 2024-05-02 16:49:37 +02:00
72 changed files with 1240 additions and 524 deletions
@@ -15,6 +15,7 @@
 	"extensions": [
 		"chiehyu.vscode-astyle",
 		"dan-c-underwood.arm",
+		"editorconfig.editorconfig",
 		"fredericbonnet.cmake-test-adapter",
 		"github.vscode-pull-request-github",
 		"marus25.cortex-debug",
@@ -4,6 +4,7 @@
    "recommendations": [
        "chiehyu.vscode-astyle",
        "dan-c-underwood.arm",
+        "editorconfig.editorconfig",
        "fredericbonnet.cmake-test-adapter",
        "github.vscode-pull-request-github",
        "marus25.cortex-debug",
@@ -34,24 +34,23 @@ def extract_timer(line):
    if search:
        return search.group(1), 'generic'

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

    return None, 'unknown'

-def extract_timer_from_channel(line, num_channels_already_found):
+def extract_timer_from_channel(line, timer_names):
    # 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 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)
+    # 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)
    if search:
-        # imxrt uses a 1:1 timer group to channel association
-        return str(num_channels_already_found)
+        return str(timer_names.index((search.group(1) + '_' +  search.group(2))))

    return None

@@ -69,6 +68,7 @@ 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,14 +77,12 @@ def get_timer_groups(timer_config_file, verbose=False):

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

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

        if timer:
            if verbose: print('Found timer: {:} in channel line {:}'.format(timer, line))
@@ -6,6 +6,7 @@
 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_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1)
+#define GPIO_LED_SAFETY       /* PA1  */ (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN1)

 /* Tone alarm output. */
 #define TONE_ALARM_TIMER             2    /* timer 2 */
@@ -15,6 +15,7 @@ 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 13
+param set-default EKF2_BARO_DELAY 39
 param set-default EKF2_BARO_NOISE 0.9
-param set-default EKF2_HGT_REF 2
-param set-default EKF2_MAG_DELAY 100
+param set-default EKF2_HGT_REF 0
+param set-default EKF2_MAG_DELAY 60
 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 48
+param set-default EKF2_OF_DELAY 28
 param set-default EKF2_OF_N_MIN 0.05
 param set-default EKF2_RNG_A_HMAX 25
-param set-default EKF2_RNG_DELAY 48
+param set-default EKF2_RNG_DELAY 105
 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,8 +21,10 @@ then
 fi

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

 # Internal Baro on I2C
 bmp388 -I -b 4 start
@@ -185,7 +185,6 @@ 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,7 +86,6 @@ 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_heartbeat				# timestamp of the last successful sbd session
+uint64 last_at_ok_timestamp			# timestamp of the last "OK" received after the "AT" command
 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,ERR,INFO}
+# A logging message, output with PX4_WARN, PX4_ERR, PX4_INFO

 uint64 timestamp		# time since system start (microseconds)

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

-uint8 ORB_QUEUE_LENGTH = 8
+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

 float32 param1			# Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.
 float32 param2			# Parameter 2, as defined by MAVLink uint16 VEHICLE_CMD enum.
@@ -184,4 +187,6 @@ 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,13 +5,16 @@
 uint64 timestamp		# time since system start (microseconds)

 # Result cases. This follows the MAVLink MAV_RESULT enum definition
-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
+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

 # Arming denied specific cases
 uint16 ARM_AUTH_DENIED_REASON_GENERIC = 0
@@ -50,3 +50,15 @@
 * @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 <libevents_definitions.h>
+#include <uORB/topics/event.h>

 #include <stdint.h>

@@ -93,7 +93,7 @@ constexpr unsigned sizeofArguments(const T &t, const Args &... args)
 /**
 * publish/send an event
 */
-void send(EventType &event);
+void send(event_s &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)
 {
-	EventType e{};
+	event_s 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)
 {
-	EventType e{};
+	event_s 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,6 +59,7 @@ 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;
 			}

@@ -87,3 +88,48 @@ 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,6 +255,34 @@ 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;
@@ -391,41 +419,66 @@ static int allocate_channel(unsigned channel, io_timer_channel_mode_t mode)
 	return rv;
 }

-static int timer_set_rate(unsigned channel, unsigned rate)
+static int timer_set_rate(unsigned timer, unsigned rate)
 {
+	int channels = get_timer_channels(timer);
+
 	irqstate_t flags = px4_enter_critical_section();
-	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;
+
+	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;
+
+		}
+	}
+
 	px4_leave_critical_section(flags);
 	return 0;
 }

-static inline void io_timer_set_oneshot_mode(unsigned channel)
+static inline void io_timer_set_oneshot_mode(unsigned timer)
 {
+	int channels = get_timer_channels(timer);
+
 	irqstate_t flags = px4_enter_critical_section();
-	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;
+
+	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;
+		}
+	}
+
 	px4_leave_critical_section(flags);

 }

-static inline void io_timer_set_PWM_mode(unsigned channel)
+static inline void io_timer_set_PWM_mode(unsigned timer)
 {
+	int channels = get_timer_channels(timer);
+
 	irqstate_t flags = px4_enter_critical_section();
-	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;
+
+	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;
+		}
+	}
+
 	px4_leave_critical_section(flags);
 }

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

-		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;
+		int channels = get_timer_channels(timer);

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

-			/* 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;
-			io_timer_set_PWM_mode(chan);
-			timer_set_rate(chan, 50);
+				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(timer);
+		timer_set_rate(timer, 50);
+
 		px4_leave_critical_section(flags);
 	}

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

-// The rt has 1:1 group to channel
-uint32_t io_timer_get_group(unsigned group)
+uint32_t io_timer_get_group(unsigned timer)
 {
-	return get_channel_mask(group);
+	return get_timer_channels(timer);
 }
@@ -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,7 +157,6 @@ 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;
@@ -669,34 +668,35 @@ 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 (raw_yaw > 180) {
-					raw_yaw = raw_yaw - 180;
+				if (scaled_yaw > 180) {
+					scaled_yaw = scaled_yaw - 180;

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

 				break;

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

 			case 3:
-				raw_yaw = raw_yaw - 90; // rotation facing left
+				scaled_yaw = scaled_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,6 +64,7 @@
 #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,7 +939,8 @@ GPS::run()
 						set_device_type(DRV_GPS_DEVTYPE_UBX_9);
 						break;

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

@@ -247,14 +247,16 @@ PARAM_DEFINE_INT32(GPS_2_PROTOCOL, 1);
 * 2 : Use Galileo
 * 3 : Use BeiDou
 * 4 : Use GLONASS
+ * 5 : Use NAVIC
 *
 * @min 0
- * @max 31
+ * @max 63
 * @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
@@ -277,14 +279,16 @@ PARAM_DEFINE_INT32(GPS_1_GNSS, 0);
 * 2 : Use Galileo
 * 3 : Use BeiDou
 * 4 : Use GLONASS
+ * 5 : Use NAVIC
 *
 * @min 0
- * @max 31
+ * @max 63
 * @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,10 +441,11 @@ 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_ned_valid = true;
+			sensor_gps.vel_m_s = matrix::Vector3f(velocityGpsNed.c).length();
 			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,4 +41,5 @@ add_subdirectory(lis3mdl)
 add_subdirectory(lsm303agr)
 add_subdirectory(memsic)
 add_subdirectory(rm3100)
+add_subdirectory(st)
 add_subdirectory(vtrantech)
@@ -14,6 +14,7 @@ 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"
@@ -0,0 +1,3 @@
+menu "ST"
+    rsource "*/Kconfig"
+endmenu
@@ -0,0 +1,45 @@
+############################################################################
+#
+#   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
+	)
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_MAGNETOMETER_ST_IIS2MDC
+	bool "iis2mdc"
+	default n
+	---help---
+		Enable support for iis2mdc
@@ -0,0 +1,137 @@
+/****************************************************************************
+ *
+ *   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);
+}
@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   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,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,18 +31,67 @@
 *
 ****************************************************************************/

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

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

+	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);
+}
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   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,14 +87,6 @@ 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_heartbeat);
+	PX4_INFO("Last heartbeat:               %" PRId64, _last_at_ok_timestamp);
 	return 0;
 }

@@ -333,6 +333,11 @@ 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();

@@ -477,7 +482,6 @@ 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) {
@@ -498,8 +502,6 @@ 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;
@@ -552,11 +554,6 @@ 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;
 }
@@ -571,11 +568,6 @@ 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");

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

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

@@ -718,11 +710,6 @@ 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];
@@ -779,11 +766,6 @@ 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);


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

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

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

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

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

 	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_ready(void);
+	bool is_modem_responsive(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_heartbeat = 0;
+	hrt_abstime _last_at_ok_timestamp = 0;
 	hrt_abstime _session_start_time = 0;

 	satcom_state _state = SATCOM_STATE_STANDBY;
@@ -45,7 +45,7 @@ static uint16_t event_sequence{events::initial_event_sequence};
 namespace events
 {

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

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

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

-void send(EventType &event)
+void send(event_s &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_heartbeat > _boot_timestamp) {
+			if (_high_latency_datalink_timestamp < _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,
-				      3250,
+				      PX4_STACK_ADJUSTED(3250),
 				      (px4_main_t)&run_trampoline,
 				      (char *const *)argv);

@@ -2672,6 +2672,28 @@ 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;

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

 			case telemetry_status_s::LINK_TYPE_IRIDIUM: {
-					iridiumsbd_status_s iridium_status;

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

-						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;
-							}
+						_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;
 						}
 					}

@@ -2836,19 +2860,6 @@ 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()
@@ -246,8 +246,9 @@ private:

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

-	hrt_abstime _high_latency_datalink_heartbeat{0};
+	hrt_abstime _high_latency_datalink_timestamp{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};
@@ -284,7 +284,7 @@ bool Report::report(bool is_armed, bool force)

 	// send all events
 	int offset = 0;
-	events::EventType event;
+	event_s 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(events::EventType::arguments), "Too many arguments");
+	static_assert(args_size <= sizeof(event_s::arguments), "Too many arguments");
 	unsigned total_size = sizeof(EventBufferHeader) + args_size;

 	if (total_size > sizeof(_event_buffer) - _next_buffer_idx) {
@@ -99,8 +99,12 @@ 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;
-	param_get(param_find_no_notification("EKF2_EN"), &param_ekf2_en);
+
+	if (param_ekf2_en_handle != PARAM_INVALID) {
+		param_get(param_ekf2_en_handle, &param_ekf2_en);
+	}

 	if (missing_data && (param_ekf2_en == 1)) {
 		/* EVENT
@@ -48,13 +48,7 @@ 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.
@@ -66,7 +60,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>(required_groups, health_component_t::accel,
+				reporter.armingCheckFailure<uint8_t, float, float>(NavModes::All, 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());

@@ -85,13 +79,7 @@ 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.
@@ -103,7 +91,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>(required_groups, health_component_t::gyro,
+				reporter.armingCheckFailure<uint8_t, float, float>(NavModes::All, 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::system,
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_attitude, health_component_t::attitude_estimate,
 					    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::system,
+		reporter.armingCheckFailure(local_position_modes, health_component_t::local_position_estimate,
 					    events::ID("check_modes_local_pos"),
 					    events::Log::Error, "No valid local position estimate");
 		reporter.clearCanRunBits(local_position_modes);
@@ -87,7 +87,8 @@ 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::system,
+		reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position,
+					    health_component_t::global_position_estimate,
 					    events::ID("check_modes_global_pos"),
 					    events::Log::Error, "No valid global position estimate");
 		reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position);
@@ -672,7 +672,6 @@ 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,7 +358,6 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
 				&& !_gps_intermittent;

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

 				fuseGpsYaw(gps_sample.yaw_offset);
@@ -366,27 +365,14 @@ 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) {
-					if (_nb_gps_yaw_reset_available > 0) {
-						// Data seems good, attempt a reset
-						resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset);
+					stopGpsYawFusion();

-						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();
+					// 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;
 					}
-
-					// TODO: should we give a new reset credit when the fusion does not fail for some time?
 				}

 			} else {
@@ -397,14 +383,27 @@ void Ekf::controlGpsYawFusion(const gnssSample &gps_sample)
 		} else {
 			if (starting_conditions_passing) {
 				// Try to activate GPS yaw fusion
-				if (resetYawToGps(gps_sample.yaw, gps_sample.yaw_offset)) {
-					ECL_INFO("starting GPS yaw fusion");
+				const bool not_using_ne_aiding = !_control_status.flags.gps && !_control_status.flags.aux_gpos;

-					_aid_src_gnss_yaw.time_last_fuse = _time_delayed_us;
+				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;
-					_control_status.flags.yaw_align = true;
+					fuseGpsYaw(gps_sample.yaw_offset);
+				}

-					_nb_gps_yaw_reset_available = 1;
+				if (_control_status.flags.gps_yaw) {
+					ECL_INFO("starting GPS yaw fusion");
 				}
 			}
 		}
@@ -424,15 +423,7 @@ void Ekf::stopGpsYawFusion()
 		_control_status.flags.gps_yaw = false;
 		resetEstimatorAidStatus(_aid_src_gnss_yaw);

-		// 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");
-		}
+		ECL_INFO("stopping GPS yaw fusion");
 	}
 }
 #endif // CONFIG_EKF2_GNSS_YAW
@@ -114,8 +114,7 @@ parameters:
      decimal: 1
    EKF2_AVEL_DELAY:
      description:
-        short: Auxiliary Velocity Estimate (e.g from a landing target) delay relative
-          to IMU measurements
+        short: Auxiliary Velocity Estimate delay relative to IMU measurements
      type: float
      default: 5
      min: 0
@@ -582,7 +581,8 @@ parameters:
      max: 3
    EKF2_NOAID_TOUT:
      description:
-        short: Maximum lapsed time from last fusion of measurements that constrain
+        short: Maximum inertial dead-reckoning time
+        long: 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 observations used to lower bound
-          or replace the uncertainty included in the message
+        short: Measurement noise for vision position measurements
+        long:  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 observations used to lower bound
-          or replace the uncertainty included in the message
+        short: Measurement noise for vision velocity measurements
+        long:  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 observations used to lower bound
-          or replace the uncertainty included in the message
+        short: Measurement noise for vision angle measurements
+        long:  Used to lower bound or replace the uncertainty included in the message
      type: float
      default: 0.1
      min: 0.05
@@ -697,7 +697,8 @@ parameters:
      default: 0
    EKF2_OF_N_MIN:
      description:
-        short: Measurement noise for the optical flow sensor when it's reported quality
+        short: Optical flow minimum noise
+        long: Measurement noise for the optical flow sensor when it's reported quality
          metric is at the maximum
      type: float
      default: 0.15
@@ -706,9 +707,9 @@ parameters:
      decimal: 2
    EKF2_OF_N_MAX:
      description:
-        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'
+        short: Optical flow maximum noise
+        long: Measurement noise for the optical flow sensor when it's reported quality
+          metric is at the minimum
      type: float
      default: 0.5
      min: 0.05
@@ -716,7 +717,8 @@ parameters:
      decimal: 2
    EKF2_OF_QMIN:
      description:
-        short: Optical Flow data will only be used in air if the sensor reports a
+        short: In air optical flow minimum quality
+        long: Optical Flow data will only be used in air if the sensor reports a
          quality metric >= EKF2_OF_QMIN
      type: int32
      default: 1
@@ -724,7 +726,8 @@ parameters:
      max: 255
    EKF2_OF_QMIN_GND:
      description:
-        short: Optical Flow data will only be used on the ground if the sensor reports
+        short: On ground optical flow minimum quality
+        long: 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
@@ -742,8 +745,7 @@ parameters:
      decimal: 1
    EKF2_TERR_NOISE:
      description:
-        short: Terrain altitude process noise - accounts for instability in vehicle
-          height estimate
+        short: Terrain altitude process noise
      type: float
      default: 5.0
      min: 0.5
@@ -759,120 +761,120 @@ parameters:
      decimal: 2
    EKF2_IMU_POS_X:
      description:
-        short: X position of IMU in body frame (forward axis with origin relative
-          to vehicle centre of gravity)
+        short: X position of IMU in body frame
+        long: 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 (right axis with origin relative to
-          vehicle centre of gravity)
+        short: Y position of IMU in body frame
+        long: Forward 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 (down axis with origin relative to
-          vehicle centre of gravity)
+        short: Z position of IMU in body frame
+        long: Forward 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 (forward axis with origin relative
-          to vehicle centre of gravity)
+        short: X position of GPS antenna in body frame
+        long: 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 (right axis with origin relative
-          to vehicle centre of gravity)
+        short: Y position of GPS antenna in body frame
+        long: Forward 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 (down axis with origin relative
-          to vehicle centre of gravity)
+        short: Z position of GPS antenna in body frame
+        long: Forward 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 (forward axis with
-          origin relative to vehicle centre of gravity)
+        short: X position of range finder origin in body frame
+        long: 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 (right axis with origin
-          relative to vehicle centre of gravity)
+        short: Y position of range finder origin in body frame
+        long: Forward 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 (down axis with origin
-          relative to vehicle centre of gravity)
+        short: Z position of range finder origin in body frame
+        long: Forward 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 (forward axis
-          with origin relative to vehicle centre of gravity)
+        short: X position of optical flow focal point in body frame
+        long: 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 (right axis with
-          origin relative to vehicle centre of gravity)
+        short: Y position of optical flow focal point in body frame
+        long: Forward 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 (down axis with
-          origin relative to vehicle centre of gravity)
+        short: Z position of optical flow focal point in body frame
+        long: Forward 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 (forward axis with
-          origin relative to vehicle centre of gravity)
+        short: X position of VI sensor focal point in body frame
+        long: 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 (right axis with
-          origin relative to vehicle centre of gravity)
+        short: Y position of VI sensor focal point in body frame
+        long: Forward 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 (down axis with origin
-          relative to vehicle centre of gravity)
+        short: Z position of VI sensor focal point in body frame
+        long: Forward axis with origin relative to vehicle centre of gravity
      type: float
      default: 0.0
      unit: m
@@ -908,8 +910,8 @@ parameters:
      decimal: 2
    EKF2_TAU_POS:
      description:
-        short: Time constant of the position output prediction and smoothing filter.
-          Controls how tightly the output track the EKF states
+        short: Output predictor position time constant
+        long: Controls how tightly the output track the EKF states
      type: float
      default: 0.25
      min: 0.1
@@ -968,8 +970,7 @@ parameters:
      unit: m/s
    EKF2_RNG_A_HMAX:
      description:
-        short: Maximum absolute altitude (height above ground level) allowed for conditional
-          range aid mode
+        short: Maximum height above ground 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).
@@ -990,7 +991,8 @@ parameters:
      max: 5.0
    EKF2_RNG_QLTY_T:
      description:
-        short: Minimum duration during which the reported range finder signal quality
+        short: Minumum range validity period
+        long: 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
@@ -1041,9 +1043,9 @@ parameters:
      default: 0
    EKF2_DRAG_NOISE:
      description:
-        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.
+        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.
      type: float
      default: 2.5
      min: 0.5
@@ -1082,7 +1084,7 @@ parameters:
      decimal: 1
    EKF2_MCOEF:
      description:
-        short: Propeller momentum drag coefficient used for multi-rotor wind estimation
+        short: Propeller momentum drag coefficient 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
@@ -1104,8 +1106,7 @@ parameters:
      decimal: 2
    EKF2_ASPD_MAX:
      description:
-        short: Upper limit on airspeed along individual axes used to correct baro
-          for position error effects
+        short: Maximum airspeed used for baro static pressure compensation
      type: float
      default: 20.0
      min: 5.0
@@ -1209,8 +1210,7 @@ parameters:
      decimal: 1
    EKF2_ABL_TAU:
      description:
-        short: Time constant used by acceleration and angular rate magnitude checks
-          used to inhibit accel bias learning
+        short: Accel bias learning inhibit time constant
        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 observations used to lower
-          bound or replace the uncertainty included in the message
+        short: Measurement noise for aux global position measurements
+        long:  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,9 +275,15 @@ TEST_F(EkfGpsHeadingTest, yawJmpOnGround)
 	_sensor_simulator._gps.setYaw(gps_heading);
 	_sensor_simulator.runSeconds(8);

-	// THEN: the fusion should reset
-	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	// 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);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeadingFusion());
+	EXPECT_EQ(_ekf_wrapper.getQuaternionResetCounter(), initial_quat_reset_counter + 2);
 	EXPECT_LT(fabsf(matrix::wrap_pi(_ekf_wrapper.getYawAngle() - gps_heading)), math::radians(1.f));
 }

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

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

-	// 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: 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: 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,7 +170,8 @@ bool FlightTaskAuto::update()
 		waypoints[2] = _position_setpoint;
 	}

-	const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == 4 && !_yaw_sp_aligned;
+	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 force_zero_velocity_setpoint = should_wait_for_yaw_align || _is_emergency_braking_active;
 	_updateTrajConstraints();
 	PositionSmoothing::PositionSmoothingSetpoints smoothed_setpoints;
@@ -532,32 +533,37 @@ void FlightTaskAuto::_set_heading_from_mode()

 	Vector2f v; // Vector that points towards desired location

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

-	case 0: // Heading points towards the current waypoint.
-	case 4: // Same as 0 but yaw first and then go
+	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
 		v = Vector2f(_target) - Vector2f(_position);
 		break;

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

 		break;

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

 		break;

-	case 3: // Along trajectory.
+	case yaw_mode::along_trajectory: // 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,6 +81,15 @@ 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:
@@ -2304,7 +2304,7 @@ FixedwingPositionControl::Run()
 			_reference_altitude = 0.f;
 		}

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

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

 void SendProtocol::send_event(const Event &event) const
 {
-	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);
-
+	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);
+	}
 }

 void SendProtocol::on_gcs_connected()
@@ -228,8 +229,9 @@ 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
-	if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
+	// 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) {
 		return;
 	}

@@ -1057,10 +1057,9 @@ 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 (status_sub.copy(&status)) {
+	if (_vehicle_status_sub.copy(&status)) {
 		mavlink_autopilot_version_t msg{};

 		msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT;
@@ -1690,7 +1689,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		break;

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

 	case MAVLINK_MODE_MINIMAL:
@@ -1844,7 +1843,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:fswxzZp", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:F:fswxzZp", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'b':
 			if (px4_get_parameter_value(myoptarg, _baudrate) != 0) {
@@ -2032,6 +2031,26 @@ 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;
@@ -2236,12 +2255,18 @@ 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;
 		}

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

 		configure_sik_radio();

-		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);
-		}
+		handleStatus();
+		handleCommands();
+		handleAndGetCurrentCommandAck(cmd_logging_start_acknowledgement, cmd_logging_stop_acknowledgement);

 		/* check for shell output */
 		if (_mavlink_shell && _mavlink_shell->available() > 0) {
@@ -2583,6 +2460,170 @@ 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) {
@@ -2883,6 +2924,11 @@ 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);

@@ -3263,6 +3309,7 @@ $ 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,6 +584,7 @@ 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};
@@ -707,6 +708,12 @@ 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
 	 *
@@ -469,6 +469,25 @@ 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;
@@ -32,6 +32,10 @@ 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
@@ -178,3 +182,22 @@ 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,6 +53,7 @@
 #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>
@@ -125,18 +126,21 @@ private:
 			updated |= _temperature.valid();
 			updated |= _throttle.valid();
 			updated |= _windspeed.valid();
-			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_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_failsafe_flags(&msg);
-			updated |= write_wind(&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);

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

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

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

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

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

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

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

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

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

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

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

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

-	bool write_global_position(mavlink_high_latency2_t *msg)
+	bool write_global_position_if_updated(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.update(&local_pos)) {
+		if (_global_pos_sub.update(&global_pos) && _local_pos_sub.copy(&local_pos)) {
 			msg->latitude = global_pos.lat * 1e7;
 			msg->longitude = global_pos.lon * 1e7;

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

 			msg->altitude = altitude;

-			msg->heading = static_cast<uint8_t>(math::degrees(matrix::wrap_2pi(local_pos.heading)) * 0.5f);
+			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);

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

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

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

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

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

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

-		if (_wind_sub.update(&wind)) {
+		if (_wind_sub.copy(&wind)) {
 			msg->wind_heading = static_cast<uint8_t>(math::degrees(matrix::wrap_2pi(atan2f(wind.windspeed_east,
 					    wind.windspeed_north))) * 0.5f);
 			return true;
@@ -625,6 +643,7 @@ 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,6 +172,7 @@ 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,6 +108,7 @@ 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,6 +855,7 @@ 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,6 +267,7 @@ 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;
@@ -276,7 +277,22 @@ void Navigator::run()
 					position_setpoint.lon = get_global_position()->lon;
 				}

-				position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt;
+				// 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;
+				}

 				if (geofence_allows_position(position_setpoint)) {
 					position_setpoint_triplet_s *rep = get_reposition_triplet();
@@ -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 float output_offset = 100.0f;
-			float scaled_output = (float)outputs[i] - output_offset;
+			static constexpr double output_offset = 100.0;
+			double scaled_output = (double)outputs[i] - output_offset;
 			wheel_velocity_message.set_velocity(i, scaled_output);
 		}

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

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

-	wait_for_mission_finished(std::chrono::seconds(90));
+	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)));
 }

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

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

-	wait_for_mission_raw_finished(std::chrono::seconds(120));
+	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)));
 }

 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_straight_south.plan");
+	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission.plan");
 	tester.arm();

 	tester.takeoff();
@@ -49,7 +49,7 @@
                    0,
                    0,
                    null,
-                    47.38497919751799,
+                    47.392,
                    8.54578066404548,
                    30
                ],
@@ -142,13 +142,13 @@ parameters:
                            # (Extend this list as needed)
                            type: string
                            allowed: [
-                                '%', 'Hz', 'mAh',
+                                '%', 'Hz', '1/s', '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',
+                                'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad', 'g0',
                                'Ohm', 'V', 'A',
                                'us', 'ms', 's',
                                'S', 'A/%', '(m/s^2)^2',  'm/m',  'tan(rad)^2', '(m/s)^2', 'm/rad',
Author	SHA1	Message	Date
Matthias Grob	1d5d96e8f9	mavlink: return unsupported frame result when that's the case	2024-05-02 17:32:37 +02:00
Matthias Grob	3b21f3999f	mavlink: spacing, naming of command frame handling	2024-05-02 17:30:59 +02:00
Matthias Grob	6dbf39be7c	Update MAVLink to contain _INT frame deprecation warning	2024-05-02 17:19:31 +02:00
Daniel Agar	3bd14fcb16	mavlink: COMMAND_INT receiver reject unsupported MAV_FRAMEs	2024-05-02 16:49:37 +02:00
Daniel Agar	ed35155b50	vehicle_command: add initial frame support (GLOBAL & GLOBAL_RELATIVE_ALT)	2024-05-02 16:49:37 +02:00
David Sidrane	c13e3bae12	px4_fmu-v6xrt:Support_MMCSD_MULTIBLOCK with preflight	2024-05-01 20:45:42 -04:00
David Sidrane	a3e1dcce4b	NuttX with imxrt_sd-preflight backport	2024-05-01 20:45:42 -04:00
Daniel Agar	33234f4dc0	drivers/ins/vectornav: add missing sensor_gps velocity magnitude	2024-05-01 20:40:28 -04:00
Julian Oes	e79993a316	gps: split enum after rebase Signed-off-by: Julian Oes <julian@oes.ch>	2024-05-01 14:42:58 -04:00
alexklimaj	b308c4fbcc	boards: ark rtk gps safety led open drain	2024-05-01 14:42:58 -04:00
alexklimaj	c90ccabbe0	gps: add ZED-F9P-15B	2024-05-01 14:42:58 -04:00
alexklimaj	2498ce6a5c	boards: add iis2mdc mag to ark pi6x	2024-04-30 21:26:42 -04:00
alexklimaj	67b39314bf	boards: update ark_pi6x EKF delays	2024-04-30 21:26:42 -04:00
Daniel Agar	b6da0b141d	ekf2: GPS yaw only invalidate yaw_align if stopping due to fusion failure	2024-04-29 21:04:21 -04:00
Beat Küng	547209e1dc	libevents: update submodule And remove obsolete libevents_definitions.h	2024-04-29 07:22:40 -07:00
bresch	9dc7719d4a	ekf2: Only reset to GNSS heading if necessary When North-East (e.g.: GNSS pos/vel) aiding is active, the heading estimate is constrained and consistent with the vel/pos aiding. Reset to GNSS heading should only occur if no N-E aiding is active or if the filter is not yes aligned. Otherwise, just wait for the consistency check to pass again (will pass at some point if the heading uncertainty of the filter is getting too high).	2024-04-29 07:22:01 -07:00
Jukka Laitinen	6435e25929	commander: Use PX4_STACK_ADJUSTED to increase stack for 64-bit targets Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2024-04-29 08:11:54 +02:00
Hamish Willee	902712b97f	LogMessage.msg - expand out descriptive string (#23054 )	2024-04-29 14:02:09 +12:00
Peter van der Perk	500332e424	imxrt: flexpwm remove 1:1 mapping requirement	2024-04-27 07:57:32 -04:00
Silvan Fuhrer	34cb69898e	FW Position Controller: fix Altitude mode without valid z reference (e.g. no GPS) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-04-26 10:44:40 +02:00
oravla5	f91103af6e	boards: removed CONFIG_SYSTEMCMDS_REFLECT from Sky-Drones AIRLink board support	2024-04-26 09:42:06 +02:00
oravla5	9d6ac0b87a	mavlink: Added MAV_{i}_HL_FREQ parameter	2024-04-26 09:42:06 +02:00
oravla5	2b2e1c9521	mavlink: Added parsing of CLI option to configure HL frequency	2024-04-26 09:42:06 +02:00
oravla5	e7b4c5903f	px4_cli: Added px4_get_parameter_value function overload for float type	2024-04-26 09:42:06 +02:00
bresch	7cefc3172a	estimatorCheck: get param only if handle is valid	2024-04-25 14:48:23 -04:00
murata,katsutoshi	ba448fb549	MC Auto: add fixed yaw mode	2024-04-25 13:53:05 +02:00
oravla5	98b23e41f7	mavlink: fixed compilation error after var renaming	2024-04-25 08:23:32 +02:00
oravla5	283ae60a15	telemetry: removed iridium driver	2024-04-25 08:23:32 +02:00
oravla5	3cb48feb61	high_latency_stream: minor PR fix	2024-04-25 08:23:32 +02:00
oravla5	bf1266af11	mavlink: added back gimbal v1 protocol command	2024-04-25 08:23:32 +02:00
oravla5	03652beef8	commander: fixed format	2024-04-25 08:23:32 +02:00
oravla5	d0e7f2c368	high_latency_stream: heading taken from vehicle_attitude topic	2024-04-25 08:23:32 +02:00
Igor Mišić	d0532f45b2	telemetry: enable iridium	2024-04-25 08:23:32 +02:00
Igor Mišić	61ca65d863	mavlink: use high_latency_data_link_lost as backup to normal data_link	2024-04-25 08:23:32 +02:00
Igor Mišić	4f8de500af	iridiumsbd: update logic for detecting if the modem is not responsive	2024-04-25 08:23:32 +02:00
Igor Mišić	5be0adc779	mavlink: don't send command ACK for internal commands over Iridium	2024-04-25 08:23:32 +02:00
Igor Mišić	29af189cd0	mavlink: don't send events over Iridium	2024-04-25 08:23:32 +02:00
Igor Mišić	208909d471	mavlink: use high_latency_data_link_lost as backup to normal data_link	2024-04-25 08:23:32 +02:00
Igor Mišić	de23c10b68	commander: improve handling high latency link lost/regain	2024-04-25 08:23:32 +02:00
Igor Mišić	d3b853a7f9	mavlink: fix handling of transmission enable/disable Co-authored-by: RomanBapst <bapstroman@gmail.com>	2024-04-25 08:23:32 +02:00
Igor Mišić	760bcdec2f	high_latency_stream: fixed bug where fields were not updating - topic update was checked twice in the same loop and thus the second time the topic would never indicate to have updated Co-authored-by: RomanBapst <bapstroman@gmail.com>	2024-04-25 08:23:32 +02:00
Igor Mišić	df2cc4af05	commander: fix check for availability of high latency link Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-04-25 08:23:32 +02:00
Beat Küng	f543951e10	commander: set correct health component when reporting errors	2024-04-24 16:06:32 -04:00
Jacob Dahl	69e082c83d	drivers/magnetometer: new ST IIS2MDC Magnetometer driver	2024-04-24 13:01:18 -07:00
dirksavage88	6a3e57d428	Shift vertical orientation above scaling yaw operation, cp angle sign change Signed-off-by: dirksavage88 <dirksavage88@gmail.com>	2024-04-24 15:58:12 -04:00
dirksavage88	0f6f4c5b07	fix to orientation offsets for scaled yaw, removed unused param Signed-off-by: dirksavage88 <dirksavage88@gmail.com>	2024-04-24 15:58:12 -04:00
Thomas Frans	65c7e034dc	VSCode: add EditorConfig extension to recommended and devcontainer.json	2024-04-24 15:52:23 -04:00
Daniel Agar	eb59bb9de9	simulation/gz_bridge: eliminate implicit float conversion	2024-04-24 15:51:30 -04:00
bresch	b508df39a2	imu consistency: don't scale param threshold	2024-04-24 15:51:07 -04:00
bresch	8bf1cf0b15	ekf2_params: reduce "short" description	2024-04-24 15:09:57 -04:00
Silvan Fuhrer	97191bd60f	autopilot_tester: for mission end timeout check take speed factor into account And increase the (simulation time) timeouts. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-04-23 16:37:31 +02:00
Silvan Fuhrer	818e318334	autopilot_tester: reduce mission distance for wind world Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-04-23 16:37:31 +02:00
Silvan Fuhrer	59232c27ae	autopilot_tester: use normal VTOL mission for airspeed blockage test Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-04-23 16:37:31 +02:00
fury1895	2683128205	PM Selector Auterion: remove INA226_SHUNT value reset (skynode only)	2024-04-23 16:02:35 +02:00