Merge remote-tracking branch 'px4/main' into pr-ekf2_external_vision_control_refactor

2026-05-02 05:04:08 +08:00 · 2022-11-08 12:10:53 -05:00 · 2022-11-08 12:10:53 -05:00 · 77042cf6d7
commit 77042cf6d7
parent 2ab87d029d 5239993c88
84 changed files with 936 additions and 364 deletions
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert
@ -14,7 +14,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.3
+param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5

--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter
@ -70,7 +70,7 @@ param set-default MPC_XY_VEL_D_ACC 0.1
 param set-default NAV_ACC_RAD 5

 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.5
+param set-default VT_FW_DIFTHR_S_Y 0.5
 param set-default VT_F_TRANS_DUR 1.5
 param set-default VT_F_TRANS_THR 0.7
 param set-default VT_TYPE 0
--- a/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil
+++ b/ROMFS/px4fmu_common/init.d/airframes/1102_tailsitter_duo_sih.hil
@ -22,7 +22,7 @@ param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0
 param set-default VT_FW_DIFTHR_EN 1
-param set-default VT_FW_DIFTHR_SC 0.3
+param set-default VT_FW_DIFTHR_S_Y 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5

--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@ -191,7 +191,10 @@ then
 	ist8308 -X -q start
 	ist8310 -X -q start
 	lis2mdl -X -q start
-	lis3mdl -X -q start
+	if ! lis3mdl -X -q start
+	then
+		lis3mdl -X -q -a 0x1c start
+	fi
 	qmc5883l -X -q start
 	rm3100 -X -q start

--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@ -582,7 +582,35 @@ class uploader(object):
        self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE)

    # upload the firmware
-    def upload(self, fw, force=False, boot_delay=None):
+    def upload(self, fw_list, force=False, boot_delay=None, boot_check=False):
+        # select correct binary
+        found_suitable_firmware = False
+        for file in fw_list:
+            fw = firmware(file)
+            if self.board_type == fw.property('board_id'):
+                if len(fw_list) > 1: print("using firmware binary {}".format(file))
+                found_suitable_firmware = True
+                break
+
+        if not found_suitable_firmware:
+            msg = "Firmware not suitable for this board (Firmware board_type=%u board_id=%u)" % (
+                self.board_type, fw.property('board_id'))
+            print("WARNING: %s" % msg)
+            if force:
+                if len(fw_list) > 1:
+                    raise FirmwareNotSuitableException("force flashing failed, more than one file provided, none suitable")
+                print("FORCED WRITE, FLASHING ANYWAY!")
+            else:
+                raise FirmwareNotSuitableException(msg)
+
+        percent = fw.property('image_size') / fw.property('image_maxsize')
+        binary_size = float(fw.property('image_size'))
+        binary_max_size = float(fw.property('image_maxsize'))
+        percent = (binary_size / binary_max_size) * 100
+
+        print("Loaded firmware for board id: %s,%s size: %d bytes (%.2f%%) " % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'), percent))
+        print()
+
        # Make sure we are doing the right thing
        start = _time()
        if self.board_type != fw.property('board_id'):
@ -764,7 +792,7 @@ def main():
    parser.add_argument('--force', action='store_true', default=False, help='Override board type check, or silicon errata checks and continue loading')
    parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash')
    parser.add_argument('--use-protocol-splitter-format', action='store_true', help='use protocol splitter format for reboot')
-    parser.add_argument('firmware', action="store", help="Firmware file to be uploaded")
+    parser.add_argument('firmware', action="store", nargs='+', help="Firmware file(s)")
    args = parser.parse_args()

    if args.use_protocol_splitter_format:
@ -776,17 +804,7 @@ def main():
        print("WARNING: You should uninstall ModemManager as it conflicts with any non-modem serial device (like Pixhawk)")
        print("==========================================================================================================")

-    # Load the firmware file
-    fw = firmware(args.firmware)
-
-    percent = fw.property('image_size') / fw.property('image_maxsize')
-    binary_size = float(fw.property('image_size'))
-    binary_max_size = float(fw.property('image_maxsize'))
-    percent = (binary_size / binary_max_size) * 100
-
-    print("Loaded firmware for board id: %s,%s size: %d bytes (%.2f%%), waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'), fw.property('image_size'), percent))
-    print()
-
+    print("Waiting for bootloader...")
    # tell any GCS that might be connected to the autopilot to give up
    # control of the serial port

@ -889,7 +907,7 @@ def main():

                try:
                    # ok, we have a bootloader, try flashing it
-                    up.upload(fw, force=args.force, boot_delay=args.boot_delay)
+                    up.upload(args.firmware, force=args.force, boot_delay=args.boot_delay)

                    # if we made this far without raising exceptions, the upload was successful
                    successful = True
--- a/Tools/simulation/gazebo/sitl_gazebo
+++ b/Tools/simulation/gazebo/sitl_gazebo
@ -1 +1 @@
-Subproject commit e80432759540c91f85a012f47aa6ebb0ee9de7e4
+Subproject commit 56b5508b72f40339448f3525dd4dc51f30512cbd
--- a/boards/nxp/ucans32k146/src/board_config.h
+++ b/boards/nxp/ucans32k146/src/board_config.h
@ -99,14 +99,14 @@ __BEGIN_DECLS

 /* Timer I/O PWM and capture
 *
- * ?? PWM outputs are configured.
+ * 2 PWM outputs are configured.
 * ?? Timer inputs are configured.
 *
 * Pins:
 *      Defined in board.h
 */

-#define DIRECT_PWM_OUTPUT_CHANNELS  1
+#define DIRECT_PWM_OUTPUT_CHANNELS  2


 #define BOARD_HAS_LED_PWM              1
--- a/boards/nxp/ucans32k146/src/timer_config.cpp
+++ b/boards/nxp/ucans32k146/src/timer_config.cpp
@ -105,11 +105,13 @@
 #define rPWMLOAD(t)    REG(t,S32K1XX_FTM_PWMLOAD_OFFSET)

 constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
+	initIOTimer(Timer::FTM1),
 	initIOTimer(Timer::FTM2),
 };

 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 	initIOTimerChannel(io_timers, {Timer::FTM2, Timer::Channel1}, {GPIO::PortA, GPIO::Pin0}),
+	initIOTimerChannel(io_timers, {Timer::FTM1, Timer::Channel1}, {GPIO::PortA, GPIO::Pin1}),
 };

 constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
@ -154,17 +156,26 @@ void ucans32k_timer_initialize(void)
 	regval |= FTM_SC_CLKS_FTM;
 	_REG(S32K1XX_FTM0_SC) = regval;

+	regval = _REG(S32K1XX_FTM1_SC);
+	regval &= ~(FTM_SC_CLKS_MASK);
+	regval |= FTM_SC_CLKS_FTM;
+	_REG(S32K1XX_FTM1_SC) = regval;
+
 	regval = _REG(S32K1XX_FTM2_SC);
 	regval &= ~(FTM_SC_CLKS_MASK);
 	regval |= FTM_SC_CLKS_FTM;
 	_REG(S32K1XX_FTM2_SC) = regval;

-	/* Enabled System Clock Gating Control for FTM0, and FTM2 */
+	/* Enabled System Clock Gating Control for FTM0, FTM1 and FTM2 */

 	regval = _REG(S32K1XX_PCC_FTM0);
 	regval |= PCC_CGC;
 	_REG(S32K1XX_PCC_FTM0) = regval;

+	regval = _REG(S32K1XX_PCC_FTM1);
+	regval |= PCC_CGC;
+	_REG(S32K1XX_PCC_FTM1) = regval;
+
 	regval = _REG(S32K1XX_PCC_FTM2);
 	regval |= PCC_CGC;
 	_REG(S32K1XX_PCC_FTM2) = regval;
--- a/boards/px4/fmu-v6x/default.px4board
+++ b/boards/px4/fmu-v6x/default.px4board
@ -74,6 +74,7 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
--- a/msg/ControlAllocatorStatus.msg
+++ b/msg/ControlAllocatorStatus.msg
@ -1,12 +1,10 @@
 uint64 timestamp                        # time since system start (microseconds)

 bool torque_setpoint_achieved           # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
-float32[3] allocated_torque             # Torque allocated to actuators. Equal to `vehicle_torque_setpoint_s::xyz` if the setpoint was achieved.
 float32[3] unallocated_torque           # Unallocated torque. Equal to 0 if the setpoint was achieved.
                                        # Computed as: unallocated_torque = torque_setpoint - allocated_torque

 bool thrust_setpoint_achieved           # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
-float32[3] allocated_thrust             # Thrust allocated to actuators. Equal to `vehicle_thrust_setpoint_s::xyz` if the setpoint was achieved.
 float32[3] unallocated_thrust           # Unallocated thrust. Equal to 0 if the setpoint was achieved.
                                        # Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust

--- a/msg/VehicleAttitudeSetpoint.msg
+++ b/msg/VehicleAttitudeSetpoint.msg
@ -13,7 +13,7 @@ float32[4] q_d			# Desired quaternion for quaternion control
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
 float32[3] thrust_body		# Normalized thrust command in body NED frame [-1,1]

-bool reset_rate_integrals	# Reset roll/pitch/yaw integrals (navigation logic change)
+bool reset_integral	# Reset roll/pitch/yaw integrals (navigation logic change)

 bool fw_control_yaw		# control heading with rudder (used for auto takeoff on runway)

--- a/msg/VehicleLandDetected.msg
+++ b/msg/VehicleLandDetected.msg
@ -12,6 +12,7 @@ bool has_low_throttle

 bool vertical_movement
 bool horizontal_movement
+bool rotational_movement

 bool close_to_ground_or_skipped_check

--- a/msg/VehicleRatesSetpoint.msg
+++ b/msg/VehicleRatesSetpoint.msg
@ -8,3 +8,5 @@ float32 yaw		# [rad/s] yaw rate setpoint
 # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand.
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
 float32[3] thrust_body	# Normalized thrust command in body NED frame [-1,1]
+
+bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change)
--- a/platforms/common/include/px4_platform_common/defines.h
+++ b/platforms/common/include/px4_platform_common/defines.h
@ -136,5 +136,9 @@ __END_DECLS
 #define M_LOG2_E_F		0.69314718f
 #define M_INVLN2_F		1.44269504f	// 1 / log(2)

+/* The M_PI, as stated above, is not C standard. If you need it and
+ * it isn't in your math.h file then you can use this instead. */
+#define M_PI_PRECISE	3.141592653589793238462643383279502884
+
 #define M_DEG_TO_RAD 		0.017453292519943295
 #define M_RAD_TO_DEG 		57.295779513082323
--- a/platforms/common/spi.cpp
+++ b/platforms/common/spi.cpp
@ -47,7 +47,7 @@ void px4_set_spi_buses_from_hw_version()
 #if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
 	int hw_version_revision = board_get_hw_version();
 #else
-	int hw_version_revision = (board_get_hw_version() << 16) | board_get_hw_revision();
+	int hw_version_revision = HW_VER_REV(board_get_hw_version(), board_get_hw_revision());
 #endif


--- a/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h
+++ b/platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/io_timer.h
@ -45,8 +45,8 @@
 #pragma once
 __BEGIN_DECLS
 /* configuration limits */
-#define MAX_IO_TIMERS			1
-#define MAX_TIMER_IO_CHANNELS		1
+#define MAX_IO_TIMERS			2
+#define MAX_TIMER_IO_CHANNELS		2

 #define MAX_LED_TIMERS			1
 #define MAX_TIMER_LED_CHANNELS		3
--- a/platforms/qurt/cmake/px4_impl_os.cmake
+++ b/platforms/qurt/cmake/px4_impl_os.cmake
@ -126,6 +126,7 @@ function(px4_os_add_flags)

 		-Wno-unknown-warning-option
 		-Wno-cast-align
+		--include=${PX4_SOURCE_DIR}/platforms/qurt/include/qurt_reqs.h
 	)

 	# Clear -rdynamic flag which fails for hexagon
--- a/platforms/qurt/include/px4_arch/micro_hal.h
+++ b/platforms/qurt/include/px4_arch/micro_hal.h
@ -0,0 +1,35 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 ModalAI, Inc. 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.
+ *
+ ****************************************************************************/
+
+// Placeholder
+
--- a/platforms/qurt/include/qurt_reqs.h
+++ b/platforms/qurt/include/qurt_reqs.h
@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 ModalAI, Inc. 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.
+ *
+ ****************************************************************************/
+
+// This file is meant to tackle the dependencies found in PX4
+// that have not been implemented in the Hexagon SDK yet.
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef unsigned long useconds_t;
+int usleep(useconds_t usec);
+
+#ifdef __cplusplus
+}
+#endif
--- a/platforms/qurt/src/px4/CMakeLists.txt
+++ b/platforms/qurt/src/px4/CMakeLists.txt
@ -33,9 +33,12 @@

 # Placeholder
 set(QURT_LAYER_SRCS
+	drv_hrt.cpp
 	tasks.cpp
 	)

 add_library(px4_layer
        ${QURT_LAYER_SRCS}
 )
+
+target_link_libraries(px4_layer PRIVATE work_queue)
--- a/platforms/qurt/src/px4/drv_hrt.cpp
+++ b/platforms/qurt/src/px4/drv_hrt.cpp
@ -0,0 +1,326 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 ModalAI, Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/time.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/workqueue.h>
+#include <drivers/drv_hrt.h>
+
+#include <semaphore.h>
+#include <time.h>
+#include <string.h>
+#include <errno.h>
+
+#include "hrt_work.h"
+
+static constexpr unsigned HRT_INTERVAL_MIN = 50;
+static constexpr unsigned HRT_INTERVAL_MAX = 50000000;
+
+static struct sq_queue_s	callout_queue;
+
+static uint64_t			latency_baseline;
+
+static uint64_t			latency_actual;
+
+const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
+const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 };
+__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1];
+
+static px4_sem_t 	_hrt_lock;
+static struct work_s	_hrt_work;
+
+static int32_t dsp_offset = 0;
+
+static void hrt_latency_update();
+
+static void hrt_call_reschedule();
+static void hrt_call_invoke();
+
+hrt_abstime hrt_absolute_time_offset()
+{
+	return 0;
+}
+
+static void hrt_lock()
+{
+	px4_sem_wait(&_hrt_lock);
+}
+
+static void hrt_unlock()
+{
+	px4_sem_post(&_hrt_lock);
+}
+
+int px4_clock_settime(clockid_t clk_id, struct timespec *tp)
+{
+	return 0;
+}
+
+int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
+{
+	int rv = clock_gettime(clk_id, tp);
+	hrt_abstime temp_abstime = ts_to_abstime(tp);
+
+	if (dsp_offset < 0) {
+		hrt_abstime temp_offset = -dsp_offset;
+
+		if (temp_offset >= temp_abstime) { temp_abstime = 0; }
+
+		else { temp_abstime -= temp_offset; }
+
+	} else {
+		temp_abstime += (hrt_abstime) dsp_offset;
+	}
+
+	tp->tv_sec = temp_abstime / 1000000;
+	tp->tv_nsec = (temp_abstime % 1000000) * 1000;
+	return rv;
+}
+
+hrt_abstime hrt_absolute_time()
+{
+	struct timespec ts;
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts);
+	return ts_to_abstime(&ts);
+}
+
+int hrt_set_absolute_time_offset(int32_t time_diff_us)
+{
+	dsp_offset = time_diff_us;
+	return 0;
+}
+
+hrt_abstime hrt_elapsed_time_atomic(const volatile hrt_abstime *then)
+{
+	hrt_abstime delta = hrt_absolute_time() - *then;
+	return delta;
+}
+
+void hrt_store_absolute_time(volatile hrt_abstime *t)
+{
+	*t = hrt_absolute_time();
+}
+
+bool	hrt_called(struct hrt_call *entry)
+{
+	return (entry->deadline == 0);
+}
+
+void	hrt_cancel(struct hrt_call *entry)
+{
+	hrt_lock();
+	sq_rem(&entry->link, &callout_queue);
+	entry->deadline = 0;
+	entry->period = 0;
+	hrt_unlock();
+}
+
+static void hrt_latency_update()
+{
+	uint16_t latency = latency_actual - latency_baseline;
+	unsigned	index;
+
+	for (index = 0; index < LATENCY_BUCKET_COUNT; index++) {
+		if (latency <= latency_buckets[index]) {
+			latency_counters[index]++;
+			return;
+		}
+	}
+
+	latency_counters[index]++;
+}
+
+void	hrt_call_init(struct hrt_call *entry)
+{
+	memset(entry, 0, sizeof(*entry));
+}
+
+void	hrt_call_delay(struct hrt_call *entry, hrt_abstime delay)
+{
+	entry->deadline = hrt_absolute_time() + delay;
+}
+
+void	hrt_init()
+{
+	sq_init(&callout_queue);
+
+	int sem_ret = px4_sem_init(&_hrt_lock, 0, 1);
+
+	if (sem_ret) {
+		PX4_ERR("SEM INIT FAIL: %s", strerror(errno));
+	}
+
+	memset(&_hrt_work, 0, sizeof(_hrt_work));
+}
+
+static void
+hrt_call_enter(struct hrt_call *entry)
+{
+	struct hrt_call	*call, *next;
+
+	call = (struct hrt_call *)sq_peek(&callout_queue);
+
+	if ((call == nullptr) || (entry->deadline < call->deadline)) {
+		sq_addfirst(&entry->link, &callout_queue);
+		hrt_call_reschedule();
+
+	} else {
+		do {
+			next = (struct hrt_call *)sq_next(&call->link);
+
+			if ((next == nullptr) || (entry->deadline < next->deadline)) {
+				//lldbg("call enter after head\n");
+				sq_addafter(&call->link, &entry->link, &callout_queue);
+				break;
+			}
+		} while ((call = next) != nullptr);
+	}
+}
+
+static void
+hrt_tim_isr(void *p)
+{
+	latency_actual = hrt_absolute_time();
+	hrt_latency_update();
+	hrt_call_invoke();
+	hrt_lock();
+	hrt_call_reschedule();
+	hrt_unlock();
+}
+
+static void
+hrt_call_reschedule()
+{
+	hrt_abstime	now = hrt_absolute_time();
+	hrt_abstime	delay = HRT_INTERVAL_MAX;
+	struct hrt_call	*next = (struct hrt_call *)sq_peek(&callout_queue);
+	hrt_abstime	deadline = now + HRT_INTERVAL_MAX;
+
+	if (next != nullptr) {
+		if (next->deadline <= (now + HRT_INTERVAL_MIN)) {
+			delay = HRT_INTERVAL_MIN;
+
+		} else if (next->deadline < deadline) {
+			delay = next->deadline - now;
+		}
+	}
+
+	latency_baseline = now + delay;
+	hrt_work_cancel(&_hrt_work);
+	hrt_work_queue(&_hrt_work, (worker_t)&hrt_tim_isr, nullptr, delay);
+}
+
+static void
+hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	PX4_DEBUG("hrt_call_internal deadline=%lu interval = %lu", deadline, interval);
+	hrt_lock();
+
+	if (entry->deadline != 0) {
+		sq_rem(&entry->link, &callout_queue);
+	}
+
+	entry->deadline = deadline;
+	entry->period = interval;
+	entry->callout = callout;
+	entry->arg = arg;
+
+	hrt_call_enter(entry);
+	hrt_unlock();
+}
+
+void	hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  0,
+			  callout,
+			  arg);
+}
+
+void	hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry,
+			  hrt_absolute_time() + delay,
+			  interval,
+			  callout,
+			  arg);
+}
+
+void	hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg)
+{
+	hrt_call_internal(entry, calltime, 0, callout, arg);
+}
+
+static void
+hrt_call_invoke()
+{
+	struct hrt_call	*call;
+	hrt_abstime deadline;
+
+	hrt_lock();
+
+	while (true) {
+		hrt_abstime now = hrt_absolute_time();
+
+		call = (struct hrt_call *)sq_peek(&callout_queue);
+
+		if (call == nullptr) {
+			break;
+		}
+
+		if (call->deadline > now) {
+			break;
+		}
+
+		sq_rem(&call->link, &callout_queue);
+		deadline = call->deadline;
+		call->deadline = 0;
+
+		if (call->callout) {
+			hrt_unlock();
+			call->callout(call->arg);
+			hrt_lock();
+		}
+
+		if (call->period != 0) {
+			if (call->deadline <= now) {
+				call->deadline = deadline + call->period;
+			}
+
+			hrt_call_enter(call);
+		}
+	}
+
+	hrt_unlock();
+}
--- a/platforms/qurt/src/px4/tasks.cpp
+++ b/platforms/qurt/src/px4/tasks.cpp
@ -35,8 +35,11 @@
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/tasks.h>
+#include <px4_platform_common/workqueue.h>

+#include <drivers/drv_hrt.h>
 #include <pthread.h>
+#include "hrt_work.h"

 #define PX4_TASK_STACK_SIZE 8192
 #define PX4_TASK_MAX_NAME_LENGTH 32
@ -338,25 +341,25 @@ const char *px4_get_taskname()

 }

-// static void timer_cb(void *data)
-// {
-// 	px4_sem_t *sem = reinterpret_cast<px4_sem_t *>(data);
+static void timer_cb(void *data)
+{
+	px4_sem_t *sem = reinterpret_cast<px4_sem_t *>(data);

-// 	sem_post(sem);
-// }
+	sem_post(sem);
+}

 int px4_sem_timedwait(px4_sem_t *sem, const struct timespec *ts)
 {
-	// work_s _hpwork = {};
+	work_s _hpwork = {};

-	// struct timespec ts_now;
-	// px4_clock_gettime(CLOCK_MONOTONIC, &ts_now);
+	struct timespec ts_now;
+	px4_clock_gettime(CLOCK_MONOTONIC, &ts_now);

-	// hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now);
+	hrt_abstime timeout_us = ts_to_abstime((struct timespec *)ts) - ts_to_abstime(&ts_now);

-	// hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us);
-	// sem_wait(sem);
-	// hrt_work_cancel(&_hpwork);
+	hrt_work_queue(&_hpwork, (worker_t)&timer_cb, (void *)sem, timeout_us);
+	sem_wait(sem);
+	hrt_work_cancel(&_hpwork);
 	return 0;
 }

--- a/src/drivers/gps/devices
+++ b/src/drivers/gps/devices
@ -1 +1 @@
-Subproject commit 1ff87868f6008f06e2033ee05dd904ec54109e52
+Subproject commit fa2177d690207e42e0d8c92e9663578340d44fe4
--- a/src/drivers/imu/invensense/icm20649/InvenSense_ICM20649_registers.hpp
+++ b/src/drivers/imu/invensense/icm20649/InvenSense_ICM20649_registers.hpp
@ -41,6 +41,7 @@
 #pragma once

 #include <cstdint>
+#include <cstdlib>

 namespace InvenSense_ICM20649
 {
--- a/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp
+++ b/src/drivers/imu/invensense/icm42670p/InvenSense_ICM42670P_registers.hpp
@ -41,6 +41,7 @@
 #pragma once

 #include <cstdint>
+#include <cstdlib>

 namespace InvenSense_ICM42670P
 {
--- a/src/drivers/magnetometer/lis2mdl/lis2mdl.h
+++ b/src/drivers/magnetometer/lis2mdl/lis2mdl.h
@ -80,8 +80,8 @@
 #define CFG_REG_C_BDU                   (1 << 4) /* avoids reading of incorrect data due to async reads */

 /* interface factories */
-extern device::Device *LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
-extern device::Device *LIS2MDL_I2C_interface(int bus, int bus_frequency);
+extern device::Device *LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config);
+extern device::Device *LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config);

 #define LIS2MDLL_ADDRESS        0x1e

--- a/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp
+++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_i2c.cpp
@ -56,7 +56,7 @@
 class LIS2MDL_I2C : public device::I2C
 {
 public:
-	LIS2MDL_I2C(int bus, int bus_frequency);
+	LIS2MDL_I2C(const I2CSPIDriverConfig &config);
 	virtual ~LIS2MDL_I2C() = default;

 	virtual int     read(unsigned address, void *data, unsigned count);
@ -68,16 +68,16 @@ protected:
 };

 device::Device *
-LIS2MDL_I2C_interface(int bus, int bus_frequency);
+LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config);

 device::Device *
-LIS2MDL_I2C_interface(int bus, int bus_frequency)
+LIS2MDL_I2C_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS2MDL_I2C(bus, bus_frequency);
+	return new LIS2MDL_I2C(config);
 }

-LIS2MDL_I2C::LIS2MDL_I2C(int bus, int bus_frequency) :
-	I2C(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_I2C", bus, LIS2MDLL_ADDRESS, bus_frequency)
+LIS2MDL_I2C::LIS2MDL_I2C(const I2CSPIDriverConfig &config) :
+	I2C(config)
 {
 }

--- a/src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp
+++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_main.cpp
@ -46,10 +46,10 @@ I2CSPIDriverBase *LIS2MDL::instantiate(const I2CSPIDriverConfig &config, int run
 	device::Device *interface = nullptr;

 	if (config.bus_type == BOARD_I2C_BUS) {
-		interface = LIS2MDL_I2C_interface(config.bus, config.bus_frequency);
+		interface = LIS2MDL_I2C_interface(config);

 	} else if (config.bus_type == BOARD_SPI_BUS) {
-		interface = LIS2MDL_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
+		interface = LIS2MDL_SPI_interface(config);
 	}

 	if (interface == nullptr) {
@ -94,6 +94,7 @@ extern "C" int lis2mdl_main(int argc, char *argv[])
 	using ThisDriver = LIS2MDL;
 	int ch;
 	BusCLIArguments cli{true, true};
+	cli.i2c_address = LIS2MDLL_ADDRESS;
 	cli.default_i2c_frequency = 400000;
 	cli.default_spi_frequency = 11 * 1000 * 1000;

@ -112,8 +113,6 @@ extern "C" int lis2mdl_main(int argc, char *argv[])
 		return -1;
 	}

-	cli.i2c_address = LIS2MDLL_ADDRESS;
-
 	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS2MDL);

 	if (!strcmp(verb, "start")) {
--- a/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp
+++ b/src/drivers/magnetometer/lis2mdl/lis2mdl_spi.cpp
@ -61,7 +61,7 @@
 class LIS2MDL_SPI : public device::SPI
 {
 public:
-	LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+	LIS2MDL_SPI(const I2CSPIDriverConfig &config);
 	virtual ~LIS2MDL_SPI() = default;

 	virtual int     init();
@ -70,16 +70,16 @@ public:
 };

 device::Device *
-LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config);

 device::Device *
-LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
+LIS2MDL_SPI_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS2MDL_SPI(bus, devid, bus_frequency, spi_mode);
+	return new LIS2MDL_SPI(config);
 }

-LIS2MDL_SPI::LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_SPI", bus, devid, spi_mode, bus_frequency)
+LIS2MDL_SPI::LIS2MDL_SPI(const I2CSPIDriverConfig &config) :
+	SPI(config)
 {
 }

--- a/src/drivers/magnetometer/lis3mdl/lis3mdl.h
+++ b/src/drivers/magnetometer/lis3mdl/lis3mdl.h
@ -84,8 +84,8 @@
 #define CNTL_REG5_DEFAULT 0x00

 /* interface factories */
-extern device::Device *LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
-extern device::Device *LIS3MDL_I2C_interface(int bus, int bus_frequency);
+extern device::Device *LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config);
+extern device::Device *LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config);

 enum OPERATING_MODE {
 	CONTINUOUS = 0,
--- a/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp
+++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_i2c.cpp
@ -56,7 +56,7 @@
 class LIS3MDL_I2C : public device::I2C
 {
 public:
-	LIS3MDL_I2C(int bus, int bus_frequency);
+	LIS3MDL_I2C(const I2CSPIDriverConfig &config);
 	virtual ~LIS3MDL_I2C() = default;

 	virtual int     read(unsigned address, void *data, unsigned count);
@ -68,16 +68,16 @@ protected:
 };

 device::Device *
-LIS3MDL_I2C_interface(int bus, int bus_frequency);
+LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config);

 device::Device *
-LIS3MDL_I2C_interface(int bus, int bus_frequency)
+LIS3MDL_I2C_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS3MDL_I2C(bus, bus_frequency);
+	return new LIS3MDL_I2C(config);
 }

-LIS3MDL_I2C::LIS3MDL_I2C(int bus, int bus_frequency) :
-	I2C(DRV_MAG_DEVTYPE_LIS3MDL, MODULE_NAME, bus, LIS3MDLL_ADDRESS, bus_frequency)
+LIS3MDL_I2C::LIS3MDL_I2C(const I2CSPIDriverConfig &config) :
+	I2C(config)
 {
 }

--- a/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp
+++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_main.cpp
@ -46,10 +46,10 @@ I2CSPIDriverBase *LIS3MDL::instantiate(const I2CSPIDriverConfig &config, int run
 	device::Device *interface = nullptr;

 	if (config.bus_type == BOARD_I2C_BUS) {
-		interface = LIS3MDL_I2C_interface(config.bus, config.bus_frequency);
+		interface = LIS3MDL_I2C_interface(config);

 	} else if (config.bus_type == BOARD_SPI_BUS) {
-		interface = LIS3MDL_SPI_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
+		interface = LIS3MDL_SPI_interface(config);
 	}

 	if (interface == nullptr) {
@ -90,6 +90,7 @@ void LIS3MDL::print_usage()
 	PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x1e);
 	PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
 	PRINT_MODULE_USAGE_COMMAND("reset");
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
@ -100,6 +101,7 @@ extern "C" int lis3mdl_main(int argc, char *argv[])
 	using ThisDriver = LIS3MDL;
 	int ch;
 	BusCLIArguments cli{true, true};
+	cli.i2c_address = LIS3MDLL_ADDRESS;
 	cli.default_i2c_frequency = 400000;
 	cli.default_spi_frequency = 11 * 1000 * 1000;

@ -118,8 +120,6 @@ extern "C" int lis3mdl_main(int argc, char *argv[])
 		return -1;
 	}

-	cli.i2c_address = LIS3MDLL_ADDRESS;
-
 	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_LIS3MDL);

 	if (!strcmp(verb, "start")) {
--- a/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp
+++ b/src/drivers/magnetometer/lis3mdl/lis3mdl_spi.cpp
@ -61,7 +61,7 @@
 class LIS3MDL_SPI : public device::SPI
 {
 public:
-	LIS3MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+	LIS3MDL_SPI(const I2CSPIDriverConfig &config);
 	virtual ~LIS3MDL_SPI() = default;

 	virtual int     init();
@ -70,16 +70,16 @@ public:
 };

 device::Device *
-LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
+LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config);

 device::Device *
-LIS3MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
+LIS3MDL_SPI_interface(const I2CSPIDriverConfig &config)
 {
-	return new LIS3MDL_SPI(bus, devid, bus_frequency, spi_mode);
+	return new LIS3MDL_SPI(config);
 }

-LIS3MDL_SPI::LIS3MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) :
-	SPI(DRV_MAG_DEVTYPE_LIS3MDL, MODULE_NAME, bus, devid, spi_mode, bus_frequency)
+LIS3MDL_SPI::LIS3MDL_SPI(const I2CSPIDriverConfig &config) :
+	SPI(config)
 {
 }

--- a/src/drivers/osd/msp_osd/msp_osd.cpp
+++ b/src/drivers/osd/msp_osd/msp_osd.cpp
@ -121,7 +121,7 @@ MspOsd::MspOsd(const char *device) :
 	strcpy(_device, device);

 	// _is_initialized = true;
-	PX4_INFO("MSP OSD prepared to run on %s", _device);
+	PX4_INFO("MSP OSD running on %s", _device);
 }

 MspOsd::~MspOsd()
@ -504,10 +504,10 @@ int MspOsd::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-Msp OSD!
+MSP telemetry streamer

 ### Implementation
-Does the things for the DJI Air Unit OSD
+Converts uORB messages to MSP telemetry packets

 ### Examples
 CLI usage example:
--- a/src/lib/events/libevents
+++ b/src/lib/events/libevents
@ -1 +1 @@
-Subproject commit a89808bffd1efb0543e66f17a7fa12dfce5e6bf0
+Subproject commit 179f86a8fc7fd74cb80630e77b1b9435d3c5b748
--- a/src/lib/matrix/matrix/Euler.hpp
+++ b/src/lib/matrix/matrix/Euler.hpp
@ -93,11 +93,11 @@ public:
 	{
 		theta() = std::asin(-dcm(2, 0));

-		if ((std::fabs(theta() - Type(M_PI / 2))) < Type(1.0e-3)) {
+		if ((std::fabs(theta() - Type(M_PI_PRECISE / 2))) < Type(1.0e-3)) {
 			phi() = 0;
 			psi() = std::atan2(dcm(1, 2), dcm(0, 2));

-		} else if ((std::fabs(theta() + Type(M_PI / 2))) < Type(1.0e-3)) {
+		} else if ((std::fabs(theta() + Type(M_PI_PRECISE / 2))) < Type(1.0e-3)) {
 			phi() = 0;
 			psi() = std::atan2(-dcm(1, 2), -dcm(0, 2));

--- a/src/lib/matrix/matrix/helper_functions.hpp
+++ b/src/lib/matrix/matrix/helper_functions.hpp
@ -95,7 +95,7 @@ Integer wrap(Integer x, Integer low, Integer high)
 template<typename Type>
 Type wrap_pi(Type x)
 {
-	return wrap(x, Type(-M_PI), Type(M_PI));
+	return wrap(x, Type(-M_PI_PRECISE), Type(M_PI_PRECISE));
 }

 /**
@ -104,7 +104,7 @@ Type wrap_pi(Type x)
 template<typename Type>
 Type wrap_2pi(Type x)
 {
-	return wrap(x, Type(0), Type((2 * M_PI)));
+	return wrap(x, Type(0), Type((2 * M_PI_PRECISE)));
 }

 /**
@ -132,7 +132,7 @@ Type unwrap(const Type last_x, const Type new_x, const Type low, const Type high
 template<typename Type>
 Type unwrap_pi(const Type last_angle, const Type new_angle)
 {
-	return unwrap(last_angle, new_angle, Type(-M_PI), Type(M_PI));
+	return unwrap(last_angle, new_angle, Type(-M_PI_PRECISE), Type(M_PI_PRECISE));
 }

 /**
--- a/src/lib/matrix/matrix/math.hpp
+++ b/src/lib/matrix/matrix/math.hpp
@ -1,6 +1,7 @@
 #pragma once

 #include <assert.h>
+#include <px4_platform_common/defines.h>

 #include "helper_functions.hpp"

--- a/src/lib/matrix/test/MatrixAttitudeTest.cpp
+++ b/src/lib/matrix/test/MatrixAttitudeTest.cpp
@ -200,8 +200,8 @@ TEST(MatrixAttitudeTest, Attitude)
 	}

 	// constants
-	double deg2rad = M_PI / 180.0;
-	double rad2deg = 180.0 / M_PI;
+	double deg2rad = M_PI_PRECISE / 180.0;
+	double rad2deg = 180.0 / M_PI_PRECISE;

 	// euler dcm round trip check
 	for (double roll = -90; roll <= 90; roll += 90) {
@ -417,7 +417,7 @@ TEST(MatrixAttitudeTest, Attitude)
 	EXPECT_EQ(q, q_true);

 	// from axis angle, with length of vector the rotation
-	float n = float(std::sqrt(4 * M_PI * M_PI / 3));
+	float n = float(std::sqrt(4 * M_PI_F * M_PI_F / 3));
 	q = AxisAnglef(n, n, n);
 	EXPECT_EQ(q, Quatf(-1, 0, 0, 0));
 	q = AxisAnglef(0, 0, 0);
--- a/src/lib/matrix/test/MatrixHelperTest.cpp
+++ b/src/lib/matrix/test/MatrixHelperTest.cpp
@ -81,20 +81,20 @@ TEST(MatrixHelperTest, Helper)

 	// wrap pi
 	EXPECT_FLOAT_EQ(wrap_pi(0.), 0.);
-	EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI)));
-	EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI)));
+	EXPECT_FLOAT_EQ(wrap_pi(4.), (4. - (2 * M_PI_PRECISE)));
+	EXPECT_FLOAT_EQ(wrap_pi(-4.), (-4. + (2 * M_PI_PRECISE)));
 	EXPECT_FLOAT_EQ(wrap_pi(3.), 3.);
-	EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI));
-	EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI));
-	EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI));
+	EXPECT_FLOAT_EQ(wrap_pi(100.), (100. - 32. * M_PI_PRECISE));
+	EXPECT_FLOAT_EQ(wrap_pi(-100.), (-100. + 32. * M_PI_PRECISE));
+	EXPECT_FLOAT_EQ(wrap_pi(-101.), (-101. + 32. * M_PI_PRECISE));
 	EXPECT_FALSE(std::isfinite(wrap_pi(NAN)));

 	// wrap 2pi
 	EXPECT_FLOAT_EQ(wrap_2pi(0.), 0.);
-	EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI));
+	EXPECT_FLOAT_EQ(wrap_2pi(-4.), (-4. + 2. * M_PI_PRECISE));
 	EXPECT_FLOAT_EQ(wrap_2pi(3.), (3.));
-	EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI)));
-	EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI)));
+	EXPECT_FLOAT_EQ(wrap_2pi(200.), (200. - 31. * (2 * M_PI_PRECISE)));
+	EXPECT_FLOAT_EQ(wrap_2pi(-201.), (-201. + 32. * (2 * M_PI_PRECISE)));
 	EXPECT_FALSE(std::isfinite(wrap_2pi(NAN)));

 	// Equality checks
--- a/src/lib/matrix/test/MatrixUnwrapTest.cpp
+++ b/src/lib/matrix/test/MatrixUnwrapTest.cpp
@ -5,7 +5,7 @@ using namespace matrix;

 TEST(MatrixUnwrapTest, UnwrapFloats)
 {
-	const float M_TWO_PI_F = float(M_PI * 2);
+	const float M_TWO_PI_F = float(M_PI_F * 2);

 	float unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25};
 	float wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25};
@ -34,7 +34,7 @@ TEST(MatrixUnwrapTest, UnwrapFloats)

 TEST(MatrixUnwrapTest, UnwrapDoubles)
 {
-	const double M_TWO_PI = M_PI * 2;
+	const double M_TWO_PI = M_PI_PRECISE * 2;

 	double unwrapped_angles[6] = {0.0, 0.25, 0.5, 0.75, 1.0, 1.25};
 	double wrapped_angles[6] = {0.0, 0.25, 0.5, -0.25, 0.0, 0.25};
--- a/src/lib/parameters/param_translation.cpp
+++ b/src/lib/parameters/param_translation.cpp
@ -250,5 +250,14 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}

+	// 2022-07-18: translate VT_FW_DIFTHR_SC->VT_FW_DIFTHR_S_Y
+	{
+		if (strcmp("VT_FW_DIFTHR_SC", node->name) == 0) {
+			strcpy(node->name, "VT_FW_DIFTHR_S_Y");
+			PX4_INFO("copying %s -> %s", "VT_FW_DIFTHR_SC", "VT_FW_DIFTHR_S_Y");
+			return true;
+		}
+	}
+
 	return false;
 }
--- a/src/modules/angular_velocity_controller/AngularVelocityController.cpp
+++ b/src/modules/angular_velocity_controller/AngularVelocityController.cpp
@ -218,10 +218,10 @@ AngularVelocityController::Run()
 			// publish rate controller status
 			rate_ctrl_status_s rate_ctrl_status{};
 			Vector3f integral = _control.getIntegral();
-			rate_ctrl_status.timestamp = hrt_absolute_time();
 			rate_ctrl_status.rollspeed_integ = integral(0);
 			rate_ctrl_status.pitchspeed_integ = integral(1);
 			rate_ctrl_status.yawspeed_integ = integral(2);
+			rate_ctrl_status.timestamp = hrt_absolute_time();
 			_rate_ctrl_status_pub.publish(rate_ctrl_status);

 			// publish controller output
@ -238,14 +238,12 @@ void
 AngularVelocityController::publish_angular_acceleration_setpoint()
 {
 	Vector3f angular_accel_sp = _control.getAngularAccelerationSetpoint();
-
 	vehicle_angular_acceleration_setpoint_s v_angular_accel_sp = {};
-	v_angular_accel_sp.timestamp = hrt_absolute_time();
 	v_angular_accel_sp.timestamp_sample = _timestamp_sample;
 	v_angular_accel_sp.xyz[0] = (PX4_ISFINITE(angular_accel_sp(0))) ? angular_accel_sp(0) : 0.0f;
 	v_angular_accel_sp.xyz[1] = (PX4_ISFINITE(angular_accel_sp(1))) ? angular_accel_sp(1) : 0.0f;
 	v_angular_accel_sp.xyz[2] = (PX4_ISFINITE(angular_accel_sp(2))) ? angular_accel_sp(2) : 0.0f;
-
+	v_angular_accel_sp.timestamp = hrt_absolute_time();
 	_vehicle_angular_acceleration_setpoint_pub.publish(v_angular_accel_sp);
 }

@ -253,14 +251,12 @@ void
 AngularVelocityController::publish_torque_setpoint()
 {
 	Vector3f torque_sp = _control.getTorqueSetpoint();
-
 	vehicle_torque_setpoint_s v_torque_sp = {};
-	v_torque_sp.timestamp = hrt_absolute_time();
 	v_torque_sp.timestamp_sample = _timestamp_sample;
 	v_torque_sp.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
 	v_torque_sp.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
 	v_torque_sp.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
-
+	v_torque_sp.timestamp = hrt_absolute_time();
 	_vehicle_torque_setpoint_pub.publish(v_torque_sp);
 }

@ -268,12 +264,11 @@ void
 AngularVelocityController::publish_thrust_setpoint()
 {
 	vehicle_thrust_setpoint_s v_thrust_sp = {};
-	v_thrust_sp.timestamp = hrt_absolute_time();
 	v_thrust_sp.timestamp_sample = _timestamp_sample;
 	v_thrust_sp.xyz[0] = (PX4_ISFINITE(_thrust_sp(0))) ? (_thrust_sp(0)) : 0.0f;
 	v_thrust_sp.xyz[1] = (PX4_ISFINITE(_thrust_sp(1))) ? (_thrust_sp(1)) : 0.0f;
 	v_thrust_sp.xyz[2] = (PX4_ISFINITE(_thrust_sp(2))) ? (_thrust_sp(2)) : 0.0f;
-
+	v_thrust_sp.timestamp = hrt_absolute_time();
 	_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
 }

--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@ -1575,7 +1575,7 @@ void Commander::updateParameters()
 			       && _vtol_vehicle_status.vehicle_vtol_state != vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
 	const bool is_fixed = is_fixed_wing(_vehicle_status) || (is_vtol(_vehicle_status)
 			      && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW);
-	const bool is_ground = is_ground_rover(_vehicle_status);
+	const bool is_ground = is_ground_vehicle(_vehicle_status);

 	/* disable manual override for all systems that rely on electronic stabilization */
 	if (is_rotary) {
--- a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.cpp
@ -35,11 +35,6 @@

 using namespace time_literals;

-OffboardChecks::OffboardChecks()
-{
-	_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1_s);
-}
-
 void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
 {
 	reporter.failsafeFlags().offboard_control_signal_lost = true;
@ -47,9 +42,10 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
 	offboard_control_mode_s offboard_control_mode;

 	if (_offboard_control_mode_sub.copy(&offboard_control_mode)) {
-		bool offboard_available = offboard_control_mode.position || offboard_control_mode.velocity
-					  || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
-					  || offboard_control_mode.actuator;
+		bool data_is_recent = hrt_absolute_time() < offboard_control_mode.timestamp + _param_com_of_loss_t.get() * 1_s;
+		bool offboard_available = (offboard_control_mode.position || offboard_control_mode.velocity
+					   || offboard_control_mode.acceleration || offboard_control_mode.attitude || offboard_control_mode.body_rate
+					   || offboard_control_mode.actuator) && data_is_recent;

 		if (offboard_control_mode.position && reporter.failsafeFlags().local_position_invalid) {
 			offboard_available = false;
@ -62,9 +58,7 @@ void OffboardChecks::checkAndReport(const Context &context, Report &reporter)
 			offboard_available = false;
 		}

-		_offboard_available.set_state_and_update(offboard_available, hrt_absolute_time());
-
 		// This is a mode requirement, no need to report
-		reporter.failsafeFlags().offboard_control_signal_lost = !_offboard_available.get_state();
+		reporter.failsafeFlags().offboard_control_signal_lost = !offboard_available;
 	}
 }
--- a/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/offboardCheck.hpp
@ -37,19 +37,17 @@

 #include <uORB/Subscription.hpp>
 #include <uORB/topics/offboard_control_mode.h>
-#include <lib/hysteresis/hysteresis.h>

 class OffboardChecks : public HealthAndArmingCheckBase
 {
 public:
-	OffboardChecks();
+	OffboardChecks() = default;
 	~OffboardChecks() = default;

 	void checkAndReport(const Context &context, Report &reporter) override;

 private:
 	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
-	systemlib::Hysteresis _offboard_available{false};

 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamFloat<px4::params::COM_OF_LOSS_T>) _param_com_of_loss_t
--- a/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/rcAndDataLinkCheck.cpp
@ -119,7 +119,7 @@ void RcAndDataLinkChecks::checkAndReport(const Context &context, Report &reporte
 					    events::ID("check_rc_dl_no_dllink"),
 					    log_level, "No connection to the ground control station");

-		if (reporter.mavlink_log_pub()) {
+		if (gcs_connection_required && reporter.mavlink_log_pub()) {
 			mavlink_log_warning(reporter.mavlink_log_pub(), "Preflight Fail: No connection to the ground control station\t");
 		}

--- a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.cpp
@ -32,6 +32,8 @@
 ****************************************************************************/

 #include "sdcardCheck.hpp"
+#include <dirent.h>
+#include <string.h>

 #ifdef __PX4_DARWIN
 #include <sys/param.h>
@ -42,38 +44,85 @@

 void SdCardChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	if (_param_com_arm_sdcard.get() <= 0) {
-		return;
-	}
+	if (_param_com_arm_sdcard.get() > 0) {

-	struct statfs statfs_buf;
+		struct statfs statfs_buf;

-	if (!_sdcard_detected && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) {
-		// on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted
-		_sdcard_detected = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0);
-	}
-
-	if (!_sdcard_detected) {
-		NavModes affected_modes{NavModes::None};
-
-		if (_param_com_arm_sdcard.get() == 2) {
-			// disallow arming without sd card
-			affected_modes = NavModes::All;
+		if (!_sdcard_detected && statfs(PX4_STORAGEDIR, &statfs_buf) == 0) {
+			// on NuttX we get a data block count f_blocks and byte count per block f_bsize if an SD card is inserted
+			_sdcard_detected = (statfs_buf.f_blocks > 0) && (statfs_buf.f_bsize > 0);
 		}

+		if (!_sdcard_detected) {
+			NavModes affected_modes{NavModes::None};
+
+			if (_param_com_arm_sdcard.get() == 2) {
+				// disallow arming without sd card
+				affected_modes = NavModes::All;
+			}
+
+			/* EVENT
+			 * @description
+			 * Insert an SD Card to the autopilot and reboot the system.
+			 *
+			 * <profile name="dev">
+			 * This check can be configured via <param>COM_ARM_SDCARD</param> parameter.
+			 * </profile>
+			 */
+			reporter.armingCheckFailure(affected_modes, health_component_t::system,
+						    events::ID("check_missing_fmu_sdcard"),
+						    events::Log::Error, "Missing FMU SD Card");
+
+			if (reporter.mavlink_log_pub()) {
+				mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Missing FMU SD Card");
+			}
+		}
+	}
+
+#ifdef __PX4_NUTTX
+	// Check for hardfault files
+
+	if (!_hardfault_checked_once && _param_com_arm_hardfault_check.get()) {
+		_hardfault_checked_once = true;
+
+		DIR *dp = opendir(PX4_STORAGEDIR);
+
+		if (dp != nullptr) {
+
+			struct dirent *result;
+
+			while ((result = readdir(dp)) && !_hardfault_file_present) {
+
+				// Check for pattern fault_*.log
+				if (strncmp("fault_", result->d_name, 6) == 0 && strcmp(result->d_name + strlen(result->d_name) - 4, ".log") == 0) {
+					_hardfault_file_present = true;
+				}
+			}
+
+			closedir(dp);
+		}
+	}
+
+	if (_hardfault_file_present && _param_com_arm_hardfault_check.get()) {
 		/* EVENT
 		 * @description
-		 * Insert an SD Card to the autopilot and reboot the system.
+		 * The SD card contains crash dump files, service the vehicle before continuing to fly.
 		 *
 		 * <profile name="dev">
-		 * This check can be configured via <param>COM_ARM_SDCARD</param> parameter.
+		 * Check how to debug hardfaults on <a>https://docs.px4.io/main/en/debug/gdb_debugging.html#hard-fault-debugging</a>.
+		 * When completed, remove the files 'fault_*.log' on the SD card.
+		 *
+		 * This check can be configured via <param>COM_ARM_HFLT_CHK</param> parameter.
 		 * </profile>
 		 */
-		reporter.armingCheckFailure(affected_modes, health_component_t::system, events::ID("check_missing_fmu_sdcard"),
-					    events::Log::Error, "Missing FMU SD Card");
+		reporter.healthFailure(NavModes::All, health_component_t::system,
+				       events::ID("check_hardfault_present"),
+				       events::Log::Error, "Crash dumps present on SD card");

 		if (reporter.mavlink_log_pub()) {
-			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Missing FMU SD Card");
+			mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Crash dumps present on SD, vehicle needs service");
 		}
 	}
+
+#endif /* __PX4_NUTTX */
 }
--- a/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp
+++ b/src/modules/commander/HealthAndArmingChecks/checks/sdcardCheck.hpp
@ -46,7 +46,13 @@ public:
 private:
 	bool _sdcard_detected{false};

+#ifdef __PX4_NUTTX
+	bool _hardfault_checked_once {false};
+	bool _hardfault_file_present{false};
+#endif
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamInt<px4::params::COM_ARM_SDCARD>) _param_com_arm_sdcard
+					(ParamInt<px4::params::COM_ARM_SDCARD>) _param_com_arm_sdcard,
+					(ParamBool<px4::params::COM_ARM_HFLT_CHK>) _param_com_arm_hardfault_check
 				       )
 };
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@ -118,19 +118,9 @@ bool is_fixed_wing(const vehicle_status_s &current_status)
 	return current_status.system_type == VEHICLE_TYPE_FIXED_WING;
 }

-bool is_ground_rover(const vehicle_status_s &current_status)
-{
-	return current_status.system_type == VEHICLE_TYPE_GROUND_ROVER;
-}
-
-bool is_boat(const vehicle_status_s &current_status)
-{
-	return current_status.system_type == VEHICLE_TYPE_BOAT;
-}
-
 bool is_ground_vehicle(const vehicle_status_s &current_status)
 {
-	return is_ground_rover(current_status) || is_boat(current_status);
+	return (current_status.system_type == VEHICLE_TYPE_BOAT || current_status.system_type == VEHICLE_TYPE_GROUND_ROVER);
 }

 // End time for currently blinking LED message, 0 if no blink message
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@ -55,8 +55,6 @@ bool is_rotary_wing(const vehicle_status_s &current_status);
 bool is_vtol(const vehicle_status_s &current_status);
 bool is_vtol_tailsitter(const vehicle_status_s &current_status);
 bool is_fixed_wing(const vehicle_status_s &current_status);
-bool is_ground_rover(const vehicle_status_s &current_status);
-bool is_boat(const vehicle_status_s &current_status);
 bool is_ground_vehicle(const vehicle_status_s &current_status);

 int buzzer_init(void);
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@ -1010,6 +1010,17 @@ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1);
 */
 PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1);

+/**
+ * Enable FMU SD card hardfault detection check
+ *
+ * This check detects if there are hardfault files present on the
+ * SD card. If so, and the parameter is enabled, arming is prevented.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_ARM_HFLT_CHK, 1);
+
 /**
 * Enforced delay between arming and further navigation
 *
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
@ -199,10 +199,10 @@ public:
 	virtual uint32_t getStoppedMotors() const { return 0; }

 	/**
-	 * Fill in the allocated and unallocated torque and thrust.
-	 * Should return false if not filled in and the effectivenes matrix should be used instead
+	 * Fill in the unallocated torque and thrust, customized by effectiveness type.
+	 * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead.
 	 */
-	virtual bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const { return false; }
+	virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}

 protected:
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
@ -217,48 +217,36 @@ void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &posit
 	}
 }

-bool ActuatorEffectivenessHelicopter::getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const
+void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
 {
-	status.torque_setpoint_achieved = true;
-	status.thrust_setpoint_achieved = true;

 	// Note: the values '-1', '1' and '0' are just to indicate a negative,
 	// positive or no saturation to the rate controller. The actual magnitude is not used.
 	if (_saturation_flags.roll_pos) {
 		status.unallocated_torque[0] = 1.f;
-		status.torque_setpoint_achieved = false;

 	} else if (_saturation_flags.roll_neg) {
 		status.unallocated_torque[0] = -1.f;
-		status.torque_setpoint_achieved = false;
 	}

 	if (_saturation_flags.pitch_pos) {
 		status.unallocated_torque[1] = 1.f;
-		status.torque_setpoint_achieved = false;

 	} else if (_saturation_flags.pitch_neg) {
 		status.unallocated_torque[1] = -1.f;
-		status.torque_setpoint_achieved = false;
 	}

 	if (_saturation_flags.yaw_pos) {
 		status.unallocated_torque[2] = 1.f;
-		status.torque_setpoint_achieved = false;

 	} else if (_saturation_flags.yaw_neg) {
 		status.unallocated_torque[2] = -1.f;
-		status.torque_setpoint_achieved = false;
 	}

 	if (_saturation_flags.thrust_pos) {
 		status.unallocated_thrust[2] = 1.f;
-		status.thrust_setpoint_achieved = false;

 	} else if (_saturation_flags.thrust_neg) {
 		status.unallocated_thrust[2] = -1.f;
-		status.thrust_setpoint_achieved = false;
 	}
-
-	return true;
 }
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
@ -79,7 +79,7 @@ public:
 			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
 			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;

-	bool getAllocatedAndUnallocatedControl(control_allocator_status_s &status) const override;
+	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:
 	float throttleSpoolupProgress();
 	bool mainMotorEnaged();
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp
@ -233,10 +233,11 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry
 	return num_actuators;
 }

-uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control)
+uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts,
+		float collective_tilt_control)
 {
-	if (!PX4_ISFINITE(tilt_control)) {
-		tilt_control = -1.f;
+	if (!PX4_ISFINITE(collective_tilt_control)) {
+		collective_tilt_control = -1.f;
 	}

 	uint32_t nontilted_motors = 0;
@ -250,8 +251,8 @@ uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectiv
 		}

 		const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index);
-		float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (tilt_control + 1.f) / 2.f);
-		float tilt_direction = math::radians((float)tilt.tilt_direction);
+		const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f);
+		const float tilt_direction = math::radians((float)tilt.tilt_direction);
 		_geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction);
 	}

--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
@ -54,7 +54,7 @@ bool
 ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration,
 		EffectivenessUpdateReason external_update)
 {
-	if (!_combined_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+	if (!_collective_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}

@ -66,9 +66,9 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 	// Update matrix with tilts in vertical position when update is triggered by a manual
 	// configuration (parameter) change. This is to make sure the normalization
 	// scales are tilt-invariant. Note: configuration updates are only possible when disarmed.
-	const float tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? -1.f :
-					   _last_tilt_control;
-	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, tilt_control_applied)
+	const float collective_tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ?
+			-1.f : _last_collective_tilt_control;
+	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied)
 			    << configuration.num_actuators[(int)ActuatorType::MOTORS];

 	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
@ -86,7 +86,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config

 	// If it was an update coming from a config change, then make sure to update matrix in
 	// the next iteration again with the correct tilt (but without updating the normalization scale).
-	_combined_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
+	_collective_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE);

 	return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully);
 }
@ -113,24 +113,24 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 		actuator_controls_s actuator_controls_1;

 		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
-			float control_tilt = actuator_controls_1.control[actuator_controls_s::INDEX_COLLECTIVE_TILT] * 2.f - 1.f;
+			float control_collective_tilt = actuator_controls_1.control[actuator_controls_s::INDEX_COLLECTIVE_TILT] * 2.f - 1.f;

-			// set control_tilt to exactly -1 or 1 if close to these end points
-			control_tilt = control_tilt < -0.99f ? -1.f : control_tilt;
-			control_tilt = control_tilt > 0.99f ? 1.f : control_tilt;
+			// set control_collective_tilt to exactly -1 or 1 if close to these end points
+			control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
+			control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt;

-			// initialize _last_tilt_control
-			if (!PX4_ISFINITE(_last_tilt_control)) {
-				_last_tilt_control = control_tilt;
+			// initialize _last_collective_tilt_control
+			if (!PX4_ISFINITE(_last_collective_tilt_control)) {
+				_last_collective_tilt_control = control_collective_tilt;

-			} else if (fabsf(control_tilt - _last_tilt_control) > 0.01f) {
-				_combined_tilt_updated = true;
-				_last_tilt_control = control_tilt;
+			} else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) {
+				_collective_tilt_updated = true;
+				_last_collective_tilt_control = control_collective_tilt;
 			}

 			for (int i = 0; i < _tilts.count(); ++i) {
 				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
-					actuator_sp(i + _first_tilt_idx) += control_tilt;
+					actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
 				}
 			}
 		}
@ -147,6 +147,21 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 			}
 		}
 	}
+
+	// Set yaw saturation flag in case of yaw through tilt. As in this case the yaw actuation is decoupled from
+	// the other axes (for now neglecting the case of 0 collective thrust), we set the saturation flags
+	// directly if the (normalized) yaw torque setpoint is outside of range (-1, 1).
+	if (matrix_index == 0 && _tilts.hasYawControl()) {
+		_yaw_tilt_saturation_flags.tilt_yaw_neg = false;
+		_yaw_tilt_saturation_flags.tilt_yaw_pos = false;
+
+		if (control_sp(2) < -1.f) {
+			_yaw_tilt_saturation_flags.tilt_yaw_neg = true;
+
+		} else if (control_sp(2) > 1.f) {
+			_yaw_tilt_saturation_flags.tilt_yaw_pos = true;
+		}
+	}
 }

 void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
@ -170,3 +185,23 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh
 		break;
 	}
 }
+
+void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
+{
+	// only handle matrix 0 (motors and tilts)
+	if (matrix_index == 1) {
+		return;
+	}
+
+	// Note: the values '-1', '1' and '0' are just to indicate a negative,
+	// positive or no saturation to the rate controller. The actual magnitude is not used.
+	if (_yaw_tilt_saturation_flags.tilt_yaw_pos) {
+		status.unallocated_torque[2] = 1.f;
+
+	} else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) {
+		status.unallocated_torque[2] = -1.f;
+
+	} else {
+		status.unallocated_torque[2] = 0.f;
+	}
+}
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
@ -81,8 +81,11 @@ public:
 	const char *name() const override { return "VTOL Tiltrotor"; }

 	uint32_t getStoppedMotors() const override { return _stopped_motors; }
+
+	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
+
 protected:
-	bool _combined_tilt_updated{true};
+	bool _collective_tilt_updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 	ActuatorEffectivenessTilts _tilts;
@ -93,8 +96,15 @@ protected:
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 	int _first_tilt_idx{0}; ///< applies to matrix 0

-	float _last_tilt_control{NAN};
+	float _last_collective_tilt_control{NAN};

 	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
 	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
+
+	struct YawTiltSaturationFlags {
+		bool tilt_yaw_pos;
+		bool tilt_yaw_neg;
+	};
+
+	YawTiltSaturationFlags _yaw_tilt_saturation_flags{};
 };
--- a/src/modules/control_allocator/ControlAllocator.cpp
+++ b/src/modules/control_allocator/ControlAllocator.cpp
@ -589,33 +589,29 @@ ControlAllocator::publish_control_allocator_status(int matrix_index)

 	// TODO: disabled motors (?)

-	if (!_actuator_effectiveness->getAllocatedAndUnallocatedControl(control_allocator_status)) {
-		// Allocated control
-		const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();
-		control_allocator_status.allocated_torque[0] = allocated_control(0);
-		control_allocator_status.allocated_torque[1] = allocated_control(1);
-		control_allocator_status.allocated_torque[2] = allocated_control(2);
-		control_allocator_status.allocated_thrust[0] = allocated_control(3);
-		control_allocator_status.allocated_thrust[1] = allocated_control(4);
-		control_allocator_status.allocated_thrust[2] = allocated_control(5);
+	// Allocated control
+	const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();

-		// Unallocated control
-		const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
-				allocated_control;
-		control_allocator_status.unallocated_torque[0] = unallocated_control(0);
-		control_allocator_status.unallocated_torque[1] = unallocated_control(1);
-		control_allocator_status.unallocated_torque[2] = unallocated_control(2);
-		control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
-		control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
-		control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
+	// Unallocated control
+	const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
+			allocated_control;
+	control_allocator_status.unallocated_torque[0] = unallocated_control(0);
+	control_allocator_status.unallocated_torque[1] = unallocated_control(1);
+	control_allocator_status.unallocated_torque[2] = unallocated_control(2);
+	control_allocator_status.unallocated_thrust[0] = unallocated_control(3);
+	control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
+	control_allocator_status.unallocated_thrust[2] = unallocated_control(5);

-		// Allocation success flags
-		control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1),
-				unallocated_control(2)).norm_squared() < 1e-6f);
-		control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4),
-				unallocated_control(5)).norm_squared() < 1e-6f);
-	}
+	// override control_allocator_status in customized saturation logic for certain effectiveness types
+	_actuator_effectiveness->getUnallocatedControl(matrix_index, control_allocator_status);

+	// Allocation success flags
+	control_allocator_status.torque_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_torque[0],
+			control_allocator_status.unallocated_torque[1],
+			control_allocator_status.unallocated_torque[2]).norm_squared() < 1e-6f);
+	control_allocator_status.thrust_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_thrust[0],
+			control_allocator_status.unallocated_thrust[1],
+			control_allocator_status.unallocated_thrust[2]).norm_squared() < 1e-6f);

 	// Actuator saturation
 	const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint();
--- a/src/modules/ekf2/EKF/common.h
+++ b/src/modules/ekf2/EKF/common.h
@ -359,6 +359,7 @@ struct parameters {
 	float arsp_thr{2.0f};                   ///< Airspeed fusion threshold. A value of zero will deactivate airspeed fusion

 	// synthetic sideslip fusion
+	int32_t beta_fusion_enabled{0};
 	float beta_innov_gate{5.0f};            ///< synthetic sideslip innovation consistency gate size in standard deviation (STD)
 	float beta_noise{0.3f};                 ///< synthetic sideslip noise (rad)
 	const float beta_avg_ft_us{150000.0f};  ///< The average time between synthetic sideslip measurements (uSec)
--- a/src/modules/ekf2/EKF/control.cpp
+++ b/src/modules/ekf2/EKF/control.cpp
@ -332,30 +332,31 @@ void Ekf::controlAirDataFusion()

 void Ekf::controlBetaFusion()
 {
-	if (_control_status.flags.fake_pos) {
-		return;
-	}
+	_control_status.flags.fuse_beta = _params.beta_fusion_enabled && _control_status.flags.fixed_wing
+		&& _control_status.flags.in_air && !_control_status.flags.fake_pos;

-	// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fuson had been enabled externally:
-	const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us);
+	if (_control_status.flags.fuse_beta) {

-	if (beta_fusion_time_triggered &&
-	    _control_status.flags.fuse_beta &&
-	    _control_status.flags.in_air) {
-		updateSideslip(_aid_src_sideslip);
-		_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
+		// Perform synthetic sideslip fusion at regular intervals when in-air and sideslip fusion had been enabled externally:
+		const bool beta_fusion_time_triggered = isTimedOut(_aid_src_sideslip.time_last_fuse, _params.beta_avg_ft_us);

-		// If starting wind state estimation, reset the wind states and covariances before fusing any data
-		if (!_control_status.flags.wind) {
-			// activate the wind states
-			_control_status.flags.wind = true;
-			// reset the timeout timers to prevent repeated resets
-			_aid_src_sideslip.time_last_fuse = _imu_sample_delayed.time_us;
-			resetWind();
-		}
+		if (beta_fusion_time_triggered) {

-		if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
-			fuseSideslip(_aid_src_sideslip);
+			updateSideslip(_aid_src_sideslip);
+			_innov_check_fail_status.flags.reject_sideslip = _aid_src_sideslip.innovation_rejected;
+
+			// If starting wind state estimation, reset the wind states and covariances before fusing any data
+			if (!_control_status.flags.wind) {
+				// activate the wind states
+				_control_status.flags.wind = true;
+				// reset the timeout timers to prevent repeated resets
+				_aid_src_sideslip.time_last_fuse = _imu_sample_delayed.time_us;
+				resetWind();
+			}
+
+			if (Vector2f(Vector2f(_state.vel) - _state.wind_vel).longerThan(7.f)) {
+				fuseSideslip(_aid_src_sideslip);
+			}
 		}
 	}
 }
--- a/src/modules/ekf2/EKF/estimator_interface.h
+++ b/src/modules/ekf2/EKF/estimator_interface.h
@ -132,9 +132,6 @@ public:
 	// set vehicle is fixed wing status
 	void set_is_fixed_wing(bool is_fixed_wing) { _control_status.flags.fixed_wing = is_fixed_wing; }

-	// set flag if synthetic sideslip measurement should be fused
-	void set_fuse_beta_flag(bool fuse_beta) { _control_status.flags.fuse_beta = (fuse_beta && _control_status.flags.in_air); }
-
 	// set flag if static pressure rise due to ground effect is expected
 	// use _params.gnd_effect_deadzone to adjust for expected rise in static pressure
 	// flag will clear after GNDEFFECT_TIMEOUT uSec
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@ -150,6 +150,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_ev_pos_y(_params->ev_pos_body(1)),
 	_param_ekf2_ev_pos_z(_params->ev_pos_body(2)),
 	_param_ekf2_arsp_thr(_params->arsp_thr),
+	_param_ekf2_fuse_beta(_params->beta_fusion_enabled),
 	_param_ekf2_tau_vel(_params->vel_Tau),
 	_param_ekf2_tau_pos(_params->pos_Tau),
 	_param_ekf2_gbias_init(_params->switch_on_gyro_bias),
@ -581,9 +582,6 @@ void EKF2::Run()
 			if (_status_sub.copy(&vehicle_status)) {
 				const bool is_fixed_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);

-				// only fuse synthetic sideslip measurements if conditions are met
-				_ekf.set_fuse_beta_flag(is_fixed_wing && (_param_ekf2_fuse_beta.get() == 1));
-
 				// let the EKF know if the vehicle motion is that of a fixed wing (forward flight only relative to wind)
 				_ekf.set_is_fixed_wing(is_fixed_wing);

--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@ -571,7 +571,7 @@ private:
 		// control of airspeed and sideslip fusion
 		(ParamExtFloat<px4::params::EKF2_ARSP_THR>)
 		_param_ekf2_arsp_thr, 	///< A value of zero will disabled airspeed fusion. Any positive value sets the minimum airspeed which will be used (m/sec)
-		(ParamInt<px4::params::EKF2_FUSE_BETA>)
+		(ParamExtInt<px4::params::EKF2_FUSE_BETA>)
 		_param_ekf2_fuse_beta,		///< Controls synthetic sideslip fusion, 0 disables, 1 enables

 		// output predictor filter time constants
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@ -157,7 +157,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 					q.copyTo(_att_sp.q_d);

-					_att_sp.reset_rate_integrals = false;
+					_att_sp.reset_integral = false;

 					_att_sp.timestamp = hrt_absolute_time();

@ -392,8 +392,8 @@ void FixedwingAttitudeControl::Run()
 			const float airspeed = get_airspeed_and_update_scaling();

 			/* reset integrals where needed */
-			if (_att_sp.reset_rate_integrals) {
-				_rate_control.resetIntegral();
+			if (_att_sp.reset_integral) {
+				_rates_sp.reset_integral = true;
 			}

 			/* Reset integrators if the aircraft is on ground
@ -403,7 +403,7 @@ void FixedwingAttitudeControl::Run()
 			    || (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 				&& !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) {

-				_rate_control.resetIntegral();
+				_rates_sp.reset_integral = true;
 				_wheel_ctrl.reset_integrator();
 			}

@ -605,7 +605,7 @@ void FixedwingAttitudeControl::Run()
 				_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw,
 						-1.f, 1.f) : trim_yaw;

-				if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u)) {
+				if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u) || _rates_sp.reset_integral) {
 					_rate_control.resetIntegral();
 				}

--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@ -1487,7 +1487,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo

 		if (_runway_takeoff.resetIntegrators()) {
 			// reset integrals except yaw (which also counts for the wheel controller)
-			_att_sp.reset_rate_integrals = true;
+			_att_sp.reset_integral = true;

 			// throttle is open loop anyway during ground roll, no need to wind up the integrator
 			_tecs.resetIntegrals();
@ -1626,7 +1626,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo

 		} else {
 			/* Tell the attitude controller to stop integrating while we are waiting for the launch */
-			_att_sp.reset_rate_integrals = true;
+			_att_sp.reset_integral = true;

 			/* Set default roll and pitch setpoints during detection phase */
 			_att_sp.roll_body = 0.0f;
@ -2249,7 +2249,7 @@ FixedwingPositionControl::Run()
 		_npfg.setPeriod(_param_npfg_period.get());
 		_l1_control.set_l1_period(_param_fw_l1_period.get());

-		_att_sp.reset_rate_integrals = false;
+		_att_sp.reset_integral = false;

 		// by default we don't want yaw to be contoller directly with rudder
 		_att_sp.fw_control_yaw = false;
--- a/src/modules/land_detector/LandDetector.cpp
+++ b/src/modules/land_detector/LandDetector.cpp
@ -57,6 +57,7 @@ LandDetector::LandDetector() :
 	_land_detected.has_low_throttle = false;
 	_land_detected.vertical_movement = false;
 	_land_detected.horizontal_movement = false;
+	_land_detected.rotational_movement = false;
 	_land_detected.close_to_ground_or_skipped_check = true;
 	_land_detected.at_rest = true;
 }
@ -174,6 +175,7 @@ void LandDetector::Run()
 		_land_detected.has_low_throttle = _get_has_low_throttle();
 		_land_detected.horizontal_movement = _get_horizontal_movement();
 		_land_detected.vertical_movement = _get_vertical_movement();
+		_land_detected.rotational_movement = _get_rotational_movement();
 		_land_detected.close_to_ground_or_skipped_check = _get_close_to_ground_or_skipped_check();
 		_land_detected.at_rest = at_rest;
 		_land_detected.timestamp = hrt_absolute_time();
--- a/src/modules/land_detector/LandDetector.h
+++ b/src/modules/land_detector/LandDetector.h
@ -138,6 +138,7 @@ protected:
 	virtual bool _get_has_low_throttle() { return false; }
 	virtual bool _get_horizontal_movement() { return false; }
 	virtual bool _get_vertical_movement() { return false; }
+	virtual bool _get_rotational_movement() { return false; }
 	virtual bool _get_close_to_ground_or_skipped_check() {  return false; }
 	virtual void _set_hysteresis_factor(const int factor) = 0;

--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2022 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
@ -75,12 +75,10 @@ namespace land_detector

 MulticopterLandDetector::MulticopterLandDetector()
 {
-	_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
 	_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
 	_paramHandle.minThrottle = param_find("MPC_THR_MIN");
 	_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
 	_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
-	_paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL");
 }

 void MulticopterLandDetector::_update_topics()
@ -119,16 +117,6 @@ void MulticopterLandDetector::_update_params()
 {
 	param_get(_paramHandle.minThrottle, &_params.minThrottle);
 	param_get(_paramHandle.minManThrottle, &_params.minManThrottle);
-	param_get(_paramHandle.landSpeed, &_params.landSpeed);
-	param_get(_paramHandle.crawlSpeed, &_params.crawlSpeed);
-
-	if (_param_lndmc_z_vel_max.get() > _params.landSpeed) {
-		PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_SPEED, updating %.3f -> %.3f",
-			(double)_param_lndmc_z_vel_max.get(), (double)_params.landSpeed);
-
-		_param_lndmc_z_vel_max.set(_params.landSpeed);
-		_param_lndmc_z_vel_max.commit_no_notification();
-	}

 	int32_t use_hover_thrust_estimate = 0;
 	param_get(_paramHandle.useHoverThrustEstimate, &use_hover_thrust_estimate);
@ -158,22 +146,19 @@ bool MulticopterLandDetector::_get_ground_contact_state()

 	const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s);

-	// land speed threshold, 90% of MPC_LAND_SPEED
-	const float crawl_speed_threshold = 0.9f * math::max(_params.crawlSpeed, 0.1f);
-
 	if (lpos_available && _vehicle_local_position.v_z_valid) {
-		// Check if we are moving vertically - this might see a spike after arming due to
-		// throttle-up vibration. If accelerating fast the throttle thresholds will still give
-		// an accurate in-air indication.
-		float max_climb_rate = math::min(crawl_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get());
+		// Check if we are moving vertically.
+		// Use wider threshold if currently in "maybe landed" state, as estimation for
+		// vertical speed is often deteriorated when on the ground or due to propeller
+		// up/down throttling.

-		if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
-			// Widen acceptance thresholds for landed state right after arming
-			// so that motor spool-up and other effects do not trigger false negatives.
-			max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f;
+		float vertical_velocity_threshold = _param_lndmc_z_vel_max.get();
+
+		if (_landed_hysteresis.get_state()) {
+			vertical_velocity_threshold *= 2.5f;
 		}

-		_vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate);
+		_vertical_movement = (fabsf(_vehicle_local_position.vz) > vertical_velocity_threshold);

 	} else {
 		_vertical_movement = true;
@ -217,7 +202,7 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 		if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
 			// Setpoints can be NAN
 			_in_descend = PX4_ISFINITE(trajectory_setpoint.velocity[2])
-				      && (trajectory_setpoint.velocity[2] >= crawl_speed_threshold);
+				      && (trajectory_setpoint.velocity[2] >= 1.1f * _param_lndmc_z_vel_max.get());
 		}

 		// ground contact requires commanded descent until landed
@ -278,21 +263,21 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
 		return false;
 	}

+	// Next look if vehicle is not rotating (do not consider yaw)
+	float max_rotation_threshold = math::radians(_param_lndmc_rot_max.get());

-	float landThresholdFactor = 1.f;
-
-	// Widen acceptance thresholds for landed state right after landed
-	if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
-		landThresholdFactor = 2.5f;
+	// Widen max rotation thresholds if either in landed state, thus making it harder
+	// to trigger a false positive !landed e.g. due to propeller throttling up/down.
+	if (_landed_hysteresis.get_state()) {
+		max_rotation_threshold *= 2.5f;
 	}

-	// Next look if all rotation angles are not moving.
-	const float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
-
-	matrix::Vector2f angular_velocity{_angular_velocity(0), _angular_velocity(1)};
-
-	if (angular_velocity.norm() > max_rotation_scaled) {
+	if (_angular_velocity.xy().norm() > max_rotation_threshold) {
+		_rotational_movement = true;
 		return false;
+
+	} else {
+		_rotational_movement = false;
 	}

 	// If vertical velocity is available: ground contact, no thrust, no movement -> landed
@ -306,14 +291,6 @@ bool MulticopterLandDetector::_get_maybe_landed_state()

 bool MulticopterLandDetector::_get_landed_state()
 {
-	// reset the landed_time
-	if (!_maybe_landed_hysteresis.get_state()) {
-		_landed_time = 0;
-
-	} else if (_landed_time == 0) {
-		_landed_time = hrt_absolute_time();
-	}
-
 	// When not armed, consider to be landed
 	if (!_armed) {
 		return true;
--- a/src/modules/land_detector/MulticopterLandDetector.h
+++ b/src/modules/land_detector/MulticopterLandDetector.h
@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2022 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
@ -74,6 +74,7 @@ protected:
 	bool _get_has_low_throttle() override { return _has_low_throttle; }
 	bool _get_horizontal_movement() override { return _horizontal_movement; }
 	bool _get_vertical_movement() override { return _vertical_movement; }
+	bool _get_rotational_movement() override { return _rotational_movement; }
 	bool _get_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; }

 	void _set_hysteresis_factor(const int factor) override;
@ -83,9 +84,6 @@ private:
 	/** Time in us that freefall has to hold before triggering freefall */
 	static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms;

-	/** Time interval in us in which wider acceptance thresholds are used after landed. */
-	static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s;
-
 	/** Distance above ground below which entering ground contact state is possible when distance to ground is available. */
 	static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f;

@ -94,8 +92,6 @@ private:
 		param_t minThrottle;
 		param_t hoverThrottle;
 		param_t minManThrottle;
-		param_t landSpeed;
-		param_t crawlSpeed;
 		param_t useHoverThrustEstimate;
 	} _paramHandle{};

@ -103,8 +99,6 @@ private:
 		float minThrottle;
 		float hoverThrottle;
 		float minManThrottle;
-		float landSpeed;
-		float crawlSpeed;
 		bool useHoverThrustEstimate;
 	} _params{};

@ -126,11 +120,11 @@ private:
 	uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED};

 	hrt_abstime _min_thrust_start{0};	///< timestamp when minimum trust was applied first
-	hrt_abstime _landed_time{0};

 	bool _in_descend{false};		///< vehicle is commanded to desend
 	bool _horizontal_movement{false};	///< vehicle is moving horizontally
 	bool _vertical_movement{false};
+	bool _rotational_movement{false};
 	bool _has_low_throttle{false};
 	bool _close_to_ground_or_skipped_check{false};
 	bool _below_gnd_effect_hgt{false};	///< vehicle height above ground is below height where ground effect occurs
--- a/src/modules/land_detector/land_detector_params_mc.c
+++ b/src/modules/land_detector/land_detector_params_mc.c
@ -48,16 +48,19 @@
 PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f);

 /**
- * Multicopter max climb rate
+ * Multicopter vertical velocity threshold
 *
- * Maximum vertical velocity allowed in the landed state
+ * Vertical velocity threshold to detect landing.
+ * Should be set lower than the expected minimal speed for landing
+ * so MPC_LAND_SPEED for autonomous landing and MPC_LAND_CRWL
+ * if distance sensor is present and slowing down close to ground.
 *
 * @unit m/s
 * @decimal 1
 *
 * @group Land Detector
 */
-PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.50f);
+PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.25f);

 /**
 * Multicopter max horizontal velocity
--- a/src/modules/mavlink/mavlink
+++ b/src/modules/mavlink/mavlink
@ -1 +1 @@
-Subproject commit dda5a18ddb002a871ba301bb584893ee6378e2f3
+Subproject commit d012c7afd5f4e2a9388710596fd3eb9a1b3eb086
--- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
@ -62,7 +62,7 @@ TEST(PositionControlTest, EmptySetpoint)
 	EXPECT_FLOAT_EQ(attitude.yaw_sp_move_rate, 0.f);
 	EXPECT_EQ(Quatf(attitude.q_d), Quatf(1.f, 0.f, 0.f, 0.f));
 	EXPECT_EQ(Vector3f(attitude.thrust_body), Vector3f(0.f, 0.f, 0.f));
-	EXPECT_EQ(attitude.reset_rate_integrals, false);
+	EXPECT_EQ(attitude.reset_integral, false);
 	EXPECT_EQ(attitude.fw_control_yaw, false);
 	EXPECT_FLOAT_EQ(attitude.apply_flaps, 0.f);//vehicle_attitude_setpoint_s::FLAPS_OFF); // TODO why no reference?
 }
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@ -448,10 +448,12 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f);
 PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f);

 /**
- * Land crawl descend rate. Used below
+ * Land crawl descend rate
+ *
+ * Used below MPC_LAND_ALT3 if distance sensor data is availabe.
 *
 * @unit m/s
- * @min 0.3
+ * @min 0.1
 * @decimal 1
 * @group Multicopter Position Control
 */
--- a/src/modules/mc_rate_control/MulticopterRateControl.cpp
+++ b/src/modules/mc_rate_control/MulticopterRateControl.cpp
@ -296,22 +296,20 @@ MulticopterRateControl::Run()
 void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, const hrt_abstime &timestamp_sample)
 {
 	vehicle_torque_setpoint_s vehicle_torque_setpoint{};
-	vehicle_torque_setpoint.timestamp = hrt_absolute_time();
 	vehicle_torque_setpoint.timestamp_sample = timestamp_sample;
 	vehicle_torque_setpoint.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
 	vehicle_torque_setpoint.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
 	vehicle_torque_setpoint.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
-
+	vehicle_torque_setpoint.timestamp = hrt_absolute_time();
 	_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
 }

 void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
 {
 	vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
-	vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
 	vehicle_thrust_setpoint.timestamp_sample = timestamp_sample;
 	_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
-
+	vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
 	_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
 }

--- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
+++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
@ -280,9 +280,8 @@ void VehicleMagnetometer::UpdateMagCalibration()

 							_mag_cal[i].device_id = estimator_sensor_bias.mag_device_id;

-							// readd estimated bias that was removed before publishing vehicle_magnetometer
-							_mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias) +
-									     _calibration_estimator_bias[mag_index];
+							// readd estimated bias that was removed before publishing vehicle_magnetometer (_calibration_estimator_bias)
+							_mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias + _calibration_estimator_bias[mag_index]);

 							_mag_cal[i].variance = bias_variance;

--- a/src/modules/vtol_att_control/tailsitter.cpp
+++ b/src/modules/vtol_att_control/tailsitter.cpp
@ -343,9 +343,22 @@ void Tailsitter::fill_actuator_outputs()
 		_thrust_setpoint_0->xyz[2] = -fw_in[actuator_controls_s::INDEX_THROTTLE];

 		/* allow differential thrust if enabled */
-		if (_param_vt_fw_difthr_en.get()) {
-			mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
-			_torque_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
+		if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT)) {
+			float yaw_control = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get();
+			mc_out[actuator_controls_s::INDEX_ROLL] = yaw_control;
+			_torque_setpoint_0->xyz[0] = yaw_control;
+		}
+
+		if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::PITCH_BIT)) {
+			float pitch_control = fw_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get();
+			mc_out[actuator_controls_s::INDEX_PITCH] = pitch_control;
+			_torque_setpoint_0->xyz[1] = pitch_control;
+		}
+
+		if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::ROLL_BIT)) {
+			float roll_control = -fw_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_r.get();
+			mc_out[actuator_controls_s::INDEX_YAW] = roll_control;
+			_torque_setpoint_0->xyz[2] = roll_control;
 		}

 	} else {
--- a/src/modules/vtol_att_control/tiltrotor.cpp
+++ b/src/modules/vtol_att_control/tiltrotor.cpp
@ -481,9 +481,9 @@ void Tiltrotor::fill_actuator_outputs()
 		_thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE];

 		/* allow differential thrust if enabled */
-		if (_param_vt_fw_difthr_en.get()) {
-			mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
-			_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
+		if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT)) {
+			mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ;
+			_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ;
 		}

 	} else {
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@ -254,27 +254,63 @@ PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f);
 /**
 * Differential thrust in forwards flight.
 *
- * Set to 1 to enable differential thrust in fixed-wing flight.
+ * Enable differential thrust seperately for roll, pitch, yaw in forward (fixed-wing) mode.
+ * The effectiveness of differential thrust around the corresponding axis can be
+ * tuned by setting VT_FW_DIFTHR_S_R / VT_FW_DIFTHR_S_P / VT_FW_DIFTHR_S_Y.
 *
 * @min 0
- * @max 1
- * @decimal 0
+ * @max 7
+ * @bit 0 Yaw
+ * @bit 1 Roll
+ * @bit 2 Pitch
 * @group VTOL Attitude Control
 */
 PARAM_DEFINE_INT32(VT_FW_DIFTHR_EN, 0);

 /**
- * Differential thrust scaling factor
+ * Roll differential thrust factor in forward flight
 *
- * This factor specifies how the yaw input gets mapped to differential thrust in forwards flight.
+ * Maps the roll control output in forward flight to the actuator differential thrust output.
+ *
+ * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN.
 *
 * @min 0.0
- * @max 1.0
+ * @max 2.0
 * @decimal 2
 * @increment 0.1
 * @group VTOL Attitude Control
 */
-PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f);
+PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_R, 1.f);
+
+/**
+ * Pitch differential thrust factor in forward flight
+ *
+ * Maps the pitch control output in forward flight to the actuator differential thrust output.
+ *
+ * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_P, 1.f);
+
+/**
+ * Yaw differential thrust factor in forward flight
+ *
+ * Maps the yaw control output in forward flight to the actuator differential thrust output.
+ *
+ * Differential thrust in forward flight is enabled via VT_FW_DIFTHR_EN.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 2
+ * @increment 0.1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_S_Y, 0.1f);

 /**
 * Backtransition deceleration setpoint to pitch feedforward gain.
--- a/src/modules/vtol_att_control/vtol_type.h
+++ b/src/modules/vtol_att_control/vtol_type.h
@ -77,6 +77,13 @@ enum VtolForwardActuationMode {
 	ENABLE_ABOVE_MPC_LAND_ALT2_WITHOUT_LAND
 };

+// enum for bitmask of VT_FW_DIFTHR_EN parameter options
+enum class VtFwDifthrEnBits : int32_t {
+	YAW_BIT = (1 << 0),
+	ROLL_BIT = (1 << 1),
+	PITCH_BIT = (1 << 2),
+};
+
 class VtolAttitudeControl;

 class VtolType : public ModuleParams
@ -239,8 +246,10 @@ protected:
 					(ParamBool<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
 					(ParamFloat<px4::params::VT_TRANS_TIMEOUT>) _param_vt_trans_timeout,
 					(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
-					(ParamBool<px4::params::VT_FW_DIFTHR_EN>) _param_vt_fw_difthr_en,
-					(ParamFloat<px4::params::VT_FW_DIFTHR_SC>) _param_vt_fw_difthr_sc,
+					(ParamInt<px4::params::VT_FW_DIFTHR_EN>) _param_vt_fw_difthr_en,
+					(ParamFloat<px4::params::VT_FW_DIFTHR_S_Y>) _param_vt_fw_difthr_s_y,
+					(ParamFloat<px4::params::VT_FW_DIFTHR_S_P>) _param_vt_fw_difthr_s_p,
+					(ParamFloat<px4::params::VT_FW_DIFTHR_S_R>) _param_vt_fw_difthr_s_r,
 					(ParamFloat<px4::params::VT_B_DEC_FF>) _param_vt_b_dec_ff,
 					(ParamFloat<px4::params::VT_B_DEC_I>) _param_vt_b_dec_i,
 					(ParamFloat<px4::params::VT_B_DEC_MSS>) _param_vt_b_dec_mss,
--- a/src/systemcmds/hardfault_log/hardfault_log.c
+++ b/src/systemcmds/hardfault_log/hardfault_log.c
@ -363,7 +363,7 @@ static int  write_stack(bool inValid, int winsize, uint32_t wtopaddr,
 						ret = -EIO;
 					}

-					wtopaddr--;
+					wtopaddr -= sizeof(stack_word_t);
 				}
 			}
 		}
@ -621,7 +621,7 @@ static int write_intterupt_stack(int fdin, int fdout, info_s *pi, char *buffer,
 		lseek(fdin, offsetof(fullcontext_s, istack), SEEK_SET);
 		ret = write_stack((pi->flags & eInvalidIntStackPrt) != 0,
 				  CONFIG_ISTACK_SIZE,
-				  pi->stacks.interrupt.sp + CONFIG_ISTACK_SIZE / 2,
+				  pi->stacks.interrupt.sp + (CONFIG_ISTACK_SIZE / 2) * sizeof(stack_word_t),
 				  pi->stacks.interrupt.top,
 				  pi->stacks.interrupt.sp,
 				  pi->stacks.interrupt.top - pi->stacks.interrupt.size,
@ -644,7 +644,7 @@ static int write_user_stack(int fdin, int fdout, info_s *pi, char *buffer,
 		lseek(fdin, offsetof(fullcontext_s, ustack), SEEK_SET);
 		ret = write_stack((pi->flags & eInvalidUserStackPtr) != 0,
 				  CONFIG_USTACK_SIZE,
-				  pi->stacks.user.sp + CONFIG_USTACK_SIZE / 2,
+				  pi->stacks.user.sp + (CONFIG_USTACK_SIZE / 2) * sizeof(stack_word_t),
 				  pi->stacks.user.top,
 				  pi->stacks.user.sp,
 				  pi->stacks.user.top - pi->stacks.user.size,
--- a/src/systemcmds/ver/ver.cpp
+++ b/src/systemcmds/ver/ver.cpp
@ -158,7 +158,7 @@ extern "C" __EXPORT int ver_main(int argc, char *argv[])
 			}

 			if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
-				printf("FW git-hash: %s\n", px4_firmware_version_string());
+				printf("PX4 git-hash: %s\n", px4_firmware_version_string());
 				unsigned fwver = px4_firmware_version();
 				unsigned major = (fwver >> (8 * 3)) & 0xFF;
 				unsigned minor = (fwver >> (8 * 2)) & 0xFF;
@ -166,17 +166,17 @@ extern "C" __EXPORT int ver_main(int argc, char *argv[])
 				unsigned type = (fwver >> (8 * 0)) & 0xFF;

 				if (type == 255) {
-					printf("FW version: Release %u.%u.%u (%u)\n", major, minor, patch, fwver);
+					printf("PX4 version: Release %u.%u.%u (%u)\n", major, minor, patch, fwver);

 				} else {
-					printf("FW version: %u.%u.%u %x (%u)\n", major, minor, patch, type, fwver);
+					printf("PX4 version: %u.%u.%u %x (%u)\n", major, minor, patch, type, fwver);
 				}

 				if (show_all) {
 					const char *git_branch = px4_firmware_git_branch();

 					if (git_branch && git_branch[0]) {
-						printf("FW git-branch: %s\n", git_branch);
+						printf("PX4 git-branch: %s\n", git_branch);
 					}
 				}

--- a/test/mavsdk_tests/autopilot_tester.cpp
+++ b/test/mavsdk_tests/autopilot_tester.cpp
@ -80,27 +80,22 @@ void AutopilotTester::connect(const std::string uri)

 void AutopilotTester::wait_until_ready()
 {
-	std::cout << time_str() << "Waiting for system to be ready" << std::endl;
+	std::cout << time_str() << "Waiting for system to be ready (system health ok & able to arm)" << std::endl;
+
+	// Wiat until the system is healthy
 	CHECK(poll_condition_with_timeout(
 	[this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(30)));

-	// FIXME: workaround to prevent race between PX4 switching to Hold mode
-	// and us trying to arm and take off. If PX4 is not in Hold mode yet,
-	// our arming presumably triggers a failsafe in manual mode.
-	std::this_thread::sleep_for(std::chrono::seconds(1));
-}
+	// Note: There is a known bug in MAVSDK (https://github.com/mavlink/MAVSDK/issues/1852),
+	// where `health_all_ok()` returning true doesn't actually mean vehicle is ready to accept
+	// global position estimate as valid (due to hysteresis). This needs to be fixed properly.

-void AutopilotTester::wait_until_ready_local_position_only()
-{
-	std::cout << time_str() << "Waiting for system to be ready" << std::endl;
+	// However, this is mitigated by the `is_armable` check below as a side effect, since
+	// when the vehicle considers global position to be valid, it will then allow arming
+
+	// Wait until we can arm
 	CHECK(poll_condition_with_timeout(
-	[this]() {
-		return
-			(_telemetry->health().is_gyrometer_calibration_ok &&
-			 _telemetry->health().is_accelerometer_calibration_ok &&
-			 _telemetry->health().is_magnetometer_calibration_ok &&
-			 _telemetry->health().is_local_position_ok);
-	}, std::chrono::seconds(20)));
+	[this]() {	return _telemetry->health().is_armable;	}, std::chrono::seconds(20)));
 }

 void AutopilotTester::store_home()
--- a/test/mavsdk_tests/autopilot_tester.h
+++ b/test/mavsdk_tests/autopilot_tester.h
@ -95,8 +95,12 @@ public:
 	~AutopilotTester();

 	void connect(const std::string uri);
+
+	/**
+	 * @brief Wait until vehicle's system status is healthy & is able to arm
+	 */
 	void wait_until_ready();
-	void wait_until_ready_local_position_only();
+
 	void store_home();
 	void check_home_within(float acceptance_radius_m);
 	void check_home_not_within(float min_distance_m);
@ -204,6 +208,12 @@ private:

 	void report_speed_factor();

+	/**
+	 * @brief Continue polling until condition returns true or we have a timeout
+	 *
+	 * @param fun Boolean returning function. When true, the polling terminates.
+	 * @param duration Timeout for polling in `std::chrono::` time unit
+	 */
 	template<typename Rep, typename Period>
 	bool poll_condition_with_timeout(
 		std::function<bool()> fun, std::chrono::duration<Rep, Period> duration)