AirspeedWing: log the airspeed innovation test ratio

- if the value exceeds 1, then airspeed checks will trigger - useful to monitoring the check and tuning Signed-off-by: RomanBapst <bapstroman@gmail.com>
params: COM_RC_LOSS_T: clarify safe adjustment of RC loss timeout (#25062 )
2026-08-17 01:40:35 +08:00 · 2025-06-17 09:22:32 +03:00 · 2025-06-16 18:50:43 -08:00 · 2025-06-17 13:37:47 +12:00 · 2025-06-16 14:59:03 -08:00 · 2025-06-16 14:58:38 -08:00
53 changed files with 1005 additions and 597 deletions
@@ -27,7 +27,7 @@ jobs:
          "failsafe_web",
        ]
    container:
-      image: px4io/px4-dev:v1.16.0-ondemand
+      image: px4io/px4-dev:v1.16.0-rc1-258-g0369abd556
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
      - name: Install Node v20.18.0
@@ -28,7 +28,7 @@ jobs:
    name: Analyzing ${{ matrix.target }}
    runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
    container:
-      image: px4io/px4-dev:v1.16.0-ondemand
+      image: px4io/px4-dev:v1.16.0-rc1-258-g0369abd556
    strategy:
      matrix:
        target: [px4_fmu-v5x, px4_fmu-v6x]
@@ -22,7 +22,7 @@ jobs:
    name: Checking ${{ matrix.target }}
    runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
    container:
-      image: px4io/px4-dev:v1.16.0-ondemand
+      image: px4io/px4-dev:v1.16.0-rc1-258-g0369abd556
    strategy:
      fail-fast: false
      matrix:
@@ -31,7 +31,7 @@ jobs:
    - name: Build PX4 and Run Test [${{ matrix.config }}]
      uses: addnab/docker-run-action@v3
      with:
-        image: px4io/px4-dev:v1.16.0-ondemand
+        image: px4io/px4-dev:v1.16.0-rc1-258-g0369abd556
        options: -v ${{ github.workspace }}:/workspace
        run: |
          cd /workspace
@@ -11,13 +11,11 @@ on:
    - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-    - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'

 jobs:
  build:
@@ -33,6 +31,12 @@ jobs:
    - name: Git Ownership Workaround
      run: git config --system --add safe.directory '*'

+    - name: Update ROS Keys
+      run: |
+        sudo rm /etc/apt/sources.list.d/ros2.list && \
+        sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+        echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
+
    - name: Install gazebo
      run: |
        apt update && apt install -y gazebo11 libgazebo11-dev gstreamer1.0-plugins-bad gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-ugly libgstreamer-plugins-base1.0-dev
@@ -6,6 +6,11 @@ CONFIG:
      buildType: RelWithDebInfo
      settings:
        CONFIG: px4_sitl_default
+    px4_sitl_spacecraft:
+      short: px4_sitl_spacecraft
+      buildType: RelWithDebInfo
+      settings:
+        CONFIG: px4_sitl_spacecraft
    px4_sitl_nolockstep:
      short: px4_sitl_nolockstep
      buildType: RelWithDebInfo
@@ -301,6 +306,11 @@ CONFIG:
      buildType: MinSizeRel
      settings:
        CONFIG: holybro_durandal-v1_default
+    holybro_kakuteh7-wing_default:
+      short: holybro_kakuteh7-wing
+      buildType: MinSizeRel
+      settings:
+        CONFIG: holybro_kakuteh7-wing_default
    holybro_kakuteh7dualimu_default:
      short: holybro_kakuteh7dualimu
      buildType: MinSizeRel
@@ -216,12 +216,33 @@ foreach(board_extra_file ${OPTIONAL_BOARD_EXTRAS})
 		if(CONFIG_SYSTEMCMDS_BL_UPDATE)
 			# generate rc.board_bootloader_upgrade
 			set(BOARD_FIRMWARE_BIN "${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin")
-			configure_file(${PX4_SOURCE_DIR}/platforms/nuttx/init/rc.board_bootloader_upgrade.in ${romfs_gen_root_dir}/init.d/rc.board_bootloader_upgrade @ONLY)
+			message(STATUS "ROMFS:  Adding platforms/nuttx/init/rc.board_bootloader_upgrade -> /etc/init.d/rc.board_bootloader_upgrade")
+
+			# Generate the file using configure_file at configure time to a temporary location
+			set(bootloader_upgrade_tmp ${CMAKE_CURRENT_BINARY_DIR}/rc.board_bootloader_upgrade.tmp)
+			configure_file(${PX4_SOURCE_DIR}/platforms/nuttx/init/rc.board_bootloader_upgrade.in ${bootloader_upgrade_tmp} @ONLY)
+
+			# Then copy it at build time with proper dependencies
+			add_custom_command(
+				OUTPUT
+					${romfs_gen_root_dir}/init.d/rc.board_bootloader_upgrade
+					rc.board_bootloader_upgrade.stamp
+				COMMAND ${CMAKE_COMMAND} -E copy_if_different ${bootloader_upgrade_tmp} ${romfs_gen_root_dir}/init.d/rc.board_bootloader_upgrade
+				COMMAND ${CMAKE_COMMAND} -E touch rc.board_bootloader_upgrade.stamp
+				DEPENDS
+					${bootloader_upgrade_tmp}
+					${PX4_SOURCE_DIR}/platforms/nuttx/init/rc.board_bootloader_upgrade.in
+					romfs_copy.stamp
+				COMMENT "ROMFS: copying rc.board_bootloader_upgrade"
+			)
+
+			list(APPEND extras_dependencies
+				rc.board_bootloader_upgrade.stamp
+			)
 		else()
 			# remove bootloader from extras
 			list(REMOVE_ITEM OPTIONAL_BOARD_EXTRAS ${board_extra_file})
 		endif()
-
 	endif()
 endforeach()

@@ -1,155 +0,0 @@
-#!/bin/sh
-#
-# @name 6DoF Spacecraft Model
-#
-# @type Freeflyer with 8 thrusters
-#
-# @maintainer Pedro Roque <padr@kth.se>
-#
-
-. ${R}etc/init.d/rc.sc_defaults
-
-param set-default CA_AIRFRAME 15
-param set-default MAV_TYPE 99
-
-param set-default CA_THRUSTER_CNT 12
-param set-default CA_R_REV 0
-
-# param set-default FW_ARSP_MODE 1
-
-# Auto to be provided by Custom Airframe
-param set-default CA_METHOD 0
-
-# disable attitude failure detection
-param set-default FD_FAIL_P 0
-param set-default FD_FAIL_R 0
-
-# Set proper failsafes
-param set-default COM_ACT_FAIL_ACT 0
-param set-default COM_LOW_BAT_ACT 0
-param set-default NAV_DLL_ACT 0
-param set-default GF_ACTION 1
-param set-default NAV_RCL_ACT 1
-param set-default COM_POSCTL_NAVL 2
-
-# Set thrusters
-param set-default CA_THRUSTER0_PX -0.50
-param set-default CA_THRUSTER0_PY 0.50
-param set-default CA_THRUSTER0_PZ 0.0
-param set-default CA_THRUSTER0_CT 0.237
-param set-default CA_THRUSTER0_AX 0.0
-param set-default CA_THRUSTER0_AY -1.0
-param set-default CA_THRUSTER0_AZ 0.0
-
-param set-default CA_THRUSTER1_PX 0.50
-param set-default CA_THRUSTER1_PY 0.50
-param set-default CA_THRUSTER1_PZ 0.0
-param set-default CA_THRUSTER1_CT 0.237
-param set-default CA_THRUSTER1_AX 0.0
-param set-default CA_THRUSTER1_AY -1.0
-param set-default CA_THRUSTER1_AZ 0.0
-
-param set-default CA_THRUSTER2_PX 0.50
-param set-default CA_THRUSTER2_PY -0.50
-param set-default CA_THRUSTER2_PZ 0.0
-param set-default CA_THRUSTER2_CT 0.237
-param set-default CA_THRUSTER2_AX 0.0
-param set-default CA_THRUSTER2_AY 1.0
-param set-default CA_THRUSTER2_AZ 0.0
-
-param set-default CA_THRUSTER3_PX -0.50
-param set-default CA_THRUSTER3_PY -0.50
-param set-default CA_THRUSTER3_PZ 0.0
-param set-default CA_THRUSTER3_CT 0.237
-param set-default CA_THRUSTER3_AX 0.0
-param set-default CA_THRUSTER3_AY 1.0
-param set-default CA_THRUSTER3_AZ 0.0
-
-param set-default CA_THRUSTER4_PX -0.50
-param set-default CA_THRUSTER4_PY 0.0
-param set-default CA_THRUSTER4_PZ -0.50
-param set-default CA_THRUSTER4_CT 0.237
-param set-default CA_THRUSTER4_AX 1.0
-param set-default CA_THRUSTER4_AY 0.0
-param set-default CA_THRUSTER4_AZ 0.0
-
-param set-default CA_THRUSTER5_PX 0.50
-param set-default CA_THRUSTER5_PY 0.0
-param set-default CA_THRUSTER5_PZ -0.50
-param set-default CA_THRUSTER5_CT 0.237
-param set-default CA_THRUSTER5_AX -1.0
-param set-default CA_THRUSTER5_AY 0.0
-param set-default CA_THRUSTER5_AZ 0.0
-
-param set-default CA_THRUSTER6_PX 0.50
-param set-default CA_THRUSTER6_PY 0.0
-param set-default CA_THRUSTER6_PZ 0.50
-param set-default CA_THRUSTER6_CT 0.237
-param set-default CA_THRUSTER6_AX -1.0
-param set-default CA_THRUSTER6_AY 0.0
-param set-default CA_THRUSTER6_AZ 0.0
-
-param set-default CA_THRUSTER7_PX -0.50
-param set-default CA_THRUSTER7_PY 0.0
-param set-default CA_THRUSTER7_PZ 0.50
-param set-default CA_THRUSTER7_CT 0.237
-param set-default CA_THRUSTER7_AX 1.0
-param set-default CA_THRUSTER7_AY 0.0
-param set-default CA_THRUSTER7_AZ 0.0
-
-param set-default CA_THRUSTER8_PX 0.0
-param set-default CA_THRUSTER8_PY -0.50
-param set-default CA_THRUSTER8_PZ -0.50
-param set-default CA_THRUSTER8_CT 0.237
-param set-default CA_THRUSTER8_AX 0.0
-param set-default CA_THRUSTER8_AY 0.0
-param set-default CA_THRUSTER8_AZ 1.0
-
-param set-default CA_THRUSTER9_PX 0.0
-param set-default CA_THRUSTER9_PY 0.50
-param set-default CA_THRUSTER9_PZ -0.50
-param set-default CA_THRUSTER9_CT 0.237
-param set-default CA_THRUSTER9_AX 0.0
-param set-default CA_THRUSTER9_AY 0.0
-param set-default CA_THRUSTER9_AZ 1.0
-
-param set-default CA_THRUSTER10_PX 0.0
-param set-default CA_THRUSTER10_PY 0.50
-param set-default CA_THRUSTER10_PZ 0.50
-param set-default CA_THRUSTER10_CT 0.237
-param set-default CA_THRUSTER10_AX 0.0
-param set-default CA_THRUSTER10_AY 0.0
-param set-default CA_THRUSTER10_AZ -1.0
-
-param set-default CA_THRUSTER11_PX 0.0
-param set-default CA_THRUSTER11_PY -0.50
-param set-default CA_THRUSTER11_PZ 0.50
-param set-default CA_THRUSTER11_CT 0.237
-param set-default CA_THRUSTER11_AX 0.0
-param set-default CA_THRUSTER11_AY 0.0
-param set-default CA_THRUSTER11_AZ -1.0
-
-param set-default PWM_MAIN_FUNC1 101
-param set-default PWM_MAIN_FUNC2 102
-param set-default PWM_MAIN_FUNC3 103
-param set-default PWM_MAIN_FUNC4 104
-param set-default PWM_MAIN_FUNC5 105
-param set-default PWM_MAIN_FUNC6 106
-param set-default PWM_MAIN_FUNC7 107
-param set-default PWM_MAIN_FUNC8 108
-param set-default PWM_MAIN_FUNC9 109
-param set-default PWM_MAIN_FUNC10 110
-param set-default PWM_MAIN_FUNC11 111
-param set-default PWM_MAIN_FUNC12 112
-
-# PWM Simulation
-param set PWM_SIM_PWM_MAX 10000
-param set PWM_SIM_PWM_MIN 0
-
-# Controller Tunings
-param set-default SC_ROLLRATE_P 0.14
-param set-default SC_PITCHRATE_P 0.14
-param set-default SC_ROLLRATE_I 0.3
-param set-default SC_PITCHRATE_I 0.3
-param set-default SC_ROLLRATE_D 0.004
-param set-default SC_PITCHRATE_D 0.004
@@ -20,7 +20,7 @@ param set-default COM_ARM_CHK_ESCS 0  # We don't have ESCs
 param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later?

 param set-default CA_AIRFRAME 14
-param set-default MAV_TYPE 99
+param set-default MAV_TYPE 45

 param set-default CA_THRUSTER_CNT 8
 param set-default CA_R_REV 0
@@ -114,7 +114,6 @@ px4_add_romfs_files(
 	17001_flightgear_tf-g1
 	17002_flightgear_tf-g2

-	71001_gazebo-classic_spacecraft_dart
 	71002_gz_spacecraft_2d

 	# [22000, 22999] Reserve for custom models
@@ -11,7 +11,7 @@
 . ${R}etc/init.d/rc.sc_defaults

 param set-default CA_AIRFRAME 14
-param set-default MAV_TYPE 99
+param set-default MAV_TYPE 45

 param set-default CA_THRUSTER_CNT 8
 param set-default CA_R_REV 0
@@ -3,10 +3,10 @@
 # NOTE: Script variables are declared/initialized/unset in the rcS script.
 #

-set VEHICLE_TYPE sc
+set VEHICLE_TYPE spacecraft

-# MAV_TYPE_QUADROTOR 2
-#param set-default MAV_TYPE 12
+# MAV_TYPE_SPACECRAFT_ORBITTER
+param set-default MAV_TYPE 45

 # Set micro-dds-client to use ethernet and IP-address 192.168.0.1
 param set-default UXRCE_DDS_AG_IP -1062731775
@@ -68,6 +68,15 @@ then
 	. ${R}etc/init.d/rc.vtol_apps
 fi

+#
+# Spapcecraft setup.
+#
+if [ $VEHICLE_TYPE = spacecraft ]
+then
+	# Start standard multicopter apps.
+	. ${R}etc/init.d/rc.sc_apps
+fi
+
 #
 # Airship setup.
 #
@@ -280,6 +280,14 @@ else
 	#
 	send_event start

+	#
+	# Start the hardfault streamer.
+	#
+	if param compare -s SYS_HF_MAV 1
+	then
+		hardfault_stream start
+	fi
+
 	#
 	# Start the resource load monitor.
 	#
@@ -35,6 +35,7 @@ if args.filter:
    for board in args.filter.split(','):
        board_filter.append(board)

+default_container = 'ghcr.io/px4/px4-dev:v1.16.0-rc1-258-g0369abd556'
 build_configs = []
 grouped_targets = {}
 excluded_boards = ['modalai_voxl2', 'px4_ros2']  # TODO: fix and enable
@@ -86,7 +87,7 @@ def process_target(px4board_file, target_name):
    assert platform, f"PLATFORM not found in {px4board_file}"

    if platform not in excluded_platforms:
-        container = 'ghcr.io/px4/px4-dev:v1.16.0-ondemand'
+        container = default_container
        if platform == 'posix':
            group = 'base'
            if toolchain:
@@ -120,7 +121,7 @@ if(verbose):
 # - Events
 metadata_targets = ['airframe_metadata', 'parameters_metadata', 'extract_events']
 grouped_targets['base'] = {}
-grouped_targets['base']['container'] = 'ghcr.io/px4/px4-dev:v1.16.0-ondemand'
+grouped_targets['base']['container'] = default_container
 grouped_targets['base']['manufacturers'] = {}
 grouped_targets['base']['manufacturers']['px4'] = []
 grouped_targets['base']['manufacturers']['px4'] += metadata_targets
@@ -15,7 +15,7 @@ fi

 # otherwise default to nuttx
 if [ -z ${PX4_DOCKER_REPO+x} ]; then
-	PX4_DOCKER_REPO="px4io/px4-dev:v1.16.0-ondemand"
+	PX4_DOCKER_REPO="px4io/px4-dev:v1.16.0-rc1-258-g0369abd556"
 fi

 echo "PX4_DOCKER_REPO: $PX4_DOCKER_REPO";
@@ -27,3 +27,4 @@ six>=1.12.0
 toml>=0.9
 sympy>=1.10.1
 pycryptodome
+lark
@@ -62,6 +62,7 @@ CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_HARDFAULT_STREAM=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -43,6 +43,7 @@ CONFIG_FIGURE_OF_EIGHT=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_HARDFAULT_STREAM=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -5,6 +5,7 @@ CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS2"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS3"
 CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS5"
+CONFIG_BOARD_SERIAL_RC="/dev/ttyS4"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP280=y
 CONFIG_DRIVERS_BAROMETER_GOERTEK_SPA06=y
@@ -74,7 +74,7 @@
 #define BOARD_TYPE                     1105
 #define BOARD_FLASH_SECTORS            (14)
 #define BOARD_FLASH_SIZE               (16 * 128 * 1024)
-#define APP_RESERVATION_SIZE           (1 * 128 * 1024)
+#define APP_RESERVATION_SIZE           (2 * 128 * 1024)

 #define OSC_FREQ                       16

@@ -64,6 +64,7 @@ CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_HARDFAULT_STREAM=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -62,6 +62,7 @@ CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_HARDFAULT_STREAM=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -63,6 +63,7 @@ CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_HARDFAULT_STREAM=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -9,6 +9,7 @@ float32 variance_east		# Wind estimate error variance in east / Y direction (m/s

 float32 tas_innov 		# True airspeed innovation
 float32 tas_innov_var 		# True airspeed innovation variance
+float32 tas_innov_integ_test_ratio  # Value > 1 indicates that innovation failure check has triggered

 float32 tas_scale_raw 		# Estimated true airspeed scale factor (not validated)
 float32 tas_scale_raw_var 	# True airspeed scale factor variance
@@ -89,6 +89,7 @@ uint8 HIL_STATE_ON = 1

 # Current vehicle locomotion method. A vehicle can have different methods (e.g. VTOL transitions from RW to FW method)
 uint8 vehicle_type
+uint8 VEHICLE_TYPE_UNSPECIFIED = 0
 uint8 VEHICLE_TYPE_ROTARY_WING = 1
 uint8 VEHICLE_TYPE_FIXED_WING = 2
 uint8 VEHICLE_TYPE_ROVER = 3
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -31,27 +31,21 @@
 *
 ****************************************************************************/

-/* Include Files */
 #include "AFBRS50.hpp"
-#include "argus_hal_test.h"
+#include "s2pi.h"

 #include <lib/drivers/device/Device.hpp>
-
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/module.h>

-/*! Define the SPI baud rate (to be used in the SPI module). */
-#define SPI_BAUD_RATE 5000000
-
-#include "s2pi.h"
-#include "timer.h"
-#include "argus_hal_test.h"
+using namespace time_literals;

 AFBRS50 *g_dev{nullptr};

 AFBRS50::AFBRS50(uint8_t device_orientation):
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
+	// ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::SPI6),
 	_px4_rangefinder(0, device_orientation)
 {
 	device::Device::DeviceId device_id{};
@@ -65,278 +59,263 @@ AFBRS50::AFBRS50(uint8_t device_orientation):

 AFBRS50::~AFBRS50()
 {
-	stop();
+	ScheduleClear();
+
+	Argus_StopMeasurementTimer(_hnd);
+	Argus_Deinit(_hnd);
+	Argus_DestroyHandle(_hnd);

 	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+	perf_free(_not_ready_perf);
 }

-status_t AFBRS50::measurement_ready_callback(status_t status, argus_hnd_t *hnd)
+status_t AFBRS50::measurementReadyCallback(status_t status, argus_hnd_t *hnd)
 {
-	if (!up_interrupt_context()) {
-		if (status == STATUS_OK) {
-			if (g_dev) {
-				g_dev->ProcessMeasurement(hnd);
-			}
+	// Called from the SPI comms thread context

-		} else {
-			PX4_ERR("Measurement Ready Callback received error!: %i", (int)status);
-		}
+	if (up_interrupt_context()) {
+		// We cannot be in interrupt context
+		g_dev->recordCommsError();
+		return ERROR_FAIL;
 	}

+	if ((g_dev == nullptr) || (status != STATUS_OK)) {
+		g_dev->recordCommsError();
+		return ERROR_FAIL;
+	}
+
+	g_dev->scheduleCollect();
+
 	return status;
 }

-void AFBRS50::ProcessMeasurement(argus_hnd_t *hnd)
+void AFBRS50::scheduleCollect()
+{
+	_state = STATE::COLLECT;
+	ScheduleNow();
+}
+
+void AFBRS50::recordCommsError()
+{
+	perf_count(_comms_errors);
+}
+
+void AFBRS50::processMeasurement()
 {
 	perf_count(_sample_perf);

 	argus_results_t res{};
-	status_t evaluate_status = Argus_EvaluateData(hnd, &res);
+	status_t evaluate_status = Argus_EvaluateData(_hnd, &res);

-	if ((evaluate_status == STATUS_OK) && (res.Status == STATUS_OK)) {
-		uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000);
-		float result_m = static_cast<float>(result_mm) / 1000.f;
-		int8_t quality = res.Bin.SignalQuality;
-
-		// Signal quality indicates 100% for good signals, 50% and lower for weak signals.
-		// 1% is an errored signal (not reliable). Signal Quality of 0% is unknown.
-		if (quality == 1) {
-			quality = 0;
-		}
-
-		// distance quality check
-		if (result_m > _max_distance) {
-			result_m = 0.0;
-			quality = 0;
-		}
-
-		_current_distance = result_m;
-		_current_quality = quality;
-		_px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), result_m, quality);
+	if ((evaluate_status != STATUS_OK) || (res.Status != STATUS_OK)) {
+		perf_count(_comms_errors);
+		return;
 	}
+
+	uint32_t result_mm = res.Bin.Range / (Q9_22_ONE / 1000);
+	float distance = static_cast<float>(result_mm) / 1000.f;
+	int8_t quality = res.Bin.SignalQuality;
+
+	// Signal quality indicates 100% for good signals, 50% and lower for weak signals.
+	// 1% is an errored signal (not reliable). Signal Quality of 0% is unknown.
+	if (quality == 1) {
+		quality = 0;
+	}
+
+	// TODO: I don't think we need this..
+	if (distance > _max_distance) {
+		distance = 0.0;
+		quality = 0;
+	}
+
+	_current_distance = distance;
+	_current_quality = quality;
+	_px4_rangefinder.update(((res.TimeStamp.sec * 1000000ULL) + res.TimeStamp.usec), distance, quality);
 }

 int AFBRS50::init()
 {
-	// Retry initialization 3 times
-	for (int32_t i = 0; i < 3; i++) {
-		if (_hnd != nullptr) {
-			// retry
-			Argus_Deinit(_hnd);
-			Argus_DestroyHandle(_hnd);
-			_hnd = nullptr;
-		}
-
-		_hnd = Argus_CreateHandle();
-
-		if (_hnd == nullptr) {
-			PX4_ERR("Handle not initialized");
-			return PX4_ERROR;
-		}
-
-		// Initialize the S2PI hardware required by the API.
-		S2PI_Init(BROADCOM_AFBR_S50_S2PI_SPI_BUS, SPI_BAUD_RATE);
-
-		int32_t mode_param = _p_sens_afbr_mode.get();
-
-		if (mode_param < 0 || mode_param > 3) {
-			PX4_ERR("Invalid mode parameter: %li", mode_param);
-			return PX4_ERROR;
-		}
-
-		argus_mode_t mode = ARGUS_MODE_SHORT_RANGE;
-
-		switch (mode_param) {
-		case 0:
-			mode = ARGUS_MODE_SHORT_RANGE;
-			break;
-
-		case 1:
-			mode = ARGUS_MODE_LONG_RANGE;
-			break;
-
-		case 2:
-			mode = ARGUS_MODE_HIGH_SPEED_SHORT_RANGE;
-			break;
-
-		case 3:
-			mode = ARGUS_MODE_HIGH_SPEED_LONG_RANGE;
-			break;
-
-		default:
-			break;
-		}
-
-		status_t status = Argus_InitMode(_hnd, BROADCOM_AFBR_S50_S2PI_SPI_BUS, mode);
-
-		if (status == STATUS_OK) {
-			uint32_t id = Argus_GetChipID(_hnd);
-			uint32_t value = Argus_GetAPIVersion();
-			uint8_t a = (value >> 24) & 0xFFU;
-			uint8_t b = (value >> 16) & 0xFFU;
-			uint8_t c = value & 0xFFFFU;
-			PX4_INFO_RAW("AFBR-S50 Chip ID: %u, API Version: %u v%d.%d.%d\n", (uint)id, (uint)value, a, b, c);
-
-			argus_module_version_t mv = Argus_GetModuleVersion(_hnd);
-
-			switch (mv) {
-			case AFBR_S50MV85G_V1:
-
-			// FALLTHROUGH
-			case AFBR_S50MV85G_V2:
-
-			// FALLTHROUGH
-			case AFBR_S50MV85G_V3:
-				_min_distance = 0.0f;
-				_max_distance = 10.f;
-				_px4_rangefinder.set_min_distance(_min_distance);
-				_px4_rangefinder.set_max_distance(_max_distance);
-				_px4_rangefinder.set_fov(math::radians(6.f));
-				PX4_INFO_RAW("AFBR-S50MV85G\n");
-				break;
-
-			case AFBR_S50LV85D_V1:
-				_min_distance = 0.0f;
-				_max_distance = 30.f;
-				_px4_rangefinder.set_min_distance(_min_distance);
-				_px4_rangefinder.set_max_distance(_max_distance);
-				_px4_rangefinder.set_fov(math::radians(6.f));
-				PX4_INFO_RAW("AFBR-S50LV85D\n");
-				break;
-
-			case AFBR_S50LX85D_V1:
-				_min_distance = 0.0f;
-				_max_distance = 50.f;
-				_px4_rangefinder.set_min_distance(_min_distance);
-				_px4_rangefinder.set_max_distance(_max_distance);
-				_px4_rangefinder.set_fov(math::radians(6.f));
-				PX4_INFO_RAW("AFBR-S50LX85D\n");
-				break;
-
-			case AFBR_S50MV68B_V1:
-				_min_distance = 0.0f;
-				_max_distance = 10.f;
-				_px4_rangefinder.set_min_distance(_min_distance);
-				_px4_rangefinder.set_max_distance(_max_distance);
-				_px4_rangefinder.set_fov(math::radians(1.f));
-				PX4_INFO_RAW("AFBR-S50MV68B (v1)\n");
-				break;
-
-			case AFBR_S50MV85I_V1:
-				_min_distance = 0.0f;
-				_max_distance = 5.f;
-				_px4_rangefinder.set_min_distance(_min_distance);
-				_px4_rangefinder.set_max_distance(_max_distance);
-				_px4_rangefinder.set_fov(math::radians(6.f));
-				PX4_INFO_RAW("AFBR-S50MV85I (v1)\n");
-				break;
-
-			case AFBR_S50SV85K_V1:
-				_min_distance = 0.0f;
-				_max_distance = 10.f;
-				_px4_rangefinder.set_min_distance(_min_distance);
-				_px4_rangefinder.set_max_distance(_max_distance);
-				_px4_rangefinder.set_fov(math::radians(4.f));
-				PX4_INFO_RAW("AFBR-S50SV85K (v1)\n");
-				break;
-
-			default:
-				break;
-			}
-
-			if (_testing) {
-				_state = STATE::TEST;
-
-			} else {
-				_state = STATE::CONFIGURE;
-			}
-
-			ScheduleDelayed(_measure_interval);
-			return PX4_OK;
-
-		} else {
-			PX4_ERR("Argus_InitMode failed: %ld", status);
-		}
+	if (hrt_absolute_time() < 1_ms) {
+		PX4_WARN("Power-up time requires at least 1ms!");
 	}

-	return PX4_ERROR;
-}
-
-void AFBRS50::Run()
-{
-	if (_parameter_update_sub.updated()) {
-		// clear update
-		parameter_update_s param_update;
-		_parameter_update_sub.copy(&param_update);
-
-		// update parameters from storage
-		ModuleParams::updateParams();
+	if (_hnd != nullptr) {
+		Argus_Deinit(_hnd);
+		Argus_DestroyHandle(_hnd);
+		_hnd = nullptr;
 	}

-	switch (_state) {
-	case STATE::TEST: {
-			if (_testing) {
-				Argus_VerifyHALImplementation(Argus_GetSPISlave(_hnd));
-				_testing = false;
+	_hnd = Argus_CreateHandle();

-			} else {
-				_state = STATE::CONFIGURE;
-			}
-		}
+	if (_hnd == nullptr) {
+		PX4_ERR("Handle not initialized");
+		return PX4_ERROR;
+	}
+
+	// Initialize the S2PI hardware required by the API.
+	static constexpr uint32_t SPI_BAUD_RATE = 5000000;
+	S2PI_Init(BROADCOM_AFBR_S50_S2PI_SPI_BUS, SPI_BAUD_RATE);
+
+	// Initialize device with initial mode
+	status_t status = Argus_InitMode(_hnd, BROADCOM_AFBR_S50_S2PI_SPI_BUS, argusModeFromParameter());
+
+	if (status != STATUS_OK) {
+		PX4_ERR("Argus_InitMode failed: %ld", status);
+		return PX4_ERROR;
+	}
+
+	uint32_t id = Argus_GetChipID(_hnd);
+	uint32_t value = Argus_GetAPIVersion();
+	uint8_t a = (value >> 24) & 0xFFU;
+	uint8_t b = (value >> 16) & 0xFFU;
+	uint8_t c = value & 0xFFFFU;
+	PX4_INFO("AFBR-S50 Chip ID: %u, API Version: %u v%d.%d.%d", (uint)id, (uint)value, a, b, c);
+
+	char module_string[20] = {};
+	argus_module_version_t mv = Argus_GetModuleVersion(_hnd);
+
+	float min_distance = 0.f;
+	float max_distance = 30.f;
+	float fov_degrees = 6.f;
+
+	switch (mv) {
+	case AFBR_S50MV85G_V1:
+	case AFBR_S50MV85G_V2:
+	case AFBR_S50MV85G_V3:
+		max_distance = 10.f;
+		fov_degrees = 6.f;
+		snprintf(module_string, sizeof(module_string), "AFBR-S50MV85G");
 		break;

-	case STATE::CONFIGURE: {
-			_current_rate = (uint32_t)_p_sens_afbr_s_rate.get();
-			status_t status = set_rate_and_dfm(_current_rate, DFM_MODE_OFF);
-
-			if (status != STATUS_OK) {
-				PX4_ERR("CONFIGURE status not okay: %i", (int)status);
-				_state = STATE::STOP;
-				ScheduleNow();
-			}
-
-			status = Argus_SetConfigurationSmartPowerSaveEnabled(_hnd, false);
-
-			if (status != STATUS_OK) {
-				PX4_ERR("Argus_SetConfigurationSmartPowerSaveEnabled status not okay: %i", (int)status);
-				ScheduleNow();
-
-			} else {
-				_state = STATE::COLLECT;
-				ScheduleDelayed(_measure_interval);
-			}
-		}
+	case AFBR_S50LV85D_V1:
+		max_distance = 30.f;
+		fov_degrees = 6.f;
+		snprintf(module_string, sizeof(module_string), "AFBR-S50LV85D");
 		break;

-	case STATE::COLLECT: {
-			// Only start a new measurement if one is not ongoing
-			if (Argus_GetStatus(_hnd) == STATUS_IDLE) {
-				status_t status = Argus_TriggerMeasurement(_hnd, measurement_ready_callback);
-
-				if (status != STATUS_OK) {
-					PX4_ERR("Argus_TriggerMeasurement status not okay: %i", (int)status);
-				}
-			}
-
-			Evaluate_rate();
-		}
+	case AFBR_S50LX85D_V1:
+		max_distance = 50.f;
+		fov_degrees = 6.f;
+		snprintf(module_string, sizeof(module_string), "AFBR-S50LX85D");
 		break;

-	case STATE::STOP: {
-			Argus_StopMeasurementTimer(_hnd);
-			Argus_Deinit(_hnd);
-			Argus_DestroyHandle(_hnd);
-		}
+	case AFBR_S50MV68B_V1:
+		max_distance = 10.f;
+		fov_degrees = 1.f;
+		snprintf(module_string, sizeof(module_string), "AFBR-S50MV68B");
+		break;
+
+	case AFBR_S50MV85I_V1:
+		max_distance = 5.f;
+		fov_degrees = 6.f;
+		snprintf(module_string, sizeof(module_string), "AFBR-S50MV85I");
+		break;
+
+	case AFBR_S50SV85K_V1:
+		max_distance = 10.f;
+		fov_degrees = 4.f;
+		snprintf(module_string, sizeof(module_string), "AFBR-S50SV85K");
 		break;

 	default:
 		break;
 	}

-	ScheduleDelayed(_measure_interval);
+	PX4_INFO("Module: %s", module_string);
+	_max_distance = max_distance;
+	_px4_rangefinder.set_min_distance(min_distance);
+	_px4_rangefinder.set_max_distance(max_distance);
+	_px4_rangefinder.set_fov(math::radians(fov_degrees));
+
+	_state = STATE::CONFIGURE;
+	// Initialization Time is 300ms
+	ScheduleDelayed(350_ms);
+	return PX4_OK;
 }

-void AFBRS50::Evaluate_rate()
+void AFBRS50::Run()
+{
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s param_update;
+		_parameter_update_sub.copy(&param_update);
+		ModuleParams::updateParams();
+	}
+
+	switch (_state) {
+	case STATE::CONFIGURE: {
+			_current_rate = (uint32_t)_p_sens_afbr_s_rate.get();
+			status_t status = setRateAndDfm(_current_rate, DFM_MODE_OFF);
+
+			if (status != STATUS_OK) {
+				PX4_ERR("CONFIGURE status not okay: %i", (int)status);
+				ScheduleDelayed(350_ms);
+				return;
+			}
+
+			status = Argus_SetConfigurationSmartPowerSaveEnabled(_hnd, false);
+
+			if (status != STATUS_OK) {
+				PX4_ERR("Argus_SetConfigurationSmartPowerSaveEnabled status not okay: %i", (int)status);
+				// TODO: delay?
+				ScheduleNow();
+				return;
+			}
+
+			// Enable interrupt on falling edge
+			px4_arch_configgpio(BROADCOM_AFBR_S50_S2PI_IRQ);
+			_state = STATE::TRIGGER;
+			// TODO: delay after configure?
+			ScheduleNow();
+			// ScheduleDelayed(50_ms);
+		}
+		break;
+
+	case STATE::TRIGGER: {
+			if (Argus_GetStatus(_hnd) != STATUS_IDLE) {
+				perf_count(_not_ready_perf);
+				ScheduleDelayed(10_ms);
+				return;
+			}
+
+			// Trigger continuous measurement mode. An hrt_call_after will trigger
+			// measurements periodically -- see API/Src/timer.c
+			status_t status = Argus_StartMeasurementTimer(_hnd, measurementReadyCallback);
+
+			if (status != STATUS_OK) {
+				PX4_ERR("Argus_TriggerMeasurement status not okay: %i", (int)status);
+				perf_count(_not_ready_perf);
+				ScheduleDelayed(50_ms);
+			}
+		}
+		break;
+
+	case STATE::COLLECT: {
+			processMeasurement();
+
+			// Change measurement rate and mode based on range
+			updateMeasurementRateFromRange();
+
+			// Rechedule watchdog, push back by 2x measurement rate
+			_state = STATE::WATCHDOG;
+			ScheduleDelayed(_measurement_inverval * 2);
+		}
+		break;
+
+	case STATE::WATCHDOG: {
+			PX4_WARN("watchdog triggered, rescheduling");
+			_state = STATE::TRIGGER;
+			// When this occurs the device locks up for ~160ms
+			ScheduleDelayed(160_ms);
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+void AFBRS50::updateMeasurementRateFromRange()
 {
 	// only update mode if _current_distance is a valid measurement and if the last rate switch was more than 1 second ago
 	if ((_current_distance > 0) && (_current_quality > 0) && ((hrt_absolute_time() - _last_rate_switch) > 1_s)) {
@@ -347,7 +326,7 @@ void AFBRS50::Evaluate_rate()
 		    && (_current_rate != (uint32_t)_p_sens_afbr_l_rate.get())) {

 			_current_rate = (uint32_t)_p_sens_afbr_l_rate.get();
-			status = set_rate_and_dfm(_current_rate, DFM_MODE_8X);
+			status = setRateAndDfm(_current_rate, DFM_MODE_8X);

 			if (status != STATUS_OK) {
 				PX4_ERR("set_rate status not okay: %i", (int)status);
@@ -361,7 +340,7 @@ void AFBRS50::Evaluate_rate()
 			   && (_current_rate != (uint32_t)_p_sens_afbr_s_rate.get())) {

 			_current_rate = (uint32_t)_p_sens_afbr_s_rate.get();
-			status = set_rate_and_dfm(_current_rate, DFM_MODE_OFF);
+			status = setRateAndDfm(_current_rate, DFM_MODE_OFF);

 			if (status != STATUS_OK) {
 				PX4_ERR("set_rate status not okay: %i", (int)status);
@@ -374,28 +353,7 @@ void AFBRS50::Evaluate_rate()
 	}
 }

-void AFBRS50::stop()
-{
-	_state = STATE::STOP;
-	ScheduleNow();
-}
-
-int AFBRS50::test()
-{
-	_testing = true;
-
-	init();
-
-	return PX4_OK;
-}
-
-void AFBRS50::print_info()
-{
-	perf_print_counter(_sample_perf);
-	get_info();
-}
-
-status_t AFBRS50::set_rate_and_dfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode)
+status_t AFBRS50::setRateAndDfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode)
 {
 	while (Argus_GetStatus(_hnd) != STATUS_IDLE) {
 		px4_usleep(1_ms);
@@ -423,20 +381,53 @@ status_t AFBRS50::set_rate_and_dfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode)
 		return status;

 	} else {
-		_measure_interval = current_rate;
+		_measurement_inverval = current_rate;
 	}

 	return status;
 }

-void AFBRS50::get_info()
+argus_mode_t AFBRS50::argusModeFromParameter()
 {
-	argus_dfm_mode_t dfm_mode;
-	Argus_GetConfigurationDFMMode(_hnd, &dfm_mode);
+	int32_t mode_param = _p_sens_afbr_mode.get();
+	argus_mode_t mode = ARGUS_MODE_SHORT_RANGE;

+	if (mode_param < 0 || mode_param > 3) {
+		PX4_ERR("Invalid mode parameter: %li", mode_param);
+		return mode;
+	}
+
+	switch (mode_param) {
+	case 0:
+		mode = ARGUS_MODE_SHORT_RANGE;
+		break;
+
+	case 1:
+		mode = ARGUS_MODE_LONG_RANGE;
+		break;
+
+	case 2:
+		mode = ARGUS_MODE_HIGH_SPEED_SHORT_RANGE;
+		break;
+
+	case 3:
+		mode = ARGUS_MODE_HIGH_SPEED_LONG_RANGE;
+		break;
+
+	default:
+		break;
+	}
+
+	return mode;
+}
+
+void AFBRS50::printInfo()
+{
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+	perf_print_counter(_not_ready_perf);
 	PX4_INFO_RAW("distance: %.3fm\n", (double)_current_distance);
-	PX4_INFO_RAW("dfm mode: %d\n", dfm_mode);
-	PX4_INFO_RAW("rate: %u Hz\n", (uint)(1000000 / _measure_interval));
+	PX4_INFO_RAW("rate: %u Hz\n", (uint)(1000000 / _measurement_inverval));
 }

 namespace afbrs50
@@ -456,7 +447,6 @@ static int start(const uint8_t rotation)
 		return PX4_ERROR;
 	}

-	// Initialize the sensor.
 	if (g_dev->init() != PX4_OK) {
 		PX4_ERR("driver start failed");
 		delete g_dev;
@@ -474,7 +464,7 @@ static int status()
 		return PX4_ERROR;
 	}

-	g_dev->print_info();
+	g_dev->printInfo();

 	return PX4_OK;
 }
@@ -495,30 +485,6 @@ static int stop()
 	return PX4_OK;
 }

-static int test(const uint8_t rotation)
-{
-	if (g_dev != nullptr) {
-		PX4_ERR("already started");
-		return PX4_ERROR;
-	}
-
-	g_dev = new AFBRS50(rotation);
-
-	if (g_dev == nullptr) {
-		PX4_ERR("object instantiate failed");
-		return PX4_ERROR;
-	}
-
-	if (g_dev->test() != PX4_OK) {
-		PX4_ERR("driver test failed");
-		delete g_dev;
-		g_dev = nullptr;
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
-}
-
 static int usage()
 {
 	PRINT_MODULE_DESCRIPTION(
@@ -540,7 +506,6 @@ $ afbrs50 stop
 	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver");
 	PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false);
 	PRINT_MODULE_USAGE_PARAM_INT('r', 25, 0, 25, "Sensor rotation - downward facing by default", true);
-	PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test driver");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver");
 	return PX4_OK;
 }
@@ -581,9 +546,6 @@ extern "C" __EXPORT int afbrs50_main(int argc, char *argv[])
 	} else if (!strcmp(argv[myoptind], "stop")) {
 		return afbrs50::stop();

-	} else if (!strcmp(argv[myoptind], "test")) {
-		return afbrs50::test(rotation);
-
 	}

 	return afbrs50::usage();
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -31,12 +31,6 @@
 *
 ****************************************************************************/

-/**
- * @file AFBRS50.hpp
- *
- * Driver for the Broadcom AFBR-S50 connected via SPI.
- *
- */
 #pragma once

 #include "argus.h"
@@ -51,8 +45,6 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/parameter_update.h>

-using namespace time_literals;
-
 class AFBRS50 : public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
@@ -60,63 +52,53 @@ public:
 	~AFBRS50() override;

 	int init();
-
-	/**
-	 * Diagnostics - print some basic information about the driver.
-	 */
-	void print_info();
-
-	/**50
-	 * Stop the automatic measurement state machine.
-	 */
-	void stop();
-
-	int test();
-
-	bool _testing = false;
+	void printInfo();

 private:
 	void Run() override;

-	void Evaluate_rate();
+	void recordCommsError();
+	void scheduleCollect();
+	void processMeasurement();
+	void updateMeasurementRateFromRange();

-	void ProcessMeasurement(argus_hnd_t *hnd);
+	static status_t measurementReadyCallback(status_t status, argus_hnd_t *hnd);

-	static status_t measurement_ready_callback(status_t status, argus_hnd_t *hnd);
+	status_t setRateAndDfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode);
+	argus_mode_t argusModeFromParameter();

-	void get_info();
-	status_t set_rate_and_dfm(uint32_t rate_hz, argus_dfm_mode_t dfm_mode);
-
-	argus_hnd_t *_hnd{nullptr};
+private:
+	argus_hnd_t *_hnd {nullptr};

 	enum class STATE : uint8_t {
-		TEST,
 		CONFIGURE,
+		TRIGGER,
 		COLLECT,
-		STOP
+		WATCHDOG
 	} _state{STATE::CONFIGURE};

 	PX4Rangefinder _px4_rangefinder;

-	hrt_abstime _measurement_time{0};
 	hrt_abstime _last_rate_switch{0};

-	perf_counter_t _sample_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": sample interval")};
+	perf_counter_t _sample_perf{perf_alloc(PC_COUNT, MODULE_NAME": sample count")};
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": comms error")};
+	perf_counter_t _not_ready_perf{perf_alloc(PC_COUNT, MODULE_NAME": not ready")};

-	uint32_t _measure_interval{1000000 / 50}; // 50Hz
 	float _current_distance{0};
 	int8_t _current_quality{0};
-	float _max_distance;
-	float _min_distance;
+	float _max_distance{30.f};
 	uint32_t _current_rate{0};

 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};

+	uint32_t _measurement_inverval {1000000 / 50}; // 50Hz
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::SENS_AFBR_MODE>)   _p_sens_afbr_mode,
-		(ParamInt<px4::params::SENS_AFBR_S_RATE>)  _p_sens_afbr_s_rate,
-		(ParamInt<px4::params::SENS_AFBR_L_RATE>)  _p_sens_afbr_l_rate,
+		(ParamInt<px4::params::SENS_AFBR_S_RATE>) _p_sens_afbr_s_rate,
+		(ParamInt<px4::params::SENS_AFBR_L_RATE>) _p_sens_afbr_l_rate,
 		(ParamInt<px4::params::SENS_AFBR_THRESH>) _p_sens_afbr_thresh,
-		(ParamInt<px4::params::SENS_AFBR_HYSTER>)    _p_sens_afbr_hyster
+		(ParamInt<px4::params::SENS_AFBR_HYSTER>) _p_sens_afbr_hyster
 	);
 };
@@ -14,6 +14,8 @@

 #include <lib/perf/perf_counter.h>

+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
 /*! A structure to hold all internal data required by the S2PI module. */
 typedef struct {
 	/*! Determines the current driver status. */
@@ -52,11 +54,71 @@ s2pi_handle_t s2pi_ = { .GPIOs = { [ S2PI_CLK ]  = BROADCOM_AFBR_S50_S2PI_CLK,
 				 }
 		      };

-static struct work_s broadcom_s2pi_transfer_work = {};
+static perf_counter_t irq_perf = NULL;

-static perf_counter_t s2pi_transfer_perf = NULL;
-static perf_counter_t s2pi_transfer_callback_perf = NULL;
-static perf_counter_t s2pi_irq_callback_perf = NULL;
+class AFBRS50_SPI :  public px4::ScheduledWorkItem
+{
+public:
+	AFBRS50_SPI();
+	void schedule_now();
+	void schedule_clear();
+
+private:
+
+	void Run() override;
+
+};
+
+AFBRS50_SPI::AFBRS50_SPI():
+	// NOTE: we use SPI0 WQ since it is the 2nd highest priority thread (behind rate_ctrl).
+	// TODO: we should fix how SPI comms work. Async SPI comms is
+	// undesirable. We should use SPI TX DMA complete callback
+	// instead of relying on a high priority thread.
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::SPI0)
+{
+	// Anything to do?
+}
+
+void AFBRS50_SPI::Run()
+{
+	px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 0);
+	SPI_EXCHANGE(s2pi_.spidev, s2pi_.spi_tx_data, s2pi_.spi_rx_data, s2pi_.spi_frame_size);
+	px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 1);
+
+	//// WARNING!
+	// After the last SPI TX we have ~60us to execute the below
+	// callback otherwise the IRQ will fire and we're screwed.
+	// The proper way to solve this problem is to either fix
+	// the API or to configure SPI TX DMA callback complete
+	// to execute the below callback immediately.
+
+
+	// If we are pre-empted here and the IRQ fires before the
+	// callback has been invoked -- we're screwed.
+
+	IRQ_LOCK();
+	s2pi_.Status = STATUS_IDLE;
+
+	if (s2pi_.Callback != 0) {
+		s2pi_callback_t callback = s2pi_.Callback;
+		s2pi_.Callback = 0;
+		callback(STATUS_OK, s2pi_.CallbackData);
+	}
+
+	IRQ_UNLOCK();
+}
+
+void AFBRS50_SPI::schedule_now()
+{
+	ScheduleNow();
+}
+
+void AFBRS50_SPI::schedule_clear()
+{
+	ScheduleClear();
+}
+
+static AFBRS50_SPI *_spi_iface = nullptr;

 /*!***************************************************************************
 * @brief Initialize the S2PI module.
@@ -71,18 +133,6 @@ static perf_counter_t s2pi_irq_callback_perf = NULL;
 *
 * @return Returns the \link #status_t status\endlink (#STATUS_OK on success).
 *****************************************************************************/
-
-static int gpio_falling_edge(int irq, void *context, void *arg)
-{
-	if (s2pi_.IrqCallback != 0) {
-		perf_begin(s2pi_irq_callback_perf);
-		s2pi_.IrqCallback(s2pi_.IrqCallbackData);
-		perf_end(s2pi_irq_callback_perf);
-	}
-
-	return 0;
-}
-
 status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps)
 {
 	(void)defaultSlave;
@@ -91,12 +141,25 @@ status_t S2PI_Init(s2pi_slave_t defaultSlave, uint32_t baudRate_Bps)

 	s2pi_.spidev = px4_spibus_initialize(BROADCOM_AFBR_S50_S2PI_SPI_BUS);

-	px4_arch_configgpio(BROADCOM_AFBR_S50_S2PI_IRQ);
-	px4_arch_gpiosetevent(BROADCOM_AFBR_S50_S2PI_IRQ, false, true, false, &gpio_falling_edge, NULL);
+	// Falling edge callback
+	auto callback = [](int irq, void *context, void *arg) -> int {
+		if (s2pi_.IrqCallback != 0)
+		{
+			perf_begin(irq_perf);
+			s2pi_.IrqCallback(s2pi_.IrqCallbackData);
+			perf_end(irq_perf);
+		}

-	s2pi_transfer_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": transfer");
-	s2pi_transfer_callback_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": transfer callback");
-	s2pi_irq_callback_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": irq callback");
+		return 0;
+	};
+	// NOTE: we enable the interrupt event here but do not configure the GPIO.
+	// We configure the GPIO and enable the interrupt after the device mode
+	// has been configured. This prevents erroneous interrupts from occuring.
+	px4_arch_gpiosetevent(BROADCOM_AFBR_S50_S2PI_IRQ, false, true, false, callback, NULL);
+
+	irq_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": irq callback");
+
+	_spi_iface = new AFBRS50_SPI();

 	return S2PI_SetBaudRate(baudRate_Bps);
 }
@@ -334,25 +397,6 @@ status_t S2PI_CycleCsPin(s2pi_slave_t slave)
 * was not started.
 *****************************************************************************/

-static void broadcom_s2pi_transfer_callout(void *arg)
-{
-	perf_begin(s2pi_transfer_perf);
-	px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 0);
-	SPI_EXCHANGE(s2pi_.spidev, s2pi_.spi_tx_data, s2pi_.spi_rx_data, s2pi_.spi_frame_size);
-	s2pi_.Status = STATUS_IDLE;
-	px4_arch_gpiowrite(s2pi_.GPIOs[S2PI_CS], 1);
-	perf_end(s2pi_transfer_perf);
-
-	/* Invoke callback if there is one */
-	if (s2pi_.Callback != 0) {
-		perf_begin(s2pi_transfer_callback_perf);
-		s2pi_callback_t callback = s2pi_.Callback;
-		s2pi_.Callback = 0;
-		callback(STATUS_OK, s2pi_.CallbackData);
-		perf_end(s2pi_transfer_callback_perf);
-	}
-}
-
 status_t S2PI_TransferFrame(s2pi_slave_t spi_slave, uint8_t const *txData, uint8_t *rxData, size_t frameSize,
 			    s2pi_callback_t callback, void *callbackData)
 {
@@ -384,7 +428,8 @@ status_t S2PI_TransferFrame(s2pi_slave_t spi_slave, uint8_t const *txData, uint8
 	s2pi_.spi_tx_data = (uint8_t *)txData;
 	s2pi_.spi_rx_data = rxData;
 	s2pi_.spi_frame_size = frameSize;
-	work_queue(HPWORK, &broadcom_s2pi_transfer_work, broadcom_s2pi_transfer_callout, NULL, 0);
+
+	_spi_iface->schedule_now();

 	IRQ_UNLOCK();

@@ -410,7 +455,7 @@ status_t S2PI_Abort(s2pi_slave_t slave)

 	/* Abort SPI transfer. */
 	if (status == STATUS_BUSY) {
-		work_cancel(HPWORK, &broadcom_s2pi_transfer_work);
+		_spi_iface->schedule_clear();
 	}

 	return STATUS_OK;
@@ -47,7 +47,7 @@ px4_add_module(
 		AFBRS50.cpp
 		AFBRS50.hpp
 		API/Src/irq.c
-		API/Src/s2pi.c
+		API/Src/s2pi.cpp
 		API/Src/timer.c
 		argus_hal_test.c
 	DEPENDS
@@ -150,7 +150,7 @@ The sensor/driver must be enabled using the parameter SENS_EN_LL40LS.
 Setup/usage information: https://docs.px4.io/main/en/sensor/lidar_lite.html
 )DESCR_STR");

-	PRINT_MODULE_USAGE_NAME("ll40ls", "driver");
+	PRINT_MODULE_USAGE_NAME("ll40ls_pwm", "driver");
 	PRINT_MODULE_USAGE_SUBCATEGORY("distance_sensor");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("start","Start driver");
 	PRINT_MODULE_USAGE_PARAM_INT('R', 25, 0, 25, "Sensor rotation - downward facing by default", true);
@@ -118,6 +118,7 @@ int Decoder::parse(Header *header) const
 int Decoder::parse(DOP *message) const
 {
 	if (can_parse() && id() == BlockID::DOP) {
+		static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small");
 		memcpy(message, _message.payload, sizeof(DOP));
 		return PX4_OK;
 	}
@@ -128,6 +129,7 @@ int Decoder::parse(DOP *message) const
 int Decoder::parse(PVTGeodetic *message) const
 {
 	if (can_parse() && id() == BlockID::PVTGeodetic) {
+		static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small");
 		memcpy(message, _message.payload, sizeof(PVTGeodetic));
 		return PX4_OK;
 	}
@@ -138,6 +140,7 @@ int Decoder::parse(PVTGeodetic *message) const
 int Decoder::parse(ReceiverStatus *message) const
 {
 	if (can_parse() && id() == BlockID::ReceiverStatus) {
+		static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small");
 		memcpy(message, _message.payload, sizeof(ReceiverStatus));
 		return PX4_OK;
 	}
@@ -148,6 +151,7 @@ int Decoder::parse(ReceiverStatus *message) const
 int Decoder::parse(QualityInd *message) const
 {
 	if (can_parse() && id() == BlockID::QualityInd) {
+		static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small");
 		// Safe to copy entire size of the message as it is smaller than the maximum expected SBF message size.
 		// It's up to the user of the parsed message to ignore the invalid fields.
 		memcpy(message, _message.payload, sizeof(QualityInd));
@@ -160,11 +164,16 @@ int Decoder::parse(QualityInd *message) const
 int Decoder::parse(RFStatus *message) const
 {
 	if (can_parse() && id() == BlockID::PVTGeodetic) {
+		static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small");
 		memcpy(message, _message.payload, sizeof(RFStatus) - sizeof(RFStatus::rf_band));

 		for (uint8_t i = 0; i < math::min(message->n, k_max_rfband_blocks); i++) {
-			memcpy(&message->rf_band[i], &_message.payload[sizeof(RFStatus) - sizeof(RFStatus::rf_band) + i *
-					message->sb_length], sizeof(RFBand));
+			const unsigned offset = sizeof(RFStatus) - sizeof(RFStatus::rf_band) + i *
+						message->sb_length;
+
+			if (offset + sizeof(RFBand) <= sizeof(_message.payload)) {
+				memcpy(&message->rf_band[i], &_message.payload[offset], sizeof(RFBand));
+			}
 		}

 		return PX4_OK;
@@ -176,6 +185,7 @@ int Decoder::parse(RFStatus *message) const
 int Decoder::parse(GALAuthStatus *message) const
 {
 	if (can_parse() && id() == BlockID::GALAuthStatus) {
+		static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small");
 		memcpy(message, _message.payload, sizeof(GALAuthStatus));
 		return PX4_OK;
 	}
@@ -186,6 +196,7 @@ int Decoder::parse(GALAuthStatus *message) const
 int Decoder::parse(VelCovGeodetic *message) const
 {
 	if (can_parse() && id() == BlockID::VelCovGeodetic) {
+		static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small");
 		memcpy(message, _message.payload, sizeof(VelCovGeodetic));
 		return PX4_OK;
 	}
@@ -196,11 +207,17 @@ int Decoder::parse(VelCovGeodetic *message) const
 int Decoder::parse(GEOIonoDelay *message) const
 {
 	if (can_parse() && id() == BlockID::GEOIonoDelay) {
+		static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small");
 		memcpy(message, _message.payload, sizeof(GEOIonoDelay) - sizeof(GEOIonoDelay::idc));

-		for (size_t i = 0; i < math::min(message->n, (uint8_t)4); i++) {
-			memcpy(&message->idc[i], &_message.payload[sizeof(GEOIonoDelay) - sizeof(GEOIonoDelay::idc) + i *
-					message->sb_length], sizeof(IDC));
+		for (size_t i = 0; i < math::min(message->n, (uint8_t)(sizeof(GEOIonoDelay::idc) / sizeof(GEOIonoDelay::idc[0])));
+		     i++) {
+			const unsigned offset = sizeof(GEOIonoDelay) - sizeof(GEOIonoDelay::idc) + i *
+						message->sb_length;
+
+			if (offset + sizeof(IDC) <= sizeof(_message.payload)) {
+				memcpy(&message->idc[i], &_message.payload[offset], sizeof(IDC));
+			}
 		}

 		return PX4_OK;
@@ -212,6 +229,7 @@ int Decoder::parse(GEOIonoDelay *message) const
 int Decoder::parse(AttEuler *message) const
 {
 	if (can_parse() && id() == BlockID::AttEuler) {
+		static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small");
 		memcpy(message, _message.payload, sizeof(AttEuler));
 		return PX4_OK;
 	}
@@ -222,6 +240,7 @@ int Decoder::parse(AttEuler *message) const
 int Decoder::parse(AttCovEuler *message) const
 {
 	if (can_parse() && id() == BlockID::AttCovEuler) {
+		static_assert(sizeof(*message) <= sizeof(_message.payload), "Buffer too small");
 		memcpy(message, _message.payload, sizeof(AttCovEuler));
 		return PX4_OK;
 	}
@@ -243,7 +262,7 @@ bool Decoder::done() const

 bool Decoder::can_parse() const
 {
-	return done()
+	return done() && _message.header.length <= sizeof(_message) && _message.header.length > 4
 	       && _message.header.crc == buffer_crc16(reinterpret_cast<const uint8_t *>(&_message) + 4, _message.header.length - 4);
 }

@@ -224,7 +224,7 @@ private:
 	bool can_parse() const;

 	State _state{State::SearchingSync1};
-	uint16_t _current_index;
+	uint16_t _current_index{0};
 	message_t _message;
 };

@@ -614,7 +614,7 @@ void SagetechMXS::handle_svr(sg_svr_t svr)
 		}

 		if (svr.validity.surfHeading) {
-			t.heading = matrix::wrap_pi((float)svr.surface.heading * (M_PI_F / 180.0f) + M_PI_F);
+			t.heading = matrix::wrap_pi((float)svr.surface.heading * (M_PI_F / 180.0f));
 			t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING;
 		}
 	}
@@ -622,7 +622,7 @@ void SagetechMXS::handle_svr(sg_svr_t svr)
 	if (svr.type == svrAirborne) {
 		if (svr.validity.airSpeed) {
 			t.hor_velocity = (svr.airborne.speed * SAGETECH_SCALE_KNOTS_TO_M_PER_SEC);	//Convert from knots to meters/second
-			t.heading = matrix::wrap_pi((float)svr.airborne.heading * (M_PI_F / 180.0f) + M_PI_F);
+			t.heading = matrix::wrap_pi((float)svr.airborne.heading * (M_PI_F / 180.0f));
 			t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING;
 			t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
 		}
@@ -180,7 +180,7 @@ bool AdsbConflict::handle_traffic_conflict()

 	case TRAFFIC_STATE::ADD_CONFLICT:
 	case TRAFFIC_STATE::REMIND_CONFLICT: {
-			take_action = send_traffic_warning((int)(math::degrees(_transponder_report.heading) + 180.f),
+			take_action = send_traffic_warning((int)math::degrees(_transponder_report.heading),
 							   (int)fabsf(_crosstrack_error.distance), _transponder_report.flags,
 							   _transponder_report.callsign,
 							   _transponder_report.icao_address,
@@ -128,7 +128,6 @@ PARAM_DEFINE_FLOAT(WEIGHT_GROSS, -1.0f);
 *
 * @unit m/s
 * @min 1.0
- * @max 15.0
 * @decimal 1
 * @increment 0.5
 * @group FW Performance
@@ -144,7 +143,6 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
 *
 * @unit m/s
 * @min 1.0
- * @max 5.0
 * @decimal 1
 * @increment 0.5
 * @group FW Performance
@@ -107,6 +107,8 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
 	wind_est.tas_scale_raw = _wind_estimator.get_tas_scale();
 	wind_est.tas_scale_raw_var = _wind_estimator.get_tas_scale_var();
 	wind_est.tas_scale_validated = _CAS_scale_validated;
+	wind_est.tas_innov_integ_test_ratio = _tas_innov_integ_threshold > FLT_EPSILON ? _aspd_innov_integ_state /
+					      _tas_innov_integ_threshold : 0.f;
 	return wind_est;
 }

@@ -688,7 +688,7 @@ Commander::Commander() :
 	_vehicle_status.system_id = 1;
 	_vehicle_status.component_id = 1;
 	_vehicle_status.system_type = 0;
-	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
+	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_UNSPECIFIED;
 	_vehicle_status.nav_state = _user_mode_intention.get();
 	_vehicle_status.nav_state_user_intention = _user_mode_intention.get();
 	_vehicle_status.nav_state_timestamp = hrt_absolute_time();
@@ -127,6 +127,7 @@ PARAM_DEFINE_INT32(COM_HLDL_REG_T, 0);
 *
 * The time in seconds without a new setpoint from RC or Joystick, after which the connection is considered lost.
 * This must be kept short as the vehicle will use the last supplied setpoint until the timeout triggers.
+ * Ensure the value is not set lower than the update interval of the RC or Joystick.
 *
 * @group Commander
 * @unit s
@@ -144,7 +144,7 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -30.0f);
 *
 * @unit deg
 * @min 0.0
- * @max 60.0
+ * @max 80.0
 * @decimal 1
 * @increment 0.5
 * @group FW General
@@ -158,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 30.0f);
 *
 * @unit deg
 * @min 35.0
- * @max 65.0
+ * @max 75.0
 * @decimal 1
 * @increment 0.5
 * @group FW General
@@ -203,7 +203,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
 *
 * @unit norm
 * @min 0.0
- * @max 0.4
+ * @max 1.0
 * @decimal 2
 * @increment 0.01
 * @group FW General
@@ -218,7 +218,7 @@ PARAM_DEFINE_FLOAT(FW_THR_IDLE, 0.0f);
 *
 * @unit deg
 * @min 1.0
- * @max 15.0
+ * @max 45.0
 * @decimal 1
 * @increment 0.5
 * @group FW Auto Landing
@@ -230,7 +230,7 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
 *
 * @unit deg
 * @min -5.0
- * @max 30.0
+ * @max 80.0
 * @decimal 1
 * @increment 0.5
 * @group FW Auto Takeoff
@@ -390,8 +390,7 @@ PARAM_DEFINE_INT32(FW_POS_STK_CONF, 2);
 * In manual modes: maximum climb rate setpoint.
 *
 * @unit m/s
- * @min 0.5
- * @max 15
+ * @min 0.1
 * @decimal 2
 * @increment 0.01
 * @group FW General
@@ -405,8 +404,7 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP, 3.0f);
 * In manual modes: maximum sink rate setpoint.
 *
 * @unit m/s
- * @min 0.5
- * @max 15
+ * @min 0.1
 * @decimal 2
 * @increment 0.01
 * @group FW General
@@ -422,7 +420,6 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_R_SP, 2.0f);
 *
 * @unit s
 * @min 0
- * @max 3600
 * @group FW General
 */
 PARAM_DEFINE_INT32(FW_GPSF_LT, 30);
@@ -434,7 +431,7 @@ PARAM_DEFINE_INT32(FW_GPSF_LT, 30);
 *
 * @unit deg
 * @min 0.0
- * @max 30.0
+ * @max 60.0
 * @decimal 1
 * @increment 0.5
 * @group FW General
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2016-2025 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__hardfault_stream
+	MAIN hardfault_stream
+	COMPILE_FLAGS
+	SRCS
+		HardfaultStream.cpp
+		HardfaultStream.hpp
+	DEPENDS
+		px4_work_queue
+)
@@ -0,0 +1,224 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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
+ * 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 "HardfaultStream.hpp"
+
+using namespace time_literals;
+
+namespace hardfault_stream
+{
+
+HardfaultStream::HardfaultStream() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+{
+}
+
+HardfaultStream::~HardfaultStream()
+{
+	ScheduleClear();
+
+	if (_hardfault_file != nullptr) {
+		fclose(_hardfault_file);
+	}
+}
+
+int HardfaultStream::task_spawn(int argc, char *argv[])
+{
+	HardfaultStream *obj = new HardfaultStream();
+
+	if (!obj) {
+		PX4_ERR("alloc failed");
+		return -1;
+	}
+
+	_object.store(obj);
+	_task_id = task_id_is_work_queue;
+
+	/* Schedule a cycle to start things. */
+	obj->start();
+
+	return 0;
+}
+
+void HardfaultStream::start()
+{
+	ScheduleOnInterval(150_ms);
+}
+
+bool HardfaultStream::mavlink_gcs_up()
+{
+	for (auto &telemetry_status : _telemetry_status_subs) {
+		telemetry_status_s telemetry;
+
+		if (telemetry_status.update(&telemetry)) {
+			if (telemetry.heartbeat_type_gcs) {
+				return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+void HardfaultStream::search_hardfault_file()
+{
+	DIR *dp = opendir(PX4_STORAGEDIR);
+
+	if (dp != nullptr) {
+
+		struct dirent *result;
+		struct stat st;
+		time_t latest_mtime = 0;
+
+		while ((result = readdir(dp))) {
+			// Check for pattern fault_*.log
+			if (strncmp("fault_", result->d_name, 6) == 0 && strcmp(result->d_name + strlen(result->d_name) - 4, ".log") == 0) {
+				char current_file_path[CONFIG_PATH_MAX + 1];
+				snprintf(current_file_path, sizeof(current_file_path), "%s/%s", PX4_STORAGEDIR, result->d_name);
+
+				if (stat(current_file_path, &st) == 0) {
+					if (st.st_mtime >= latest_mtime) {
+						latest_mtime = st.st_mtime;
+						strncpy(_hardfault_file_path, current_file_path, sizeof(_hardfault_file_path));
+						_hardfault_file_path[sizeof(_hardfault_file_path) - 1] = '\0';
+						_hardfault_file_present = true;
+					}
+				}
+			}
+		}
+	}
+
+	closedir(dp);
+}
+
+void HardfaultStream::stream_hardfault()
+{
+	if (_hardfault_file == nullptr) {
+		_hardfault_file = fopen(_hardfault_file_path, "rb");
+
+		if (_hardfault_file == nullptr) {
+			PX4_ERR("Can't open hardfault log: %s", _hardfault_file_path);
+			_state = State::RequestStop;
+			return;
+		}
+
+		PX4_INFO("Streaming hardfault log: %s", _hardfault_file_path);
+	}
+
+	static constexpr int chunk_size = sizeof(mavlink_log_s::text) - 1;
+	uint8_t chunk[chunk_size];
+	size_t bytes_read = fread(chunk, 1, chunk_size, _hardfault_file);
+
+	if (bytes_read > 0) {
+		mavlink_vasprintf(_MSG_PRIO_CRITICAL, &_mavlink_log_pub, "%.*s", bytes_read, chunk);
+
+	} else {
+		fclose(_hardfault_file);
+		_hardfault_file = nullptr;
+		_stream_finished = true;
+	}
+}
+
+void HardfaultStream::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+	}
+
+	switch (_state) {
+	case State::SearchFile:
+		search_hardfault_file();
+		_state = State::WaitMavlink;
+		ScheduleNow();
+		break;
+
+	case State::WaitMavlink:
+		if (mavlink_gcs_up()) {
+			_state = State::StreamFile;
+			ScheduleNow();
+		}
+
+		break;
+
+	case State::StreamFile:
+		if (!_hardfault_file_present || _stream_finished) {
+			_state = State::RequestStop;
+
+		} else {
+			stream_hardfault();
+		}
+
+		break;
+
+	case State::RequestStop:
+		request_stop();
+		_state = State::WaitStop;
+
+	case State::WaitStop:
+		break;
+	}
+}
+
+int HardfaultStream::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_ERR("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Background process that streams the latest hardfault via MAVLink.
+
+The module is especially useful when it is necessary to quickly push a hard fault to the ground station.
+This is useful in cases where the drone experiences a hard fault during flight.
+It ensures that some data is retained in case the permanent storage is destroyed during a crash.
+
+To reliably stream, it is necessary to send the STATUSTEXT message via MAVLink at a
+high enough frequency. The recommended frequency is 10 Hz or higher.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("hardfault_stream", "system");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the background task");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	return 0;
+}
+
+extern "C" __EXPORT int hardfault_stream_main(int argc, char *argv[])
+{
+	return HardfaultStream::main(argc, argv);
+}
+
+} // namespace hardfault_stream
@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <dirent.h>
+#include <string.h>
+#include <sys/stat.h>
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/mavlink_log.h>
+#include <uORB/topics/telemetry_status.h>
+
+#include <systemlib/mavlink_log.h>
+
+namespace hardfault_stream
+{
+
+class HardfaultStream : public ModuleBase<HardfaultStream>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	HardfaultStream();
+	~HardfaultStream() override;
+
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[])
+	{
+		return print_usage("unknown command");
+	}
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	void start();
+
+private:
+	enum class State {
+		SearchFile,
+		WaitMavlink,
+		StreamFile,
+		RequestStop,
+		WaitStop,
+	};
+
+	/** Do a compute and schedule the next cycle. */
+	void Run() override;
+
+	bool mavlink_gcs_up();
+	void search_hardfault_file();
+	void stream_hardfault();
+
+	State _state {State::SearchFile};
+
+	bool _stream_finished {false};
+	bool _hardfault_file_present {false};
+
+	char _hardfault_file_path[CONFIG_PATH_MAX + 1];
+	FILE *_hardfault_file {nullptr};
+
+	orb_advert_t _mavlink_log_pub {nullptr};
+	uORB::SubscriptionMultiArray<telemetry_status_s> _telemetry_status_subs{ORB_ID::telemetry_status};
+};
+
+} // namespace hardfault_stream
@@ -0,0 +1,6 @@
+menuconfig MODULES_HARDFAULT_STREAM
+	bool "hardfault_stream"
+	default n
+	depends on PLATFORM_NUTTX
+	---help---
+		Enable support for hardfault_stream
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2025 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Enable FMU SD card hardfault streaming
+ *
+ * When this is enabled all the hardfaults on the SD card are streamed
+ * over MAVLink. This is useful for cases where the FMU does reset in-flight due
+ * to a hardfault and the SD card may not survive a crash.
+ *
+ * @group System
+ * @boolean
+ */
+PARAM_DEFINE_INT32(SYS_HF_MAV, 1);
@@ -278,9 +278,14 @@ MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t t
 void
 MavlinkMissionManager::send_mission_current(uint16_t seq)
 {
+	// Update mission state before sending
+	update_mission_state();
+
 	mavlink_mission_current_t wpc{};
 	wpc.seq = seq;
 	wpc.total = _count[MAV_MISSION_TYPE_MISSION] > 0 ? _count[MAV_MISSION_TYPE_MISSION] : UINT16_MAX;
+	wpc.mission_state = static_cast<uint8_t>(_mission_state);
+	wpc.mission_mode = static_cast<uint8_t>(_mission_mode);
 	wpc.mission_id = _crc32[MAV_MISSION_TYPE_MISSION];
 	wpc.fence_id = _crc32[MAV_MISSION_TYPE_FENCE];
 	wpc.rally_points_id = _crc32[MAV_MISSION_TYPE_RALLY];
@@ -911,6 +916,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 			_transfer_partner_compid = msg->compid;
 			_mission_type = (MAV_MISSION_TYPE)wpc.mission_type;
 			_transfer_current_crc32 = 0;
+			_last_reached = -1; // Reset last reached waypoint when new mission starts

 			if (wpc.count > current_max_item_count()) {
 				PX4_DEBUG("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, current_max_item_count());
@@ -1907,3 +1913,51 @@ uint32_t MavlinkMissionManager::crc32_for_mission_item(const mavlink_mission_ite

 	return crc32part(u.raw, sizeof(u), prev_crc32);
 }
+
+void
+MavlinkMissionManager::update_mission_state()
+{
+	// Get vehicle status
+	vehicle_status_s vehicle_status;
+
+	if (!_vehicle_status_sub.update(&vehicle_status)) {
+		return;
+	}
+
+	// Get mission result
+	mission_result_s mission_result;
+
+	if (!_mission_result_sub.update(&mission_result)) {
+		return;
+	}
+
+	// Update mission mode
+	if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
+	    vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
+	    vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND) {
+		_mission_mode = MISSION_MODE_ACTIVE;
+
+	} else {
+		_mission_mode = MISSION_MODE_SUSPENDED;
+	}
+
+	// Update mission state
+	if (_count[MAV_MISSION_TYPE_MISSION] == 0) {
+		_mission_state = MISSION_STATE_NO_MISSION;
+
+	} else if (mission_result.finished) {
+		// Mission is complete if the navigator says it's finished
+		_mission_state = MISSION_STATE_COMPLETE;
+
+	} else if (_mission_mode == MISSION_MODE_ACTIVE
+		   && vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+		_mission_state = MISSION_STATE_ACTIVE;
+
+	} else if (_mission_mode == MISSION_MODE_SUSPENDED && _last_reached >= 0) {
+		// Only PAUSED if we were actually in the middle of a mission
+		_mission_state = MISSION_STATE_PAUSED;
+
+	} else {
+		_mission_state = MISSION_STATE_NOT_STARTED;
+	}
+}
@@ -49,6 +49,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/vehicle_status.h>

 #include "mavlink_bridge_header.h"
 #include "mavlink_rate_limiter.h"
@@ -60,6 +61,13 @@ enum MAVLINK_WPM_STATES {
 	MAVLINK_WPM_STATE_ENUM_END
 };

+// Mission mode states
+enum MISSION_MODE {
+	MISSION_MODE_UNKNOWN = 0,
+	MISSION_MODE_ACTIVE = 1,
+	MISSION_MODE_SUSPENDED = 2
+};
+
 enum MAVLINK_WPM_CODES {
 	MAVLINK_WPM_CODE_OK = 0,
 	MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
@@ -94,9 +102,13 @@ public:
 private:
 	enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE};	///< Current state
 	enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION};	///< mission type of current transmission (only one at a time possible)
+	enum MISSION_STATE _mission_state {MISSION_STATE_UNKNOWN}; ///< Current mission state machine state
+	enum MISSION_MODE _mission_mode {MISSION_MODE_UNKNOWN}; ///< Current mission mode

 	DatamanClient _dataman_client{};

+	void update_mission_state();
+
 	uint64_t		_time_last_recv{0};
 	uint64_t		_time_last_sent{0};

@@ -138,6 +150,7 @@ private:

 	uORB::Subscription	_mission_result_sub{ORB_ID(mission_result)};
 	uORB::SubscriptionData<mission_s> 	_mission_sub{ORB_ID(mission)};
+	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};	///< vehicle status subscription

 	uORB::Publication<mission_s>	_offboard_mission_pub{ORB_ID(mission)};

@@ -2548,7 +2548,7 @@ MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
 	t.lon = adsb.lon * 1e-7;
 	t.altitude_type = adsb.altitude_type;
 	t.altitude = adsb.altitude / 1000.0f;
-	t.heading = adsb.heading / 100.0f / 180.0f * M_PI_F - M_PI_F;
+	t.heading = adsb.heading / 100.0f / 180.0f * M_PI_F;
 	t.hor_velocity = adsb.hor_velocity / 100.0f;
 	t.ver_velocity = adsb.ver_velocity / 100.0f;
 	memcpy(&t.callsign[0], &adsb.callsign[0], sizeof(t.callsign));
@@ -77,7 +77,7 @@ private:
 			msg.lon = pos.lon * 1e7;
 			msg.altitude_type = pos.altitude_type;
 			msg.altitude = pos.altitude * 1e3f;
-			msg.heading = (pos.heading + M_PI_F) / M_PI_F * 180.0f * 100.0f;
+			msg.heading = pos.heading / M_PI_F * 180.0f * 100.0f;
 			msg.hor_velocity = pos.hor_velocity * 100.0f;
 			msg.ver_velocity = pos.ver_velocity * 100.0f;
 			memcpy(&msg.callsign[0], &pos.callsign[0], sizeof(msg.callsign));
@@ -168,33 +168,44 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);

 /**
- * Board rotation Y (Pitch) offset
+ * Board rotation Y (pitch) offset
 *
- * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user
- * to fine tune the board offset in the event of misalignment.
+ * Rotation from flight controller board to vehicle body frame.
+ * This parameter gets set during the "level horizon" calibration or can be
+ * set manually.
 *
+ * @min -45.0
+ * @max 45.0
+ * @decimal 1
 * @unit deg
 * @group Sensors
 */
 PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f);

 /**
- * Board rotation X (Roll) offset
+ * Board rotation X (roll) offset
 *
- * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user
- * to fine tune the board offset in the event of misalignment.
+ * Rotation from flight controller board to vehicle body frame.
+ * This parameter gets set during the "level horizon" calibration or can be
+ * set manually.
 *
 * @unit deg
+ * @min -45.0
+ * @max 45.0
+ * @decimal 1
 * @group Sensors
 */
 PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f);

 /**
- * Board rotation Z (YAW) offset
+ * Board rotation Z (yaw) offset
 *
- * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user
- * to fine tune the board offset in the event of misalignment.
+ * Rotation from flight controller board to vehicle body frame.
+ * Has to be set manually (not set by any calibration).
 *
+ * @min -45.0
+ * @max 45.0
+ * @decimal 1
 * @unit deg
 * @group Sensors
 */
@@ -169,7 +169,7 @@ PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 400);
 * @reboot_required false
 * @group Sensors
 */
-PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f);
+PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 20.0f);

 /**
 * IMU gyro dynamic notch filtering
Author	SHA1	Message	Date
RomanBapst	d468ec4e1d	AirspeedWing: log the airspeed innovation test ratio - if the value exceeds 1, then airspeed checks will trigger - useful to monitoring the check and tuning Signed-off-by: RomanBapst <bapstroman@gmail.com>	2025-06-17 09:22:32 +03:00
Qiaosen Liu	59710b15ae	params: COM_RC_LOSS_T: clarify safe adjustment of RC loss timeout (#25062 ) * params: COM_RC_LOSS_T: clarify safe adjustment of RC loss timeout * Update src/modules/commander/commander_params.c --------- Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>	2025-06-16 18:50:43 -08:00
Julian Oes	0180ad3a63	mavlink: implement extended MISSION_CURRENT The message MISSION_CURRENT got extensions regarding mission state in https://github.com/mavlink/mavlink/pull/1869 This is an attempt to actually populate those fields.	2025-06-17 13:37:47 +12:00
Silvan Fuhrer	e0663cd6ad	Sensor params: improve board rotation parameter description and meta data (#25037 ) * Sensor params: add @decimals and @min, @max to SENS_BOARD_ params Signed-off-by: Silvan <silvan@auterion.com> * Sensor params: Improve parameter description Signed-off-by: Silvan <silvan@auterion.com> --------- Signed-off-by: Silvan <silvan@auterion.com> Co-authored-by: Ramon Roche <mrpollo@gmail.com>	2025-06-16 14:59:03 -08:00
Silvan Fuhrer	a6863f0930	FW mode manager: open up / remove some arbitrary params constraints (#25036 ) * performance model: remove arbitrary min/max parameter constraints Signed-off-by: Silvan <silvan@auterion.com> * FW mode manager: open up/remove unnecessary param @max constraints Signed-off-by: Silvan <silvan@auterion.com> --------- Signed-off-by: Silvan <silvan@auterion.com> Co-authored-by: Ramon Roche <mrpollo@gmail.com>	2025-06-16 14:58:38 -08:00
Ramon Roche	34d4eb7b9e	ci: update build container (#25059 ) adds lark python dependency #25056 https://github.com/PX4/PX4-Autopilot/pull/25056 Signed-off-by: Ramon Roche <mrpollo@gmail.com>	2025-06-16 14:57:17 -08:00
Ramon Roche	efc3e64c00	ci: update rosdistro apt keys (#25060 ) Signed-off-by: Ramon Roche <mrpollo@gmail.com>	2025-06-16 13:46:33 -08:00
Peter van der Perk	0369abd556	Add lark to python requirements for the rosidl parser	2025-06-16 12:41:48 -07:00
QiTao Weng	75e4047f2a	Fix ADSB heading angle obtained from driver by removing +pi. Adjusted downstream accordingly	2025-06-13 15:28:25 +02:00
Beat Küng	310cbbedb1	fix septentrio: check for buffer underflow _message.header.length - 4 is passed as unsigned to the CRC method, so if _message.header.length < 4, the length wraps and causes invalid memory access.	2025-06-13 14:39:57 +02:00
Alexander Lerach	6f81998e27	hardfault: add option to stream via mavlink	2025-06-13 13:32:00 +02:00
Jacob Dahl	edda54b26b	build: romf: fix generation of rc.board_bootloader_upgrade (#25032 )	2025-06-13 00:45:14 -08:00
Beat Küng	e29771cd97	septentrio: add static assertions to ensure the buffer is large enough And check the access for dynamic sized input. This is only a precaution if the device sends invalid data (too large sb_length) with valid crc.	2025-06-13 09:46:10 +02:00
Beat Küng	ac74a02d7c	fix septentrio: ensure the crc check does not read past the buffer size in case the input data is invalid.	2025-06-13 09:46:10 +02:00
Beat Küng	d13692ca46	fix septentrio: initialize _current_index Decoder::reset sets it to 0, but it could have been accessed before the first call to reset().	2025-06-13 09:46:10 +02:00
Hamish Willee	77d854b045	Update ll40ls.cpp (#25007 )	2025-06-12 16:08:12 -08:00
Jacob Dahl	fae563b35e	[bug] AFBRS50 fix scheduling and refactor (#24837 ) * fix scheduling bug for afbrs50 * cleanup and refactor * format * clean up --------- Co-authored-by: Alex Klimaj <alex@arkelectron.com>	2025-06-12 16:59:02 -06:00
Julian Oes	4c130769bd	boards: set RC serial for KakuteH7-Wing This was missed earlier.	2025-06-13 10:19:54 +12:00
Julian Oes	599ea7545d	boards: fixup image size of KakuteH7-Wing We have 1 sector for the bootloader and the last 2 for parameters. That's to match how ArduPilot has it.	2025-06-13 10:19:54 +12:00
Julian Oes	9fd126194b	vscode: add KakuteH7-Wing	2025-06-13 10:19:54 +12:00
Pedro Roque	6d15019717	feat: spacecraft tooling for commander and VehicleStatus (#24716 ) * feat: spacecraft tooling for commander and VehicleStatus * fix: format * fix: remove iostream * feat: mavlink compliant spacecraft definition * feat: add orbiter to define feat: spacecraft tooling for commander and VehicleStatus fix: format fix: remove iostream feat: mavlink compliant spacecraft definition * feat: add orbiter to define * feat: update mavlink to latest * fix: get away without specifying spacecraft vehicle * fix: removed unnecessary definition * fix: format	2025-06-12 21:48:48 +02:00
Matthias Grob	6b2b20bd6e	imu_gyro_parameters: lower default angular acceleration filter from 30Hz to 20Hz to avoid noise getting into the derivative term of the PID rate control loop on integration flights.	2025-06-12 17:15:12 +02:00