fix rangefinder params

reset terrain to rng before reset aid source
fixy
2026-06-08 00:00:05 +08:00 · 2025-06-24 11:11:02 -08:00 · 2025-06-24 11:11:02 -08:00 · 2025-06-24 11:11:02 -08:00 · 2025-06-24 11:11:02 -08:00 · 2025-06-24 11:11:02 -08:00
203 changed files with 3423 additions and 2125 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()

@@ -15,7 +15,6 @@ param set-default NAV_ACC_RAD 0.5

 # Differential Parameters
 param set-default RD_WHEEL_TRACK 0.3
-param set-default RD_MAX_THR_YAW_R 1.5
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533

@@ -31,14 +30,16 @@ param set-default RO_YAW_RATE_P 0.25
 param set-default RO_YAW_RATE_LIM 180
 param set-default RO_YAW_ACCEL_LIM 120
 param set-default RO_YAW_DECEL_LIM 1000
+param set-default RO_YAW_RATE_CORR 1.43

 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 5

-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 2
 param set-default RO_SPEED_I 0.01
 param set-default RO_SPEED_P 0.1
+param set-defatul RO_SPEED_RED 1

 # Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1
@@ -1,51 +1,68 @@
 #!/bin/sh
-# @name Gazebo lawnmower
+# @name Gazebo - Zero Turn Lawnmower
 # @type Rover
 # @class Rover

 . ${R}etc/init.d/rc.rover_differential_defaults

 PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
-PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=lawn}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower}

 param set-default SIM_GZ_EN 1 # Gazebo bridge

+param set-default NAV_ACC_RAD 0.5
+
 # We can arm and drive in manual mode when it slides and GPS check fails:
 param set-default COM_ARM_WO_GPS 1

-# Rover parameters
+# Differential Parameters
 param set-default RD_WHEEL_TRACK 0.9
-param set-default RD_YAW_RATE_I 0.1
-param set-default RD_YAW_RATE_P 5
-param set-default RD_MAX_ACCEL 1
-param set-default RD_MAX_JERK 3
-param set-default RD_MAX_SPEED 8
-param set-default RD_YAW_P 5
-param set-default RD_YAW_I 0.1
-param set-default RD_MAX_YAW_RATE 30
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533

+# Rover Control Parameters
+param set-default RO_ACCEL_LIM 1.5
+param set-default RO_DECEL_LIM 5
+param set-default RO_JERK_LIM 15
+param set-default RO_MAX_THR_SPEED 2.7
+
+# Rover Rate Control Parameters
+param set-default RO_YAW_RATE_I 0.2
+param set-default RO_YAW_RATE_P 1.0
+param set-default RO_YAW_RATE_LIM 60
+param set-default RO_YAW_ACCEL_LIM 50
+param set-default RO_YAW_DECEL_LIM 1000
+param set-default RO_YAW_RATE_CORR 1.0
+
+# Rover Attitude Control Parameters
+param set-default RO_YAW_P 5
+
+# Rover Velocity Control Parameters
+param set-default RO_SPEED_LIM 2.1
+param set-default RO_SPEED_I 0.01
+param set-default RO_SPEED_P 0.1
+param set-defatul RO_SPEED_RED 1.0
+
 # Pure pursuit parameters
-param set-default PP_LOOKAHD_MAX 30
-param set-default PP_LOOKAHD_MIN 2
 param set-default PP_LOOKAHD_GAIN 1
+param set-default PP_LOOKAHD_MAX 10
+param set-default PP_LOOKAHD_MIN 1

 # Actuator mapping - set SITL motors/servos output parameters:

 # "Motors" - motor channels 0 (Right) and 1 (Left) - via Wheels GZ bridge:
 param set-default SIM_GZ_WH_FUNC1 101 # right wheel
-#param set-default SIM_GZ_WH_MIN1 0
-#param set-default SIM_GZ_WH_MAX1 200
-#param set-default SIM_GZ_WH_DIS1 100
-#param set-default SIM_GZ_WH_FAIL1 100
+param set-default SIM_GZ_WH_MIN1 87
+param set-default SIM_GZ_WH_MAX1 113
+param set-default SIM_GZ_WH_DIS1 100
+param set-default SIM_GZ_WH_FAIL1 100

 param set-default SIM_GZ_WH_FUNC2 102 # left wheel
-#param set-default SIM_GZ_WH_MIN2 0
-#param set-default SIM_GZ_WH_MAX2 200
-#param set-default SIM_GZ_WH_DIS2 100
-#param set-default SIM_GZ_WH_FAIL2 100
+param set-default SIM_GZ_WH_MIN2 87
+param set-default SIM_GZ_WH_MAX2 113
+param set-default SIM_GZ_WH_DIS2 100
+param set-default SIM_GZ_WH_FAIL2 100

 param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels

@@ -30,14 +30,16 @@ param set-default RO_MAX_THR_SPEED 3.1
 param set-default RO_YAW_RATE_I 0.1
 param set-default RO_YAW_RATE_P 1
 param set-default RO_YAW_RATE_LIM 180
+param set-default RO_YAW_RATE_CORR 1

 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 3

-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 3
 param set-default RO_SPEED_I 0.1
 param set-default RO_SPEED_P 1
+param set-default RO_SPEED_RED 1

 # Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1
@@ -15,8 +15,6 @@ param set-default NAV_ACC_RAD 0.5

 # Mecanum Parameters
 param set-default RM_WHEEL_TRACK 0.3
-param set-default RM_MAX_THR_YAW_R 1.2
-param set-default RM_MISS_SPD_GAIN 1

 # Rover Control Parameters
 param set-default RO_ACCEL_LIM 3
@@ -30,14 +28,16 @@ param set-default RO_YAW_RATE_P 0.1
 param set-default RO_YAW_RATE_LIM 120
 param set-default RO_YAW_ACCEL_LIM 240
 param set-default RO_YAW_DECEL_LIM 1000
+param set-default RO_YAW_RATE_CORR 1.75

 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 5

-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 2
 param set-default RO_SPEED_I 0.5
 param set-default RO_SPEED_P 1
+param set-default RO_SPEED_RED 1

 # Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 0.5
@@ -8,3 +8,6 @@
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_lidar_down}

 . ${R}etc/init.d-posix/airframes/4001_gz_x500
+
+param set-default EKF2_RNG_POS_Z -0.177
+param set-default EKF2_MIN_RNG 0
@@ -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
@@ -26,7 +26,6 @@ param set-default NAV_ACC_RAD 0.5

 # Differential Parameters
 param set-default RD_WHEEL_TRACK 0.3
-param set-default RD_MAX_THR_YAW_R 0.7
 param set-default RD_TRANS_DRV_TRN 0.785398
 param set-default RD_TRANS_TRN_DRV 0.139626

@@ -42,14 +41,16 @@ param set-default RO_YAW_RATE_P 0.1
 param set-default RO_YAW_RATE_LIM 250
 param set-default RO_YAW_ACCEL_LIM 600
 param set-default RO_YAW_DECEL_LIM 600
+param set-default RO_YAW_RATE_CORR 2.7

 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 2.5

-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 1.6
 param set-default RO_SPEED_I 0.01
 param set-default RO_SPEED_P 0.1
+param set-default RO_SPEED_RED 1

 # Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1
@@ -33,14 +33,16 @@ param set-default RO_MAX_THR_SPEED 2.8
 param set-default RO_YAW_RATE_I 0.1
 param set-default RO_YAW_RATE_P 0.1
 param set-default RO_YAW_RATE_LIM 120
+param set-default RO_YAW_RATE_CORR 1

 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 2.5

-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 2.5
 param set-default RO_SPEED_I 0.01
 param set-default RO_SPEED_P 0.1
+param set-default RO_SPEED_RED 1

 # Pure pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1
@@ -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
@@ -20,6 +20,7 @@ CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
 CONFIG_COMMON_INS=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
@@ -42,8 +43,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_MODE_MANAGER=y
 CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -12,10 +12,17 @@ else
 fi

 iim42652 -s -R 22 start
-bmi088 -A -R 29 -s start
-bmi088 -G -R 29 -s start

-ist8310 -I -R 18 start
+bmi088 -A -R 29 -s start
+if ! bmi088 -G -R 29 -s start
+then
+	iim42653 -s -b 2 -R 22 start
+fi
+
+if ! ist8310 -I -R 18 start
+then
+	iis2mdc -I -R 37 start
+fi

 bmp581 -s start
 icp201xx -I start
@@ -42,6 +42,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI2, {
 		initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortG, GPIO::Pin2}, SPI::DRDY{GPIO::PortG, GPIO::Pin3}),
 		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortD, GPIO::Pin12}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
 	}, {GPIO::PortI, GPIO::Pin11}),
 	// initSPIBus(SPI::Bus::SPI3,{
 	// 	// no devices
@@ -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,11 +9,45 @@ This topic explains how to build the PX4 bootloader, and several methods for fla

 ::: info

- Most boards will need to use the [Debug Probe](#debug-probe-bootloader-update) to update the bootloader.
+- You can use [QGC Bootloader Update](#qgc-bootloader-update-sys-bl-update) with firmware that includes the [`bl-update` module](../modules/modules_command.md#bl-update).
+  This is the easiest way to update the bootloader, provided the board is able to load firmware.
+- You can also use the [Debug Probe](#debug-probe-bootloader-update) to update the bootloader.
+  This is useful for updating/fixing the bootloader when the board is bricked.
 - On [FMUv6X-RT](../flight_controller/pixhawk6x-rt.md) you can [install bootloader/unbrick boards via USB](bootloader_update_v6xrt.md).
  This is useful if you don't have a debug probe.
- On FMUv2 and some custom firmware (only) you can use [QGC Bootloader Update](#qgc-bootloader-update).
-  :::
+
+:::
+
+## QGC Bootloader Update (`SYS_BL_UPDATE`)
+
+The easiest way to update the bootloader is to first use _QGroundControl_ to install firmware that contains the desired/latest bootloader.
+You can then initiate bootloader update on next restart by setting the parameter: [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE).
+
+This approach can be used if the [`bl-update` module](../modules/modules_command.md#bl-update) is present in the firmware.
+The easiest way to check this is just to see if the [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE) parameter is [found in QGroundControl](../advanced_config/parameters.md#finding-a-parameter).
+
+:::warning
+Boards that include the module will have the line `CONFIG_SYSTEMCMDS_BL_UPDATE=y` in their `default.px4board` file (for examples [see this search](https://github.com/search?q=repo%3APX4%2FPX4-Autopilot+path%3A**%2Fdefault.px4board+CONFIG_SYSTEMCMDS_BL_UPDATE%3Dy&type=code)).
+You can enable this key in your own custom firmware if needed.
+:::
+
+The steps are:
+
+1. Insert an SD card (enables boot logging to debug any problems).
+1. [Update the Firmware](../config/firmware.md#custom) with an image containing the new/desired bootloader.
+
+   ::: info
+   The updated bootloader might be included the default firmware for your board or supplied in custom firmware.
+   :::
+
+1. Wait for the vehicle to reboot.
+1. [Find and enable](../advanced_config/parameters.md) the parameter [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE).
+1. Reboot (disconnect/reconnect the board).
+   The bootloader update will only take a few seconds.
+
+Generally at this point you may then want to [update the firmware](../config/firmware.md) again using the correct/newly installed bootloader.
+
+An specific example of this process for updating the [FMUv2 bootloader](#fmuv2-bootloader-update) is given below.

 ## Building the PX4 Bootloader

@@ -49,11 +83,9 @@ The instructions in the repo README explain how to use it.
 The following steps explain how you can "manually" update the bootloader using a [compatible Debug Probe](../debug/swd_debug.md#debug-probes-for-px4-hardware):

 1. Get a binary containing the bootloader (either from dev team or [build it yourself](#building-the-px4-bootloader)).
-
-1. Get a [Debug Probe](../debug/swd_debug.md#debug-probes-for-px4-hardware).
+2. Get a [Debug Probe](../debug/swd_debug.md#debug-probes-for-px4-hardware).
   Connect the probe your PC via USB and setup the `gdbserver`.
-
-1. Go into the directory containing the binary and run the command for your target bootloader in the terminal:
+3. Go into the directory containing the binary and run the command for your target bootloader in the terminal:

   - FMUv6X

@@ -78,7 +110,7 @@ The following steps explain how you can "manually" update the bootloader using a
   Bootloaders from [PX4/PX4-Bootloader](https://github.com/PX4/PX4-Bootloader) are named with the pattern `*_bl.elf`.
   :::

-1. The _gdb terminal_ appears and it should display the following output:
+4. The _gdb terminal_ appears and it should display the following output:

   ```sh
   GNU gdb (GNU Tools for Arm Embedded Processors 7-2017-q4-major) 8.0.50.20171128-git
@@ -98,28 +130,27 @@ The following steps explain how you can "manually" update the bootloader using a
   Reading symbols from px4fmuv5_bl.elf...done.
   ```

-1. Find your `<dronecode-probe-id>` by running an `ls` command in the **/dev/serial/by-id** directory.
-
-1. Now connect to the debug probe with the following command:
+5. Find your `<dronecode-probe-id>` by running an `ls` command in the **/dev/serial/by-id** directory.
+6. Now connect to the debug probe with the following command:

   ```sh
   tar ext /dev/serial/by-id/<dronecode-probe-id>
   ```

-1. Power on the Pixhawk with another USB cable and connect the probe to the `FMU-DEBUG` port.
+7. Power on the Pixhawk with another USB cable and connect the probe to the `FMU-DEBUG` port.

   ::: info
   If using a Dronecode probe you may need to remove the case in order to connect to the `FMU-DEBUG` port (e.g. on Pixhawk 4 you would do this using a T6 Torx screwdriver).
   :::

-1. Use the following command to scan for the Pixhawk`s SWD and connect to it:
+8. Use the following command to scan for the Pixhawk`s SWD and connect to it:

   ```sh
   (gdb) mon swdp_scan
   (gdb) attach 1
   ```

-1. Load the binary into the Pixhawk:
+9. Load the binary into the Pixhawk:

   ```sh
   (gdb) load
@@ -127,38 +158,10 @@ The following steps explain how you can "manually" update the bootloader using a

 After the bootloader has updated you can [Load PX4 Firmware](../config/firmware.md) using _QGroundControl_.

-## QGC Bootloader Update
-
-The easiest approach is to first use _QGroundControl_ to install firmware that contains the desired/latest bootloader.
-You can then initiate bootloader update on next restart by setting the parameter: [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE).
-
-This approach can only be used if [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE) is present in firmware.
-
-:::warning
-Currently only FMUv2 and some custom firmware includes the desired bootloader.
-:::
-
-The steps are:
-
-1. Insert an SD card (enables boot logging to debug any problems).
-1. [Update the Firmware](../config/firmware.md#custom) with an image containing the new/desired bootloader.
-
-   ::: info
-   The updated bootloader might be supplied in custom firmware (i.e. from the dev team), or it or may be included in the latest main branch.
-   :::
-
-1. Wait for the vehicle to reboot.
-1. [Find and enable](../advanced_config/parameters.md) the parameter [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE).
-1. Reboot (disconnect/reconnect the board).
-   The bootloader update will only take a few seconds.
-
-Generally at this point you may then want to [update the firmware](../config/firmware.md) again using the correct/newly installed bootloader.
-
-An specific example of this process for updating the FMUv2 bootloader is given below.
-
-### FMUv2 Bootloader Update
+## FMUv2 Bootloader Update

 If _QGroundControl_ installs the FMUv2 target (see console during installation), and you have a newer board, you may need to update the bootloader in order to access all the memory on your flight controller.
+This example explains how you can use [QGC Bootloader Update](qgc-bootloader-update-sys-bl-update) to update the bootloader.

 ::: info
 Early FMUv2 [Pixhawk-series](../flight_controller/pixhawk_series.md#fmu_versions) flight controllers had a [hardware issue](../flight_controller/silicon_errata.md#fmuv2-pixhawk-silicon-errata) that restricted them to using 1MB of flash memory.
@@ -168,17 +171,17 @@ The problem is fixed on newer boards, but you may need to update the factory-pro
 To update the bootloader:

 1. Insert an SD card (enables boot logging to debug any problems).
-1. [Update the Firmware](../config/firmware.md) to PX4 _master_ version (when updating the firmware, check **Advanced settings** and then select **Developer Build (master)** from the dropdown list).
+2. [Update the Firmware](../config/firmware.md) to PX4 _master_ version (when updating the firmware, check **Advanced settings** and then select **Developer Build (master)** from the dropdown list).
   _QGroundControl_ will automatically detect that the hardware supports FMUv2 and install the appropriate Firmware.

   ![FMUv2 update](../../assets/qgc/setup/firmware/bootloader_update.jpg)

   Wait for the vehicle to reboot.

-1. [Find and enable](../advanced_config/parameters.md) the parameter [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE).
-1. Reboot (disconnect/reconnect the board).
+3. [Find and enable](../advanced_config/parameters.md) the parameter [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE).
+4. Reboot (disconnect/reconnect the board).
   The bootloader update will only take a few seconds.
-1. Then [Update the Firmware](../config/firmware.md) again.
+5. Then [Update the Firmware](../config/firmware.md) again.
   This time _QGroundControl_ should autodetect the hardware as FMUv3 and update the Firmware appropriately.

   ![FMUv3 update](../../assets/qgc/setup/firmware/bootloader_fmu_v3_update.jpg)
@@ -3,7 +3,7 @@
 _Collision Prevention_ may be used to automatically slow and stop a vehicle before it can crash into an obstacle.
 It can be enabled for multicopter vehicles when using acceleration-based [Position mode](../flight_modes_mc/position.md) (or VTOL vehicles in MC mode).

-It can be enabled for multicopter vehicles in [Position mode](../flight_modes_mc/position.md), and can use sensor data from an offboard companion computer, offboard rangefinders over MAVLink, a rangefinder attached to the flight controller, or any combination of the above.
+It can be enabled for multicopter vehicles in [Position mode](../flight_modes_mc/position.md) (with [MPC_POS_MODE](#MPC_POS_MODE) set to `Acceleration based`), and can use sensor data from an offboard companion computer, offboard rangefinders over MAVLink, a rangefinder attached to the flight controller, or any combination of the above.

 Collision prevention may restrict vehicle maximum speed if the sensor range isn't large enough!
 It also prevents motion in directions where no sensor data is available (i.e. if you have no rear-sensor data, you will not be able to fly backwards).
@@ -26,15 +26,14 @@ Users are notified through _QGroundControl_ while _Collision Prevention_ is acti

 PX4 software setup is covered in the next section.
 If you are using a distance sensor attached to your flight controller for collision prevention, it will need to be attached and configured as described in [PX4 Distance Sensor](#rangefinder).
-If you are using a companion computer to provide obstacle information see [companion setup](#companion
+If you are using a companion computer to provide obstacle information see [companion setup](#companion) below.

-## Supported Rangefinders  {#rangefinder}
+## Supported Rangefinders {#rangefinder}

 ### Lanbao PSK-CM8JL65-CC5 [Discontinued]

 At time of writing PX4 allows you to use the [Lanbao PSK-CM8JL65-CC5](../sensor/cm8jl65_ir_distance_sensor.md) IR distance sensor for collision prevention “out of the box”, with minimal additional configuration:

-
 - First [attach and configure the sensor](../sensor/cm8jl65_ir_distance_sensor.md), and enable collision prevention (as described above, using [CP_DIST](#CP_DIST)).
 - Set the sensor orientation using [SENS_CM8JL65_R_0](../advanced_config/parameter_reference.md#SENS_CM8JL65_R_0).

@@ -51,7 +50,6 @@ Other sensors may be enabled, but this requires modification of driver code to s
  This should be done by mimicking the `SENS_CM8JL65_R_0` parameter (though you might also hard-code the orientation in the sensor _module.yaml_ file to something like `sf0x start -d ${SERIAL_DEV} -R 25` - where 25 is equivalent to `ROTATION_DOWNWARD_FACING`).
 - Modify the driver to set the _field of view_ in the distance sensor UORB topic (`distance_sensor_s.h_fov`).

-
 ## PX4 (Software) Setup

 Configure collision prevention by [setting the following parameters](../advanced_config/parameters.md) in _QGroundControl_:
@@ -62,7 +60,7 @@ Configure collision prevention by [setting the following parameters](../advanced
 | <a id="CP_DELAY"></a>[CP_DELAY](../advanced_config/parameter_reference.md#CP_DELAY)                | Set the sensor and velocity setpoint tracking delay. See [Delay Tuning](#delay_tuning) below.                                                                                                                                                                                                   |
 | <a id="CP_GUIDE_ANG"></a>[CP_GUIDE_ANG](../advanced_config/parameter_reference.md#CP_GUIDE_ANG)    | Set the angle (to both sides of the commanded direction) within which the vehicle may deviate if it finds fewer obstacles in that direction. See [Guidance Tuning](#angle_change_tuning) below.                                                                                                 |
 | <a id="CP_GO_NO_DATA"></a>[CP_GO_NO_DATA](../advanced_config/parameter_reference.md#CP_GO_NO_DATA) | Set to 1 to allow the vehicle to move in directions where there is no sensor coverage (default is 0/`False`).                                                                                                                                                                                   |
-| <a id="MPC_POS_MODE"></a>[MPC_POS_MODE](../advanced_config/parameter_reference.md#MPC_POS_MODE)    | Set to `Direct velocity` or `Smoothed velocity` to enable Collision Prevention in Position Mode (default is `Acceleration based`).                                                                                                                                                              |
+| <a id="MPC_POS_MODE"></a>[MPC_POS_MODE](../advanced_config/parameter_reference.md#MPC_POS_MODE)    | Must be set to `Acceleration based`.                                                                                                                                                                                                                                                           |

 ## Algorithm Description

@@ -131,7 +129,6 @@ The guidance feature will never direct the vehicle in a direction without sensor
 If the vehicle feels stuck with only a single distance sensor pointing forwards, this is probably because the guidance cannot safely adapt the direction due to lack of information.
 :::

-
 ## Algorithm Description

 The data from all sensors are fused into an internal representation of 72 sectors around the vehicle, each containing either the sensor data and information about when it was last observed, or an indication that no data for the sector was available.
@@ -212,7 +209,7 @@ The steps are:
     type: px4_msgs::msg::ObstacleDistance
   ```

-   For more information see [DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml) in [uXRCE-DDS](../middleware/uxrce_dds.md) (PX4-ROS 2/DDS Bridge)_.
+   For more information see [DDS Topics YAML](../middleware/uxrce_dds.md#dds-topics-yaml) in [uXRCE-DDS](../middleware/uxrce_dds.md) (PX4-ROS 2/DDS Bridge)\_.

 3. Open PlotJuggler and navigate to the **Tools > Reactive Script Editor** section.
   In the **Script Editor** tab, add following scripts in the appropriate sections:
@@ -36,7 +36,7 @@ Remove propellers before changing ESC configuration parameters!

 Enable DShot for your required outputs in the [Actuator Configuration](../config/actuators.md).

-DShot comes with different speed options: _DShot150_, _DShot300_, _DShot600_ and _DShot1200_, where the number indicates the speed in kilo-bits/second.
+DShot comes with different speed options: _DShot150_, _DShot300_, and _DShot600_ where the number indicates the speed in kilo-bits/second.
 You should set the parameter to the highest speed supported by your ESC (according to its datasheet).

 Then connect the battery and arm the vehicle.
@@ -110,18 +110,19 @@ The most important ones are:

  :::

-## Telemetry
-
-Some ESCs are capable of sending telemetry back to the flight controller, including:
-
- temperature
- voltage
- current
- accumulated current consumption
- RPM values
+## ESC Telemetry

+Some ESCs are capable of sending telemetry back to the flight controller through a UART RX port.
 These DShot ESCs will have an additional telemetry wire.

+The provided telemetry includes:
+
+- Temperature
+- Voltage
+- Current
+- Accumulated current consumption
+- RPM values
+
 To enable this feature (on ESCs that support it):

 1. Join all the telemetry wires from all the ESCs together, and then connect them to one of the RX pins on an unused flight controller serial port.
@@ -147,3 +148,40 @@ ERROR [dshot] No data received. If telemetry is setup correctly, try again.

 Check manufacturer documentation for confirmation/details.
 :::
+
+## Bidirectional DShot (Telemetry)
+
+<Badge type="tip" text="PX4 v1.16" />
+
+Bidirectional DShot is a protocol that can provide telemetry including: high rate ESC RPM data, voltage, current, and temperature with a single wire.
+
+The PX4 implementation currently enables only ESC RPM (eRPM) data collection from each ESC at high frequencies.
+This telemetry significantly improves the performance of [Dynamic Notch Filters](../config_mc/filter_tuning.md#dynamic-notch-filters) and enables more precise vehicle tuning.
+
+::: info
+The [ESC Telemetry](#esc-telemetry) described above is currently still necessary if you want voltage, current, or temperature information.
+It's setup and use is independent of bidirectional DShot.
+:::
+
+### Hardware Setup
+
+The ESC must be connected to FMU outputs only.
+These will be labeled `MAIN` on flight controllers that only have one PWM bus, and `AUX` on controllers that have both `MAIN` and `AUX` ports (i.e. FCs that have an IO board).
+
+:::warning **Limited hardware support**
+This feature is only supported on flight controllers with the following processors:
+
+- STM32H7: First four FMU outputs
+  - Must be connected to the first 4 FMU outputs, and these outputs must also be mapped to the same timer.
+  - [KakuteH7](../flight_controller/kakuteh7v2.md) is not supported because the outputs are not mapped to the same timer.
+- [i.MXRT](../flight_controller/nxp_mr_vmu_rt1176.md) (V6X-RT & Tropic): 8 FMU outputs.
+
+No other boards are supported.
+:::
+
+### Configuration {#bidirectional-dshot-configuration}
+
+To enable bidirectional DShot, set the [DSHOT_BIDIR_EN](../advanced_config/parameter_reference.md#DSHOT_BIDIR_EN) parameter.
+
+The system calculates actual motor RPM from the received eRPM data using the [MOT_POLE_COUNT](../advanced_config/parameter_reference.md#MOT_POLE_COUNT) parameter.
+This parameter must be set correctly for accurate RPM reporting.
@@ -40,7 +40,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide).

 ### Common

- TBD
+- [QGroundControl Bootloader Update](../advanced_config/bootloader_update.md#qgc-bootloader-update-sys-bl-update) via the [SYS_BL_UPDATE](../advanced_config/parameter_reference.md#SYS_BL_UPDATE) parameter has been re-enabled after being broken for a number of releases. ([PX4-Autopilot#25032: build: romf: fix generation of rc.board_bootloader_upgrade](https://github.com/PX4/PX4-Autopilot/pull/25032)).

 ### Control

@@ -4,15 +4,15 @@

 It is recommended for:

-* Connecting offboard components that require low bandwidth and low latency communication, e.g. [rangefinders](../sensor/rangefinders.md), [magnetometers](../gps_compass/magnetometer.md), [airspeed sensors](../sensor/airspeed.md) and [tachometers](../sensor/tachometers.md) .
+* Connecting offboard components that require low bandwidth and low latency communication, e.g. [rangefinders](../sensor/rangefinders.md), [magnetometers](../gps_compass/magnetometer.md), [airspeed sensors](../sensor/airspeed.md) and [tachometers](../sensor/tachometers.md).
 * Compatibility with peripheral devices that only support I2C.
 * Allowing multiple devices to attach to a single bus, which is useful for conserving ports.

 I2C allows multiple master devices to connect to multiple slave devices using only 2 wires per connection (SDA, SCL).
-in theory a bus can support 128 devices, each accessed via its unique address.
+in theory, a bus can support 128 devices, each accessed via its unique address.

 ::: info
-UAVCAN would normally be preferred where higher data rates are required, and on larger vehicles where sensors are be mounted further from the flight controller.
+UAVCAN would normally be preferred where higher data rates are required, and on larger vehicles where sensors are mounted further from the flight controller.
 :::


@@ -20,7 +20,7 @@ UAVCAN would normally be preferred where higher data rates are required, and on

 I2C uses a pair of wires: SDA (serial data) and SCL (serial clock).
 The bus is of open-drain type, meaning that devices ground the data line.
-It uses a pullup resistor to push it to `log.1` (idle state) - every wire has it usually located on the bus terminating devices.
+It uses a pull-up resistor to push it to `log.1` (idle state) - every wire has it usually located on the bus terminating devices.
 One bus can connect to multiple I2C devices.
 The individual devices are connected without any crossing.

@@ -52,41 +52,44 @@ If two I2C devices on a bus have the same ID there will be a clash, and neither
 This usually occurs because a user needs to attach two sensors of the same type to the bus, but may also happen if devices use duplicate addresses by default.

 Particular I2C devices may allow you to select a new address for one of the devices to avoid the clash.
-Some devices do not support this option, or do not have broad options for the addresses that can be used (i.e. cannot be used to avoid a clash).
+Some devices do not support this option or do not have broad options for the addresses that can be used (i.e. cannot be used to avoid a clash).

 If you can't change the addresses, one option is to use an [I2C Address Translator](#i2c-address-translators).

 ### Insufficient Transfer Capacity

-The bandwidth available for each individual device generally decreases as more devices are added. The exact decrease depends on the bandwidth used by each individual device. Therefore it is possible to connect many low bandwidth devices, like [tachometers](../sensor/tachometers.md).
+The bandwidth available for each device generally decreases as more devices are added. The exact decrease depends on the bandwidth used by each individual device. Therefore it is possible to connect many low-bandwidth devices, like [tachometers](../sensor/tachometers.md).
 If too many devices are added, it can cause transmission errors and network unreliability.

 There are several ways to reduce the problem:
-* Dividing the devices into groups, each with approximately the same number of devices and connecting each group to one autopilot port
+* Dividing the devices into groups, each with approximately the same number of devices, and connecting each group to one autopilot port
 * Increase bus speed limit (usually set to 100kHz for external I2C bus)

 ### Excessive Wiring Capacitance

-The electrical capacity of bus wiring increases as more devices/wires are added. The exact decrease depends on total length of bus wiring and wiring specific capacitance.
+The electrical capacity of bus wiring increases as more devices/wires are added. The exact decrease depends on the total length of bus wiring and wiring-specific capacitance.
 The problem can be analyzed using an oscilloscope, where we see that the edges of SDA/SCL signals are no longer sharp.

 There are several ways to reduce the problem:
-* Dividing the devices into groups, each with approximately the same number of devices and connecting each group to one autopilot port
-* Using the shortest and the highest quality I2C cables possible
-* Separating the devices with a weak open-drain driver to smaller bus with lower capacitance
-* [I2C Bus Accelerators](#i2c-bus-accelerators)
+* Dividing the devices into groups, each with approximately the same number of devices, and connecting each group to one autopilot port
+* Using the shorter and higher quality I2C cables, see the [cable wiring page](../assembly/cable_wiring.md#i2c-cables) for details
+* Separating the devices with a weak open-drain driver to smaller buses with lower capacitance by using [I2C Bus Accelerators](#i2c-bus-accelerators)

 ## I2C Bus Accelerators

-I2C bus accelerators are separate circuits that can be used to support longer wiring length on an I2C bus.
+I2C bus accelerators are separate circuits that can be used to support longer wiring lengths on an I2C bus.
 They work by physically dividing an I2C network into 2 parts and using their own transistors to amplify I2C signals.

 Available accelerators include:
- [Thunderfly TFI2CEXT01](https://github.com/ThunderFly-aerospace/TFI2CEXT01):
+- [Thunderfly TFI2CEXT01](https://docs.thunderfly.cz/avionics/TFI2CEXT01/):
  ![I2C bus extender](../../assets/peripherals/i2c_tfi2cext/tfi2cext01a_bottom.jpg)
  - This has Dronecode connectors and is hence very easy to add to a Pixhawk I2C setup.
  - The module has no settings (it works out of the box).

+### I2C Level Converter function
+
+Some I2C devices have 5V on the data lines, while the Pixhawk connector standard port expects these lines to be 3.3 V.
+You can use the TFI2CEXT01 as a level converter to connect 5V devices to a Pixhawk I2C port. This feature is possible because the SCL and SDA lines of TFI2CEXT01 are 5V tolerant. 

 ## I2C Address Translators

@@ -96,21 +99,12 @@ The work by listening for I2C communication and transforming the address when a
 Supported I2C Address Translators include:

 - [Thunderfly TFI2CADT01](../sensor_bus/translator_tfi2cadt.md)
-
+  - This has Dronecode connectors and is very easy to add to a Pixhawk I2C setup.

 ## I2C Bus Splitters

-I2C Bus Splitters are circuit boards that split the I2C port on your flight controller into multiple ports.
-They are useful if you want to use multiple I2C peripherals on a flight controller that has only one I2C port (or too few), such as an airspeed sensor and a distance sensor.
-
-You can find an appropriate board using an internet search.
-
-## I2C Level Converter
-
-Some I2C devices have 5V on the data lines, while the Pixhawk connector standard port expects these lines to be 3.3 V.
-You can use an I2C level converter to connect 5V devices to a Pixhawk I2C port.
-
-You can find an appropriate covnerter using an internet search.
+I2C Bus Splitters are devices that split the I2C port on your flight controller into multiple connectors.
+They are useful if you want to use multiple I2C peripherals on a flight controller that has only one I2C port (or too few), such as an airspeed sensor and a distance sensor. Both devices [I2C Address Translator](../sensor_bus/translator_tfi2cadt.md) and [I2C Bus Accelerators](#i2c-bus-accelerators) could also be used as I2C splitters because they have multiple I2C connectors for connecting additional I2C devices.

 ## I2C Development

@@ -1,12 +1,11 @@
 # TFI2CADT01 - I²C Address Translator

-[TFI2CADT01](https://github.com/ThunderFly-aerospace/TFI2CADT01) is an address translator module that is compatible with Pixhawk and PX4.
+[TFI2CADT01](https://docs.thunderfly.cz/avionics/TFI2CADT01/) is an address translator module that is compatible with Pixhawk and PX4.

 Address translation allows multiple I2C devices with the same address to coexist on an I2C network.
 The module may be needed if using several devices that have the same hard-coded address.

-The module has an input and an output side.
-A sensor is connected to the master device on one side.
+The module has an input and an output side and a sensor is connected to the master device on one side.
 On the output side sensors, whose addresses are to be translated, can be connected.
 The module contains two pairs of connectors, each pair responsible for different translations.

@@ -14,7 +13,7 @@ The module contains two pairs of connectors, each pair responsible for different

 ::: info
 [TFI2CADT01](https://github.com/ThunderFly-aerospace/TFI2CADT01) is designed as open-source hardware with GPLv3 license.
-It is commercially available from [ThunderFly](https://www.thunderfly.cz/) company or from [Tindie eshop](https://www.tindie.com/products/thunderfly/tfi2cadt01-i2c-address-translator/).
+It is commercially available from [ThunderFly](https://www.thunderfly.cz/) company or from [Tindie eshop](https://www.tindie.com/products/26353/).
 :::

 ## Address Translation Method
@@ -31,7 +30,7 @@ If you need your own value for address translation, changing the configuration r
 The tachometer sensor [TFRPM01](../sensor/thunderfly_tachometer.md) can be set to two different addresses using a solder jumper.
 If the autopilot has three buses, only 6 sensors can be connected and no bus remains free (2 available addresses * 3 I2C ports).
 In some multicopters or VTOL solutions, there is a need to measure the RPM of 8 or more elements.
-The [TFI2CADT01](https://www.tindie.com/products/thunderfly/tfi2cadt01-i2c-address-translator/) is highly recommended in this case.
+The [TFI2CADT01](https://www.tindie.com/products/26353/) is highly recommended in this case.

 ![Multiple sensors](../../assets/peripherals/i2c_tfi2cadt/tfi2cadt01_multi_tfrpm01.jpg)

@@ -58,7 +57,7 @@ graph TD

 ::: info
 TFI2CADT01 does not contain any I2C buffer or accelerator.
-As it adds additional capacitance on the bus, we advise combining it with some bus booster, e.g. [TFI2CEXT01](https://github.com/ThunderFly-aerospace/TFI2CEXT01).
+As it adds additional capacitance on the bus, we advise combining it with some bus booster, e.g. [TFI2CEXT01](https://docs.thunderfly.cz/avionics/TFI2CEXT01/).
 :::

 ### Other Resources
@@ -0,0 +1,9 @@
+# Battery information
+#
+# Static or near-invariant battery information.
+# Should be streamed at low rate.
+
+uint64 timestamp # [us] Time since system start
+
+uint8 id # Must match the id in the battery_status message for the same battery
+char[32] serial_number # [@invalid 0 All bytes] Serial number of the battery pack in ASCII characters, 0 terminated
@@ -47,6 +47,7 @@ set(msg_files
 	Airspeed.msg
 	AirspeedWind.msg
 	AutotuneAttitudeControlStatus.msg
+	BatteryInfo.msg
 	ButtonEvent.msg
 	CameraCapture.msg
 	CameraStatus.msg
@@ -226,6 +227,7 @@ set(msg_files
 	VehicleImu.msg
 	VehicleImuStatus.msg
 	VehicleLocalPositionSetpoint.msg
+	TaskLocalPositionSetpoint.msg
 	VehicleMagnetometer.msg
 	VehicleOpticalFlow.msg
 	VehicleOpticalFlowVel.msg
@@ -2,37 +2,37 @@
 #
 # This is currently used only for logging cell status from MAVLink.

-uint64 timestamp # [us] Time since system start.
+uint64 timestamp # [us] Time since system start

-uint16 status # [@enum STATUS_FLAG] Status bitmap.
-uint16 STATUS_FLAG_UNKNOWN = 1 # State unknown or not reportable.
-uint16 STATUS_FLAG_FAILED = 2 # Modem is unusable.
-uint16 STATUS_FLAG_INITIALIZING = 4 # Modem is being initialized.
-uint16 STATUS_FLAG_LOCKED = 8 # Modem is locked.
-uint16 STATUS_FLAG_DISABLED = 16 # Modem is not enabled and is powered down.
-uint16 STATUS_FLAG_DISABLING = 32 # Modem is currently transitioning to the STATUS_FLAG_DISABLED state.
-uint16 STATUS_FLAG_ENABLING = 64 # Modem is currently transitioning to the STATUS_FLAG_ENABLED state.
-uint16 STATUS_FLAG_ENABLED = 128 # Modem is enabled and powered on but not registered with a network provider and not available for data connections.
-uint16 STATUS_FLAG_SEARCHING = 256 # Modem is searching for a network provider to register.
-uint16 STATUS_FLAG_REGISTERED = 512 # Modem is registered with a network provider, and data connections and messaging may be available for use.
-uint16 STATUS_FLAG_DISCONNECTING = 1024 # Modem is disconnecting and deactivating the last active packet data bearer. This state will not be entered if more than one packet data bearer is active and one of the active bearers is deactivated.
-uint16 STATUS_FLAG_CONNECTING = 2048 # Modem is activating and connecting the first packet data bearer. Subsequent bearer activations when another bearer is already active do not cause this state to be entered.
-uint16 STATUS_FLAG_CONNECTED = 4096 # One or more packet data bearers is active and connected.
+uint16 status # [@enum STATUS_FLAG] Status bitmap
+uint16 STATUS_FLAG_UNKNOWN = 1 # State unknown or not reportable
+uint16 STATUS_FLAG_FAILED = 2 # Modem is unusable
+uint16 STATUS_FLAG_INITIALIZING = 4 # Modem is being initialized
+uint16 STATUS_FLAG_LOCKED = 8 # Modem is locked
+uint16 STATUS_FLAG_DISABLED = 16 # Modem is not enabled and is powered down
+uint16 STATUS_FLAG_DISABLING = 32 # Modem is currently transitioning to the STATUS_FLAG_DISABLED state
+uint16 STATUS_FLAG_ENABLING = 64 # Modem is currently transitioning to the STATUS_FLAG_ENABLED state
+uint16 STATUS_FLAG_ENABLED = 128 # Modem is enabled and powered on but not registered with a network provider and not available for data connections
+uint16 STATUS_FLAG_SEARCHING = 256 # Modem is searching for a network provider to register
+uint16 STATUS_FLAG_REGISTERED = 512 # Modem is registered with a network provider, and data connections and messaging may be available for use
+uint16 STATUS_FLAG_DISCONNECTING = 1024 # Modem is disconnecting and deactivating the last active packet data bearer. This state will not be entered if more than one packet data bearer is active and one of the active bearers is deactivated
+uint16 STATUS_FLAG_CONNECTING = 2048 # Modem is activating and connecting the first packet data bearer. Subsequent bearer activations when another bearer is already active do not cause this state to be entered
+uint16 STATUS_FLAG_CONNECTED = 4096 # One or more packet data bearers is active and connected

-uint8 failure_reason # [@enum FAILURE_REASON] Failure reason.
-uint8 FAILURE_REASON_NONE = 0 # No error.
-uint8 FAILURE_REASON_UNKNOWN = 1 # Error state is unknown.
-uint8 FAILURE_REASON_SIM_MISSING = 2 # SIM is required for the modem but missing.
-uint8 FAILURE_REASON_SIM_ERROR = 3 # SIM is available, but not usable for connection.
+uint8 failure_reason # [@enum FAILURE_REASON] Failure reason
+uint8 FAILURE_REASON_NONE = 0 # No error
+uint8 FAILURE_REASON_UNKNOWN = 1 # Error state is unknown
+uint8 FAILURE_REASON_SIM_MISSING = 2 # SIM is required for the modem but missing
+uint8 FAILURE_REASON_SIM_ERROR = 3 # SIM is available, but not usable for connection

-uint8 type # [@enum CELLULAR_NETWORK_RADIO_TYPE] Cellular network radio type.
+uint8 type # [@enum CELLULAR_NETWORK_RADIO_TYPE] Cellular network radio type
 uint8 CELLULAR_NETWORK_RADIO_TYPE_NONE = 0 # None
 uint8 CELLULAR_NETWORK_RADIO_TYPE_GSM = 1 # GSM
 uint8 CELLULAR_NETWORK_RADIO_TYPE_CDMA = 2 # CDMA
 uint8 CELLULAR_NETWORK_RADIO_TYPE_WCDMA = 3 # WCDMA
 uint8 CELLULAR_NETWORK_RADIO_TYPE_LTE = 4 # LTE

-uint8 quality # [dBm] Cellular network RSSI/RSRP, absolute value.
-uint16 mcc # [@invalid UINT16_MAX] Mobile country code.
-uint16 mnc # [@invalid UINT16_MAX] Mobile network code.
-uint16 lac # [@invalid 0] Location area code.
+uint8 quality # [dBm] Cellular network RSSI/RSRP, absolute value
+uint16 mcc # [@invalid UINT16_MAX] Mobile country code
+uint16 mnc # [@invalid UINT16_MAX] Mobile network code
+uint16 lac # [@invalid 0] Location area code
@@ -39,14 +39,12 @@ uint8 CS_FIXED_WING = 17	# 17 - true when thought to be operating as a fixed win
 uint8 CS_MAG_FAULT = 18		# 18 - true when the magnetometer has been declared faulty and is no longer being used
 uint8 CS_ASPD = 19		# 19 - true when airspeed measurements are being fused
 uint8 CS_GND_EFFECT = 20	# 20 - true when when protection from ground effect induced static pressure rise is active
-uint8 CS_RNG_STUCK = 21		# 21 - true when a stuck range finder sensor has been detected
 uint8 CS_GPS_YAW = 22		# 22 - true when yaw (not ground course) data from a GPS receiver is being fused
 uint8 CS_MAG_ALIGNED = 23	# 23 - true when the in-flight mag field alignment has been completed
 uint8 CS_EV_VEL = 24		# 24 - true when local frame velocity data fusion from external vision measurements is intended
 uint8 CS_SYNTHETIC_MAG_Z = 25	# 25 - true when we are using a synthesized measurement for the magnetometer Z component
 uint8 CS_VEHICLE_AT_REST = 26	# 26 - true when the vehicle is at rest
 uint8 CS_GPS_YAW_FAULT = 27	# 27 - true when the GNSS heading has been declared faulty and is no longer being used
-uint8 CS_RNG_FAULT = 28		# 28 - true when the range finder has been declared faulty and is no longer being used
 uint8 CS_GNSS_VEL = 44		# 44 - true if GNSS velocity measurements are being fused

 uint32 filter_fault_flags	# Bitmask to indicate EKF internal faults
@@ -25,14 +25,12 @@ bool cs_fixed_wing            # 17 - true when the vehicle is operating as a fix
 bool cs_mag_fault             # 18 - true when the magnetometer has been declared faulty and is no longer being used
 bool cs_fuse_aspd             # 19 - true when airspeed measurements are being fused
 bool cs_gnd_effect            # 20 - true when protection from ground effect induced static pressure rise is active
-bool cs_rng_stuck             # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
 bool cs_gnss_yaw              # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended
 bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed
 bool cs_ev_vel                # 24 - true when local frame velocity data fusion from external vision measurements is intended
 bool cs_synthetic_mag_z       # 25 - true when we are using a synthesized measurement for the magnetometer Z component
 bool cs_vehicle_at_rest       # 26 - true when the vehicle is at rest
 bool cs_gnss_yaw_fault        # 27 - true when the GNSS heading has been declared faulty and is no longer being used
-bool cs_rng_fault             # 28 - true when the range finder has been declared faulty and is no longer being used
 bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift
 bool cs_wind_dead_reckoning     # 30 - true if we are navigationg reliant on wind relative measurements
 bool cs_rng_kin_consistent      # 31 - true when the range finder kinematic consistency check is passing
@@ -36,3 +36,5 @@ float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
 float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint), only has an effect for rover
+
+# TOPICS position_setpoint task_position_setpoint
@@ -1,5 +1,3 @@
 uint64 timestamp # time since system start (microseconds)

-float32 normalized_steering_angle # [-1, 1] Normalized steering angle (Ackermann only, Positiv: Steer right, Negativ: Steer left)
-
-float32 normalized_speed_diff     # [-1, 1] Normalized speed difference between the left and right wheels of the rover (Differential/Mecanum only, Positiv = Turn right, Negativ: Turn left)
+float32 normalized_steering_setpoint # [-1, 1] Positiv = Turn right, Negativ: Turn left (Ackermann: Steering angle, Differential/Mecanum: Speed difference between the left and right wheels)
@@ -0,0 +1,19 @@
+# Local position setpoint in NED frame
+# Telemetry of PID position controller to monitor tracking.
+# NaN means the state was not controlled
+
+uint64 timestamp	# time since system start (microseconds)
+
+float32 x		# in meters NED
+float32 y		# in meters NED
+float32 z		# in meters NED
+
+float32 vx		# in meters/sec
+float32 vy		# in meters/sec
+float32 vz		# in meters/sec
+
+float32[3] acceleration # in meters/sec^2
+float32[3] thrust	# normalized thrust vector in NED
+
+float32 yaw		# in radians NED -PI..+PI
+float32 yawspeed	# in radians/sec
@@ -7,3 +7,5 @@ float32 speed_up # in meters/sec
 float32 speed_down # in meters/sec

 bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight)
+
+bool lock_dist_bottom # altitude is locked to the current distance to ground
@@ -0,0 +1,79 @@
+# Battery status
+#
+# Battery status information for up to 4 battery instances.
+# These are populated from power module and smart battery device drivers, and one battery updated from MAVLink.
+# Battery instance information is also logged and streamed in MAVLink telemetry.
+
+uint32 MESSAGE_VERSION = 0
+uint8 MAX_INSTANCES = 4
+
+uint64 timestamp # [us] Time since system start
+bool connected # Whether or not a battery is connected. For power modules this is based on a voltage threshold.
+float32 voltage_v # [V] [@invalid 0] Battery voltage
+float32 current_a # [A] [@invalid -1] Battery current
+float32 current_average_a # [A] [@invalid -1] Battery current average (for FW average in level flight)
+float32 discharged_mah # [mAh] [@invalid -1] Discharged amount
+float32 remaining # [@range 0,1] [@invalid -1] Remaining capacity
+float32 scale # [@range 1,] [@invalid -1] Scaling factor to compensate for lower actuation power caused by voltage sag
+float32 time_remaining_s # [s] [@invalid NaN] Predicted time remaining until battery is empty under previous averaged load
+float32 temperature # [°C] [@invalid NaN] Temperature of the battery
+uint8 cell_count # [@invalid 0] Number of cells
+
+
+uint8 source # [@enum SOURCE] Battery source
+uint8 SOURCE_POWER_MODULE = 0 # Power module
+uint8 SOURCE_EXTERNAL = 1 # External
+uint8 SOURCE_ESCS = 2 # ESCs
+
+uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
+uint16 capacity # [mAh] Capacity of the battery when fully charged
+uint16 cycle_count # Number of discharge cycles the battery has experienced
+uint16 average_time_to_empty # [minutes] Predicted remaining battery capacity based on the average rate of discharge
+uint16 serial_number # Serial number of the battery pack
+uint16 manufacture_date # Manufacture date, part of serial number of the battery pack. Formatted as: Day + Month×32 + (Year–1980)×512
+uint16 state_of_health # [%] [@range 0, 100] State of health. FullChargeCapacity/DesignCapacity
+uint16 max_error # [%] [@range 1, 100] Max error, expected margin of error in the state-of-charge calculation
+uint8 id # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed
+uint16 interface_error # Interface error counter
+
+float32[14] voltage_cell_v # [V] [@invalid 0] Battery individual cell voltages
+float32 max_cell_voltage_delta # Max difference between individual cell voltages
+
+bool is_powering_off # Power off event imminent indication, false if unknown
+bool is_required # Set if the battery is explicitly required before arming
+
+uint8 warning # [@enum WARNING STATE] Current battery warning
+uint8 WARNING_NONE = 0 # No battery low voltage warning active
+uint8 WARNING_LOW = 1 # Low voltage warning
+uint8 WARNING_CRITICAL = 2 # Critical voltage, return / abort immediately
+uint8 WARNING_EMERGENCY = 3 # Immediate landing required
+uint8 WARNING_FAILED = 4 # Battery has failed completely
+uint8 STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field
+uint8 STATE_CHARGING = 7 # Battery is charging
+
+uint16 faults # [@enum FAULT] Smart battery supply status/fault flags (bitmask) for health indication
+uint8 FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged
+uint8 FAULT_SPIKES = 1 # Voltage spikes
+uint8 FAULT_CELL_FAIL= 2 # One or more cells have failed
+uint8 FAULT_OVER_CURRENT = 3 # Over-current
+uint8 FAULT_OVER_TEMPERATURE = 4 # Over-temperature
+uint8 FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault
+uint8 FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage)
+uint8 FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware
+uint8 FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system
+uint8 FAULT_HARDWARE_FAILURE = 9 # Hardware problem
+uint8 FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming
+uint8 FAULT_COUNT = 11 # Counter. Keep this as last element
+
+float32 full_charge_capacity_wh # [Wh] Compensated battery capacity
+float32 remaining_capacity_wh # [Wh] Compensated battery capacity remaining
+uint16 over_discharge_count # Number of battery overdischarge
+float32 nominal_voltage # [V] Nominal voltage of the battery pack
+
+float32 internal_resistance_estimate # [Ohm] Internal resistance per cell estimate
+float32 ocv_estimate # [V] Open circuit voltage estimate
+float32 ocv_estimate_filtered # [V] Filtered open circuit voltage estimate
+float32 volt_based_soc_estimate # [@range 0, 1] Normalized volt based state of charge estimate
+float32 voltage_prediction # [V] Predicted voltage
+float32 prediction_error # [V] Prediction error
+float32 estimation_covariance_norm # Norm of the covariance matrix
@@ -6,12 +6,9 @@

 #include <translation_util.h>

-//#include "example_translation_direct_v1.h"
-//#include "example_translation_multi_v2.h"
-//#include "example_translation_service_v1.h"
-
-#include "translation_vehicle_status_v1.h"
 #include "translation_airspeed_validated_v1.h"
-#include "translation_vehicle_attitude_setpoint_v1.h"
 #include "translation_arming_check_reply_v1.h"
+#include "translation_battery_status_v1.h"
 #include "translation_event_v1.h"
+#include "translation_vehicle_attitude_setpoint_v1.h"
+#include "translation_vehicle_status_v1.h"
@@ -0,0 +1,114 @@
+/****************************************************************************
+ * Copyright (c) 2025 PX4 Development Team.
+ * SPDX-License-Identifier: BSD-3-Clause
+ ****************************************************************************/
+#pragma once
+
+// Translate BatteryStatus v0 <--> v1
+#include <px4_msgs_old/msg/battery_status_v0.hpp>
+#include <px4_msgs/msg/battery_status.hpp>
+
+class BatteryStatusV1Translation {
+public:
+	using MessageOlder = px4_msgs_old::msg::BatteryStatusV0;
+	static_assert(MessageOlder::MESSAGE_VERSION == 0);
+
+	using MessageNewer = px4_msgs::msg::BatteryStatus;
+	static_assert(MessageNewer::MESSAGE_VERSION == 1);
+
+	static constexpr const char* kTopic = "fmu/out/battery_status";
+
+	static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) {
+		msg_newer.timestamp = msg_older.timestamp;
+		msg_newer.connected = msg_older.connected;
+		msg_newer.voltage_v = msg_older.voltage_v;
+		msg_newer.current_a = msg_older.current_a;
+		msg_newer.current_average_a = msg_older.current_average_a;
+		msg_newer.discharged_mah = msg_older.discharged_mah;
+		msg_newer.remaining = msg_older.remaining;
+		msg_newer.scale = msg_older.scale;
+		msg_newer.time_remaining_s = msg_older.time_remaining_s;
+		msg_newer.temperature = msg_older.temperature;
+		msg_newer.cell_count = msg_older.cell_count;
+		msg_newer.source = msg_older.source;
+		msg_newer.priority = msg_older.priority;
+		msg_newer.capacity = msg_older.capacity;
+		msg_newer.cycle_count = msg_older.cycle_count;
+		msg_newer.average_time_to_empty = msg_older.average_time_to_empty;
+		// The serial number moved to the battery_info message and is char[32] instead of uint16
+		msg_newer.manufacture_date = msg_older.manufacture_date;
+		msg_newer.state_of_health = msg_older.state_of_health;
+		msg_newer.max_error = msg_older.max_error;
+		msg_newer.id = msg_older.id;
+		msg_newer.interface_error = msg_older.interface_error;
+
+		for (int i = 0; i < 14; ++i) {
+			msg_newer.voltage_cell_v[i] = msg_older.voltage_cell_v[i];
+		}
+
+		msg_newer.max_cell_voltage_delta = msg_older.max_cell_voltage_delta;
+		msg_newer.is_powering_off = msg_older.is_powering_off;
+		msg_newer.is_required = msg_older.is_required;
+		msg_newer.warning = msg_older.warning;
+		msg_newer.faults = msg_older.faults;
+		msg_newer.full_charge_capacity_wh = msg_older.full_charge_capacity_wh;
+		msg_newer.remaining_capacity_wh = msg_older.remaining_capacity_wh;
+		msg_newer.over_discharge_count = msg_older.over_discharge_count;
+		msg_newer.nominal_voltage = msg_older.nominal_voltage;
+		msg_newer.internal_resistance_estimate = msg_older.internal_resistance_estimate;
+		msg_newer.ocv_estimate = msg_older.ocv_estimate;
+		msg_newer.ocv_estimate_filtered = msg_older.ocv_estimate_filtered;
+		msg_newer.volt_based_soc_estimate = msg_older.volt_based_soc_estimate;
+		msg_newer.voltage_prediction = msg_older.voltage_prediction;
+		msg_newer.prediction_error = msg_older.prediction_error;
+		msg_newer.estimation_covariance_norm = msg_older.estimation_covariance_norm;
+	}
+
+	static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) {
+		msg_older.timestamp = msg_newer.timestamp;
+		msg_older.connected = msg_newer.connected;
+		msg_older.voltage_v = msg_newer.voltage_v;
+		msg_older.current_a = msg_newer.current_a;
+		msg_older.current_average_a = msg_newer.current_average_a;
+		msg_older.discharged_mah = msg_newer.discharged_mah;
+		msg_older.remaining = msg_newer.remaining;
+		msg_older.scale = msg_newer.scale;
+		msg_older.time_remaining_s = msg_newer.time_remaining_s;
+		msg_older.temperature = msg_newer.temperature;
+		msg_older.cell_count = msg_newer.cell_count;
+		msg_older.source = msg_newer.source;
+		msg_older.priority = msg_newer.priority;
+		msg_older.capacity = msg_newer.capacity;
+		msg_older.cycle_count = msg_newer.cycle_count;
+		msg_older.average_time_to_empty = msg_newer.average_time_to_empty;
+		msg_older.serial_number = 0; // The serial number moved to the battery_info message and is char[32] instead of uint16
+		msg_older.manufacture_date = msg_newer.manufacture_date;
+		msg_older.state_of_health = msg_newer.state_of_health;
+		msg_older.max_error = msg_newer.max_error;
+		msg_older.id = msg_newer.id;
+		msg_older.interface_error = msg_newer.interface_error;
+
+		for (int i = 0; i < 14; ++i) {
+			msg_older.voltage_cell_v[i] = msg_newer.voltage_cell_v[i];
+		}
+
+		msg_older.max_cell_voltage_delta = msg_newer.max_cell_voltage_delta;
+		msg_older.is_powering_off = msg_newer.is_powering_off;
+		msg_older.is_required = msg_newer.is_required;
+		msg_older.warning = msg_newer.warning;
+		msg_older.faults = msg_newer.faults;
+		msg_older.full_charge_capacity_wh = msg_newer.full_charge_capacity_wh;
+		msg_older.remaining_capacity_wh = msg_newer.remaining_capacity_wh;
+		msg_older.over_discharge_count = msg_newer.over_discharge_count;
+		msg_older.nominal_voltage = msg_newer.nominal_voltage;
+		msg_older.internal_resistance_estimate = msg_newer.internal_resistance_estimate;
+		msg_older.ocv_estimate = msg_newer.ocv_estimate;
+		msg_older.ocv_estimate_filtered = msg_newer.ocv_estimate_filtered;
+		msg_older.volt_based_soc_estimate = msg_newer.volt_based_soc_estimate;
+		msg_older.voltage_prediction = msg_newer.voltage_prediction;
+		msg_older.prediction_error = msg_newer.prediction_error;
+		msg_older.estimation_covariance_norm = msg_newer.estimation_covariance_norm;
+	}
+};
+
+REGISTER_TOPIC_TRANSLATION_DIRECT(BatteryStatusV1Translation);
@@ -4,7 +4,7 @@
 # These are populated from power module and smart battery device drivers, and one battery updated from MAVLink.
 # Battery instance information is also logged and streamed in MAVLink telemetry.

-uint32 MESSAGE_VERSION = 0
+uint32 MESSAGE_VERSION = 1
 uint8 MAX_INSTANCES = 4

 uint64 timestamp # [us] Time since system start
@@ -29,7 +29,6 @@ uint8 priority # Zero based priority is the connection on the Power Controller V
 uint16 capacity # [mAh] Capacity of the battery when fully charged
 uint16 cycle_count # Number of discharge cycles the battery has experienced
 uint16 average_time_to_empty # [minutes] Predicted remaining battery capacity based on the average rate of discharge
-uint16 serial_number # Serial number of the battery pack
 uint16 manufacture_date # Manufacture date, part of serial number of the battery pack. Formatted as: Day + Month×32 + (Year–1980)×512
 uint16 state_of_health # [%] [@range 0, 100] State of health. FullChargeCapacity/DesignCapacity
 uint16 max_error # [%] [@range 1, 100] Max error, expected margin of error in the state-of-charge calculation
@@ -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
@@ -0,0 +1,20 @@
+## FlightTaskManualAltitude
+- dist_bottom_var -- currently terrain variance, I see occasionally dist_bottom diverge and then clamp back to expected
+- ground slowdown (_respectGroundSlowdown) uses mpc_land_alt ... weird, remove?
+- _respectMaxAltitude ... weird, remove? We can instead use dist_bottom validity
+- _respectMinAltitude ... weird, remove? No need for a function
+
+
+## FlightTask
+- _dist_to_bottom and _dist_to_ground ... confusing, unify
+-
+
+## FlightTaskAuto
+- reuse logic for range alt lock
+- _prepareLandSetpoints -- Slow down automatic descend close to ground ... use only with terrain estimate available? (baro estimate unreliable)
+
+## EKF
+- Vz does not reflect true Vz due to noisy baro
+- Vz errors cause rangefinder kinematic consistency check to fail
+- Position setpoint error remains over long periods (might be related to Vz issues above)
+
@@ -77,6 +77,7 @@ BATT_SMBUS::BATT_SMBUS(const I2CSPIDriverConfig &config, SMBus *interface) :

 BATT_SMBUS::~BATT_SMBUS()
 {
+	orb_unadvertise(_battery_info_topic);
 	orb_unadvertise(_batt_topic);
 	perf_free(_cycle);

@@ -165,7 +166,6 @@ void BATT_SMBUS::RunImpl()
 	if (ret == PX4_OK) {
 		new_report.capacity = _batt_capacity;
 		new_report.cycle_count = _cycle_count;
-		new_report.serial_number = _serial_number;
 		new_report.max_cell_voltage_delta = _max_cell_voltage_delta;
 		new_report.cell_count = _cell_count;
 		new_report.state_of_health = _state_of_health;
@@ -192,6 +192,13 @@ void BATT_SMBUS::RunImpl()
 		int instance = 0;
 		orb_publish_auto(ORB_ID(battery_status), &_batt_topic, &new_report, &instance);

+		battery_info_s battery_info{};
+		battery_info.timestamp = new_report.timestamp;
+		battery_info.id = new_report.id;
+		snprintf(battery_info.serial_number, sizeof(battery_info.serial_number), "%" PRIu16, _serial_number);
+		orb_publish_auto(ORB_ID(battery_info), &_battery_info_topic, &battery_info, &instance);
+
+
 		_last_report = new_report;
 	}
 }
@@ -52,6 +52,7 @@
 #include <px4_platform_common/param.h>
 #include <px4_platform_common/getopt.h>
 #include <px4_platform_common/i2c_spi_buses.h>
+#include <uORB/topics/battery_info.h>
 #include <uORB/topics/battery_status.h>

 #include <board_config.h>
@@ -242,7 +243,7 @@ private:
 	/** @param _last_report Last published report, used for test(). */
 	battery_status_s _last_report{};

-	/** @param _batt_topic uORB battery topic. */
+	orb_advert_t _battery_info_topic{nullptr};
 	orb_advert_t _batt_topic{nullptr};

 	/** @param _cell_count Number of series cell. */
@@ -94,7 +94,6 @@ public:
 		bat_status.connected = bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_IN_USE;
 		bat_status.source = 1; // External
 		bat_status.capacity = bat_info.full_charge_capacity_wh;
-		bat_status.serial_number = bat_info.model_instance_id & 0xFFFF; // Take first 16 bits
 		bat_status.state_of_health = bat_info.state_of_health_pct; // External
 		bat_status.id = bat_info.battery_id;

@@ -118,9 +117,17 @@ public:

 		_battery_status_pub.publish(bat_status);
 		print_message(ORB_ID(battery_status), bat_status);
+
+		battery_info_s battery_info{};
+		battery_info.timestamp = bat_status.timestamp;
+		battery_info.id = bat_status.id;
+		snprintf(battery_info.serial_number, sizeof(battery_info.serial_number), "%" PRIu32,
+			 bat_info.model_instance_id);
+		_battery_info_pub.publish(battery_info);
 	};

 private:
+	uORB::PublicationMulti<battery_info_s> _battery_info_pub{ORB_ID(battery_info)};
 	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};

 };
@@ -41,6 +41,7 @@

 #pragma once

+#include <uORB/topics/battery_info.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/PublicationMulti.hpp>

@@ -120,6 +121,10 @@ public:
 			// TODO uORB publication rate limiting
 			_battery_status_pub.publish(bat_status);

+			_battery_info.timestamp = bat_status.timestamp;
+			_battery_info.id = bat_status.id;
+			_battery_info_pub.publish(_battery_info);
+
 		} else if (receive.metadata.port_id == _status_sub._canard_sub.port_id) {
 			reg_udral_service_battery_Status_0_2 bat {};
 			size_t bat_size_in_bytes = receive.payload_size;
@@ -163,7 +168,7 @@ public:

 			bat_status.capacity = parameters.design_capacity.coulomb / 3.6f; // Coulomb -> mAh
 			bat_status.cycle_count = parameters.cycle_count;
-			bat_status.serial_number = parameters.unique_id & 0xFFFF;
+			snprintf(_battery_info.serial_number, sizeof(_battery_info.serial_number), "%" PRIu64, parameters.unique_id);
 			bat_status.state_of_health = parameters.state_of_health_pct;
 			bat_status.max_error = 1; // UAVCAN didn't spec'ed this, but we're optimistic
 			bat_status.id = 0; //TODO instancing
@@ -174,6 +179,7 @@ public:
 private:
 	float _nominal_voltage = NAN;

+	uORB::PublicationMulti<battery_info_s> _battery_info_pub{ORB_ID(battery_info)};
 	uORB::PublicationMulti<battery_status_s> _battery_status_pub{ORB_ID(battery_status)};

 	SubjectSubscription _status_sub;
@@ -182,5 +188,6 @@ private:
 	const char *_status_name = "battery_status";
 	const char *_parameters_name = "battery_parameters";

+	battery_info_s _battery_info{};
 	battery_status_s bat_status {0};
 };
@@ -240,14 +240,14 @@ int AUAV::read_calibration_eeprom(const uint8_t eeprom_address, uint16_t &data)
 void AUAV::process_calib_data_raw(const calib_data_raw_t calib_data_raw)
 {
 	/* Conversion of calib data as described in the datasheet */
-	_calib_data.a = (float)((calib_data_raw.a_hw << 16) | calib_data_raw.a_lw) / 0x7FFFFFFF;
-	_calib_data.b = (float)((calib_data_raw.b_hw << 16) | calib_data_raw.b_lw) / 0x7FFFFFFF;
-	_calib_data.c = (float)((calib_data_raw.c_hw << 16) | calib_data_raw.c_lw) / 0x7FFFFFFF;
-	_calib_data.d = (float)((calib_data_raw.d_hw << 16) | calib_data_raw.d_lw) / 0x7FFFFFFF;
+	_calib_data.a = (float)((int32_t)((calib_data_raw.a_hw << 16) | calib_data_raw.a_lw)) / 0x7FFFFFFF;
+	_calib_data.b = (float)((int32_t)((calib_data_raw.b_hw << 16) | calib_data_raw.b_lw)) / 0x7FFFFFFF;
+	_calib_data.c = (float)((int32_t)((calib_data_raw.c_hw << 16) | calib_data_raw.c_lw)) / 0x7FFFFFFF;
+	_calib_data.d = (float)((int32_t)((calib_data_raw.d_hw << 16) | calib_data_raw.d_lw)) / 0x7FFFFFFF;

-	_calib_data.tc50h = (float)(calib_data_raw.tc50 >> 8) / 0x7F;
-	_calib_data.tc50l = (float)(calib_data_raw.tc50 & 0xFF) / 0x7F;
-	_calib_data.es = (float)(calib_data_raw.es & 0xFF) / 0x7F;
+	_calib_data.tc50h = (float)((int8_t)(calib_data_raw.tc50 >> 8)) / 0x7F;
+	_calib_data.tc50l = (float)((int8_t)(calib_data_raw.tc50 & 0xFF)) / 0x7F;
+	_calib_data.es = (float)((int8_t)(calib_data_raw.es & 0xFF)) / 0x7F;
 }

 float AUAV::correct_pressure(const uint32_t pressure_raw, const uint32_t temperature_raw) const
@@ -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);
@@ -51,6 +51,7 @@ DShotTelemetry::~DShotTelemetry()

 int DShotTelemetry::init(const char *uart_device, bool swap_rxtx)
 {
+	int ret = OK;
 	deinit();
 	_uart_fd = ::open(uart_device, O_RDONLY | O_NOCTTY);

@@ -59,23 +60,31 @@ int DShotTelemetry::init(const char *uart_device, bool swap_rxtx)
 		return -errno;
 	}

+	ret = setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE);
+
+	if (ret) {
+		PX4_ERR("failed to set baurate: %s err: %d", uart_device, ret);
+		return ret;
+	}
+
 	if (swap_rxtx) {
 		// Swap RX/TX pins if the device supports it
-		int rv = ioctl(_uart_fd, TIOCSSWAP, SER_SWAP_ENABLED);
+		ret = ioctl(_uart_fd, TIOCSSWAP, SER_SWAP_ENABLED);

 		// For other devices we can still place RX on TX pin via half-duplex single-wire mode
-		if (rv) { rv = ioctl(_uart_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); }
+		if (ret) { ret = ioctl(_uart_fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); }

-		if (rv) {
-			PX4_ERR("failed to swap rx/tx pins: %s err: %d", uart_device, rv);
-			return rv;
+		if (ret) {
+			PX4_ERR("failed to swap rx/tx pins: %s err: %d", uart_device, ret);
+			return ret;
 		}
 	}

 	_num_timeouts = 0;
 	_num_successful_responses = 0;
 	_current_motor_index_request = -1;
-	return setBaudrate(DSHOT_TELEMETRY_UART_BAUDRATE);
+
+	return ret;
 }

 void DShotTelemetry::deinit()
@@ -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;
 };

@@ -12,7 +12,6 @@ menu "Magnetometer"
        select DRIVERS_MAGNETOMETER_LIS3MDL
        select DRIVERS_MAGNETOMETER_LSM303AGR
        select DRIVERS_MAGNETOMETER_RM3100
-        select DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L
        select DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA
        select DRIVERS_MAGNETOMETER_ST_IIS2MDC
        ---help---
@@ -34,6 +34,8 @@
 #include "BMM350.hpp"
 using namespace time_literals;

+#define MAX(a,b)          (a > b ? a : b)
+
 BMM350::BMM350(const I2CSPIDriverConfig &config) :
 	I2C(config),
 	I2CSPIDriver(config),
@@ -318,7 +320,7 @@ void BMM350::RunImpl()
 				_state = STATE::RESET;
 			}

-			ScheduleDelayed(OdrToUs(_mag_odr_mode));
+			ScheduleDelayed(MAX(6000_us, OdrToUs(_mag_odr_mode)));
 			break;
 		}

@@ -191,7 +191,6 @@ void Batmon::RunImpl()
 	if (ret == PX4_OK) {
 		new_report.capacity = _batt_capacity;
 		new_report.cycle_count = _cycle_count;
-		new_report.serial_number = _serial_number;
 		new_report.max_cell_voltage_delta = _max_cell_voltage_delta;
 		new_report.cell_count = _cell_count;
 		new_report.state_of_health = _state_of_health;
@@ -220,6 +219,12 @@ void Batmon::RunImpl()
 		orb_publish_auto(ORB_ID(battery_status), &_batt_topic, &new_report, &instance);

 		_last_report = new_report;
+
+		battery_info_s battery_info{};
+		battery_info.timestamp = new_report.timestamp;
+		battery_info.id = new_report.id;
+		snprintf(battery_info.serial_number, sizeof(battery_info.serial_number), "%" PRIu16, _serial_number);
+		orb_publish_auto(ORB_ID(battery_info), &_battery_info_topic, &battery_info, &instance);
 	}
 }

@@ -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;
 		}
@@ -125,7 +125,6 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 	// _battery_status[instance].cycle_count = msg.;
 	_battery_status[instance].time_remaining_s = NAN;
 	// _battery_status[instance].average_time_to_empty = msg.;
-	_battery_status[instance].serial_number = msg.model_instance_id;
 	_battery_status[instance].id = msg.getSrcNodeID().get();

 	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
@@ -143,8 +142,14 @@ UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::
 	determineWarning(_battery_status[instance].remaining);
 	_battery_status[instance].warning = _warning;

+	_battery_info[instance].timestamp = _battery_status[instance].timestamp;
+	_battery_info[instance].id = _battery_status[instance].id;
+	snprintf(_battery_info[instance].serial_number, sizeof(_battery_info[instance].serial_number), "%" PRIu32,
+		 msg.model_instance_id);
+
 	if (_batt_update_mod[instance] == BatteryDataType::Raw) {
 		publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
+		_battery_info_pub[instance].publish(_battery_info[instance]);
 	}
 }

@@ -238,8 +243,13 @@ UavcanBatteryBridge::filterData(const uavcan::ReceivedDataStructure<uavcan::equi
 	/* Override data that is expected to arrive from UAVCAN msg*/
 	_battery_status[instance] = _battery[instance]->getBatteryStatus();
 	_battery_status[instance].temperature = msg.temperature + atmosphere::kAbsoluteNullCelsius; // Kelvin to Celsius
-	_battery_status[instance].serial_number = msg.model_instance_id;
 	_battery_status[instance].id = msg.getSrcNodeID().get(); // overwrite zeroed index from _battery

 	publish(msg.getSrcNodeID().get(), &_battery_status[instance]);
+
+	_battery_info[instance].timestamp = _battery_status[instance].timestamp;
+	_battery_info[instance].id = _battery_status[instance].id;
+	snprintf(_battery_info[instance].serial_number, sizeof(_battery_info[instance].serial_number), "%" PRIu32,
+		 msg.model_instance_id);
+	_battery_info_pub[instance].publish(_battery_info[instance]);
 }
@@ -38,6 +38,7 @@
 #pragma once

 #include "sensor_bridge.hpp"
+#include <uORB/topics/battery_info.h>
 #include <uORB/topics/battery_status.h>
 #include <uavcan/equipment/power/BatteryInfo.hpp>
 #include <ardupilot/equipment/power/BatteryInfoAux.hpp>
@@ -95,6 +96,11 @@ private:
 	float _discharged_mah_loop = 0.f;
 	uint8_t _warning;
 	hrt_abstime _last_timestamp;
+
+	// Separate battery info publication because UavcanSensorBridgeBase only supports publishing one topic
+	uORB::PublicationMulti<battery_info_s> _battery_info_pub[battery_status_s::MAX_INSTANCES] {ORB_ID(battery_info), ORB_ID(battery_info), ORB_ID(battery_info), ORB_ID(battery_info)};
+
+	battery_info_s _battery_info[battery_status_s::MAX_INSTANCES] {};
 	battery_status_s _battery_status[battery_status_s::MAX_INSTANCES] {};
 	BatteryDataType _batt_update_mod[battery_status_s::MAX_INSTANCES] {};

@@ -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,
@@ -45,6 +45,7 @@

 #include <px4_platform_common/i2c_spi_buses.h>
 #include <lib/drivers/smbus/SMBus.hpp>
+#include <uORB/topics/battery_info.h>
 #include <uORB/topics/battery_status.h>
 #include <px4_platform_common/param.h>
 #include <lib/atmosphere/atmosphere.h>
@@ -66,7 +67,7 @@ public:

 	friend SMBus;

-	int populate_smbus_data(battery_status_s &msg);
+	int populate_smbus_data(battery_status_s &msg, battery_info_s &battery_info);

 	virtual void RunImpl(); // Can be overridden by derived implimentation

@@ -136,7 +137,7 @@ protected:

 	perf_counter_t _cycle{perf_alloc(PC_ELAPSED, "batmon_cycle")}; // TODO

-	/** @param _batt_topic uORB battery topic. */
+	orb_advert_t _battery_info_topic{nullptr};
 	orb_advert_t _batt_topic{nullptr};

 	/** @param _cell_count Number of series cell (retrieved from cell_count PX4 params) */
@@ -173,8 +174,10 @@ SMBUS_SBS_BaseClass<T>::SMBUS_SBS_BaseClass(const I2CSPIDriverConfig &config, SM
 	I2CSPIDriver<T>(config),
 	_interface(interface)
 {
+	battery_info_s battery_info{};
 	battery_status_s new_report = {};
 	int SBS_instance_number = 0;
+	_battery_info_topic = orb_advertise_multi(ORB_ID(battery_info), &battery_info, &SBS_instance_number);
 	_batt_topic = orb_advertise_multi(ORB_ID(battery_status), &new_report, &SBS_instance_number);
 	_interface->init();
 }
@@ -251,7 +254,7 @@ int SMBUS_SBS_BaseClass<T>::get_startup_info()
 }

 template<class T>
-int SMBUS_SBS_BaseClass<T>::populate_smbus_data(battery_status_s &data)
+int SMBUS_SBS_BaseClass<T>::populate_smbus_data(battery_status_s &data, battery_info_s &battery_info)
 {

 	// Temporary variable for storing SMBUS reads.
@@ -285,7 +288,7 @@ int SMBUS_SBS_BaseClass<T>::populate_smbus_data(battery_status_s &data)

 	// Read serial number.
 	ret |= _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, result);
-	data.serial_number = result;
+	snprintf(battery_info.serial_number, sizeof(battery_info.serial_number), "%" PRIu16, result);

 	// Read battery temperature and covert to Celsius.
 	ret |= _interface->read_word(BATT_SMBUS_TEMP, result);
@@ -312,12 +315,17 @@ void SMBUS_SBS_BaseClass<T>::RunImpl()

 	new_report.connected = true;

-	int ret = populate_smbus_data(new_report);
+	battery_info_s battery_info{};
+	battery_info.timestamp = now;
+	battery_info.id = new_report.id;
+
+	int ret = populate_smbus_data(new_report, battery_info);

 	new_report.cell_count = _cell_count;

 	// Only publish if no errors.
 	if (!ret) {
 		orb_publish(ORB_ID(battery_status), _batt_topic, &new_report);
+		orb_publish(ORB_ID(battery_info), _battery_info_topic, &battery_info);
 	}
 }
@@ -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
@@ -184,7 +184,6 @@ class SourceParser(object):
    re_cut_type_specifier = re.compile(r'[a-z]+$')
    re_is_a_number = re.compile(r'^-?[0-9\.]')
    re_remove_dots = re.compile(r'\.+$')
-    re_remove_carriage_return = re.compile('\n+')

    valid_tags = set(["group", "board", "min", "max", "unit", "decimal", "increment", "reboot_required", "value", "boolean", "bit", "category", "volatile"])

@@ -311,7 +310,6 @@ class SourceParser(object):
                                raise Exception('short description too long (150 max, is {:}, parameter: {:})'.format(len(short_desc), name))
                            param.SetField("short_desc", self.re_remove_dots.sub('', short_desc))
                        if long_desc is not None:
-                            long_desc = self.re_remove_carriage_return.sub(' ', long_desc)
                            param.SetField("long_desc", long_desc)
                        for tag in tags:
                            if tag == "group":
@@ -161,7 +161,8 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, const
 }

 float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate, const float yaw_rate_setpoint,
-		  const float vehicle_yaw_rate, const float max_thr_yaw_r, const float max_yaw_accel, const float max_yaw_decel,
+		  const float vehicle_yaw_rate, const float max_thr_speed, const float yaw_rate_corr, const float max_yaw_accel,
+		  const float max_yaw_decel,
 		  const float wheel_track, const float dt)
 {
 	// Apply acceleration and deceleration limit
@@ -194,11 +195,11 @@ float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate
 	// Transform yaw rate into speed difference
 	float speed_diff_normalized{0.f};

-	if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward
-		const float speed_diff = adjusted_yaw_rate_setpoint.getState() * wheel_track /
-					 2.f;
-		speed_diff_normalized = math::interpolate<float>(speed_diff, -max_thr_yaw_r,
-					max_thr_yaw_r, -1.f, 1.f);
+	if (wheel_track > FLT_EPSILON && max_thr_speed > FLT_EPSILON) { // Feedforward
+		const float speed_diff = (adjusted_yaw_rate_setpoint.getState() * wheel_track /
+					  2.f) * yaw_rate_corr;
+		speed_diff_normalized = math::interpolate<float>(speed_diff, -max_thr_speed,
+					max_thr_speed, -1.f, 1.f);
 	}

 	// Feedback control
@@ -100,7 +100,8 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, float
 * @param pid_yaw_rate Yaw rate PID (Updated by this function).
 * @param yaw_rate_setpoint Yaw rate setpoint [rad/s].
 * @param vehicle_yaw_rate Measured vehicle yaw rate [rad/s].
- * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s].
+ * @param max_thr_speed Speed at maximum throttle [m/s].
+ * @param yaw_rate_corr Yaw rate correction factor to convert yaw rate to speed difference.
 * @param max_yaw_accel Maximum allowed yaw acceleration [rad/s^2].
 * @param max_yaw_decel Maximum allowed yaw deceleration [rad/s^2].
 * @param wheel_track Distance from the center of the right wheel to the center of the left wheel [m].
@@ -108,7 +109,8 @@ float speedControl(SlewRate<float> &speed_with_rate_limit, PID &pid_speed, float
 * @return Normalized speed difference setpoint [-1, 1].
 */
 float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate,
-		  float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, float max_yaw_decel,
+		  float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_speed, float yaw_rate_corr, float max_yaw_accel,
+		  float max_yaw_decel,
 		  float wheel_track, float dt);

 /**
@@ -131,6 +131,22 @@ PARAM_DEFINE_FLOAT(RO_YAW_ACCEL_LIM, -1.f);
 */
 PARAM_DEFINE_FLOAT(RO_YAW_DECEL_LIM, -1.f);

+/**
+ * Yaw rate correction factor
+ *
+ * Multiplicative correction factor for the feedforward mapping of the yaw rate controller.
+ * Increase this value (x > 1) if the measured yaw rate is lower than the setpoint, decrease (0 < x < 1) otherwise.
+ * Note: Tuning this is particularly useful for skid-steered rovers, or rovers with misaligned wheels/steering axes
+ * that cause a lot of friction when turning.
+ *
+ * @min 0.01
+ * @max 10000
+ * @increment 0.01
+ * @decimal 2
+ * @group Rover Rate Control
+ */
+PARAM_DEFINE_FLOAT(RO_YAW_RATE_CORR, 1.f);
+
 /**
 * Proportional gain for closed loop yaw controller
 *
@@ -253,3 +269,21 @@ PARAM_DEFINE_FLOAT(RO_JERK_LIM, -1.f);
 * @group Rover Velocity Control
 */
 PARAM_DEFINE_FLOAT(RO_SPEED_TH, 0.1f);
+
+/**
+ * Tuning parameter for the speed reduction based on the course error
+ *
+ * Reduced_speed = RO_MAX_THR_SPEED * (1 - normalized_course_error * RO_SPEED_RED)
+ * The normalized course error is the angle between the current course and the bearing setpoint
+ * interpolated from [0, 180] -> [0, 1].
+ * Higher value -> More speed reduction.
+ * Note: This is also used to calculate the speed at which the vehicle arrives at a waypoint in auto modes.
+ * Set to -1 to disable bearing error based speed reduction.
+ *
+ * @min -1
+ * @max 100
+ * @increment 0.01
+ * @decimal 2
+ * @group Rover Velocity Control
+ */
+PARAM_DEFINE_FLOAT(RO_SPEED_RED, -1.f);
@@ -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();
@@ -2110,6 +2110,7 @@ void Commander::landDetectorUpdate()
 				events::send(events::ID("commander_takeoff_detected"), events::Log::Info, "Takeoff detected");
 				_vehicle_status.takeoff_time = hrt_absolute_time();
 				_have_taken_off_since_arming = true;
+				_home_position.setTakeoffTime(_vehicle_status.takeoff_time);
 			}

 			// automatically set or update home position
@@ -39,12 +39,8 @@
 #include <lib/geo/geo.h>
 #include "commander_helper.h"

-using namespace time_literals;
-
-HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags)
-	: _failsafe_flags(failsafe_flags)
-{
-}
+HomePosition::HomePosition(const failsafe_flags_s &failsafe_flags): ModuleParams(nullptr),
+	_failsafe_flags(failsafe_flags) {}

 bool HomePosition::hasMovedFromCurrentHomeLocation()
 {
@@ -308,6 +304,22 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
 	_local_position_sub.update();
 	_global_position_sub.update();

+	if (_vehicle_air_data_sub.updated()) {
+		vehicle_air_data_s baro_data;
+		_vehicle_air_data_sub.copy(&baro_data);
+		const float baro_alt = baro_data.baro_alt_meter;
+
+		if (_last_baro_timestamp != 0) {
+			const float dt = baro_data.timestamp - _last_baro_timestamp;
+			_lpf_baro.update(baro_alt, dt);
+
+		} else {
+			_lpf_baro.reset(baro_alt);
+		}
+
+		_last_baro_timestamp = baro_data.timestamp;
+	}
+
 	if (_vehicle_gps_position_sub.updated()) {
 		sensor_gps_s vehicle_gps_position;
 		_vehicle_gps_position_sub.copy(&vehicle_gps_position);
@@ -319,12 +331,56 @@ void HomePosition::update(bool set_automatically, bool check_if_changed)
 		_gps_epv = vehicle_gps_position.epv;

 		const hrt_abstime now = hrt_absolute_time();
-		const bool time = (now < vehicle_gps_position.timestamp + 1_s);
-		const bool fix = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;
-		const bool eph = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH;
-		const bool epv = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV;
-		const bool evh = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH;
-		_gps_position_for_home_valid = time && fix && eph && epv && evh;
+		const bool time_valid = now < (vehicle_gps_position.timestamp + 1_s);
+		const bool fix_valid = vehicle_gps_position.fix_type >= kHomePositionGPSRequiredFixType;
+		const bool eph_valid = vehicle_gps_position.eph < kHomePositionGPSRequiredEPH;
+		const bool epv_valid = vehicle_gps_position.epv < kHomePositionGPSRequiredEPV;
+		const bool evh_valid = vehicle_gps_position.s_variance_m_s < kHomePositionGPSRequiredEVH;
+
+		_gps_position_for_home_valid = time_valid && fix_valid && eph_valid && epv_valid && evh_valid;
+
+		if (_param_com_home_en.get() && _gps_position_for_home_valid && _last_gps_timestamp != 0 && _last_baro_timestamp != 0
+		    && _takeoff_time != 0 && now < _takeoff_time + kHomePositionCorrectionTimeWindow) {
+
+			const float gps_alt = static_cast<float>(_gps_alt);
+
+			if (!PX4_ISFINITE(_gps_vel_integral)) {
+				_gps_vel_integral = gps_alt; // initialize the gps-vel-integral at same altitude as gps-pos
+				_baro_gps_static_offset = gps_alt - _lpf_baro.getState();
+			}
+
+			_gps_vel_integral += 1e-6f * (vehicle_gps_position.timestamp - _last_gps_timestamp) * (-vehicle_gps_position.vel_d_m_s);
+
+			// correct baro_alt with offset from GPS alt from when the drift integral was initialized
+			const float baro_alt_corrected = _lpf_baro.getState() + _baro_gps_static_offset;
+			const float gps_alt_with_home_offset = gps_alt + _home_altitude_offset_applied;
+
+			// Apply home altitude correction only if the GPS velocity-integrated altitude and baro altitude
+			// are more consistent with each other than either is with the GPS altitude (with home offset).
+			if (fabsf(baro_alt_corrected - _gps_vel_integral) < fabsf(baro_alt_corrected - gps_alt_with_home_offset) &&
+			    fabsf(baro_alt_corrected - _gps_vel_integral) < fabsf(_gps_vel_integral - gps_alt_with_home_offset)) {
+
+				const float offset_new = baro_alt_corrected - gps_alt - _home_altitude_offset_applied;
+
+				if (fabsf(offset_new) > kAltitudeDifferenceThreshold) {
+
+					home_position_s home = _home_position_pub.get();
+					home.alt -= offset_new;
+					home.z += offset_new;
+					home.timestamp = now;
+					home.manual_home = false;
+					home.update_count = _home_position_pub.get().update_count + 1U;
+
+					_home_position_pub.update(home);
+					_home_altitude_offset_applied = baro_alt_corrected - gps_alt; // offset present when home position was last corrected
+				}
+			}
+
+		} else {
+			_gps_vel_integral = NAN;
+		}
+
+		_last_gps_timestamp = vehicle_gps_position.timestamp;
 	}

 	const vehicle_local_position_s &lpos = _local_position_sub.get();
@@ -40,6 +40,11 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/failsafe_flags.h>
+#include <uORB/topics/vehicle_air_data.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <px4_platform_common/module_params.h>
+
+using namespace time_literals;

 static constexpr int kHomePositionGPSRequiredFixType = 2;
 static constexpr float kHomePositionGPSRequiredEPH = 5.f;
@@ -47,8 +52,11 @@ static constexpr float kHomePositionGPSRequiredEPV = 10.f;
 static constexpr float kHomePositionGPSRequiredEVH = 1.f;
 static constexpr float kMinHomePositionChangeEPH = 1.f;
 static constexpr float kMinHomePositionChangeEPV = 1.5f;
+static constexpr float kLpfBaroTimeConst = 5.f;
+static constexpr float kAltitudeDifferenceThreshold = 1.f; // altitude difference after which home position gets updated
+static constexpr uint64_t kHomePositionCorrectionTimeWindow = 120_s;

-class HomePosition
+class HomePosition: public ModuleParams
 {
 public:
 	HomePosition(const failsafe_flags_s &failsafe_flags);
@@ -57,6 +65,7 @@ public:
 	bool setHomePosition(bool force = false);
 	void setInAirHomePosition();
 	bool setManually(double lat, double lon, float alt, float yaw);
+	void setTakeoffTime(uint64_t takeoff_time) { _takeoff_time = takeoff_time; }

 	void update(bool set_automatically, bool check_if_changed);

@@ -76,6 +85,15 @@ private:

 	uORB::SubscriptionData<vehicle_global_position_s>	_global_position_sub{ORB_ID(vehicle_global_position)};
 	uORB::SubscriptionData<vehicle_local_position_s>	_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+
+	uint64_t _last_gps_timestamp{0};
+	uint64_t _last_baro_timestamp{0};
+	AlphaFilter<float> _lpf_baro{kLpfBaroTimeConst};
+	float _gps_vel_integral{NAN};
+	float _home_altitude_offset_applied{0.f};
+	float _baro_gps_static_offset{0.f};
+	uint64_t _takeoff_time{0};

 	uORB::PublicationData<home_position_s>			_home_position_pub{ORB_ID(home_position)};

@@ -88,4 +106,8 @@ private:
 	double							_gps_alt{0};
 	float							_gps_eph{0.f};
 	float							_gps_epv{0.f};
+
+	DEFINE_PARAMETERS(
+		(ParamBool<px4::params::COM_HOME_EN>) _param_com_home_en
+	)
 };
@@ -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
@@ -142,8 +143,9 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f);
 *
 * Set home position automatically if possible.
 *
- * During missions, the home position is locked and will not reset during intermediate landings.
+ * During missions, the latitude/longitude of the home position is locked and will not reset during intermediate landings.
 * It will only update once the mission is complete or landed outside of a mission.
+ * However, the altitude is still being adjusted to correct for GNSS vertical drift in the first 2 minutes after takeoff.
 *
 * @group Commander
 * @reboot_required true
@@ -211,7 +211,7 @@ endif()
 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
-		EKF/aid_sources/range_finder/range_height_control.cpp
+		EKF/aid_sources/range_finder/jake_range_height_control.cpp
 		EKF/aid_sources/range_finder/range_height_fusion.cpp
 		EKF/aid_sources/range_finder/sensor_range_finder.cpp
 	)
@@ -124,7 +124,7 @@ endif()
 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
 		aid_sources/range_finder/range_finder_consistency_check.cpp
-		aid_sources/range_finder/range_height_control.cpp
+		aid_sources/range_finder/jake_range_height_control.cpp
 		aid_sources/range_finder/range_height_fusion.cpp
 		aid_sources/range_finder/sensor_range_finder.cpp
 	)
@@ -58,7 +58,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 	const bool airspeed_timed_out = isTimedOut(_aid_src_airspeed.time_last_fuse, (uint64_t)10e6);
 	const bool sideslip_timed_out = isTimedOut(_aid_src_sideslip.time_last_fuse, (uint64_t)10e6);

-	if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_params.drag_ctrl == 0))) {
+	if (_control_status.flags.fake_pos || (airspeed_timed_out && sideslip_timed_out && (_params.ekf2_drag_ctrl == 0))) {
 		_control_status.flags.wind = false;
 	}

@@ -72,7 +72,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 	if (_control_status.flags.fixed_wing) {
 		if (_control_status.flags.in_air && !_control_status.flags.vehicle_at_rest) {
 			if (!_control_status.flags.fuse_aspd) {
-				_yawEstimator.setTrueAirspeed(_params.EKFGSF_tas_default);
+				_yawEstimator.setTrueAirspeed(_params.ekf2_gsf_tas);
 			}

 		} else {
@@ -82,7 +82,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)

 #endif // CONFIG_EKF2_GNSS

-	if (_params.arsp_thr <= 0.f) {
+	if (_params.ekf2_arsp_thr <= 0.f) {
 		stopAirspeedFusion();
 		return;
 	}
@@ -99,7 +99,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 		const bool continuing_conditions_passing = _control_status.flags.in_air
 				&& !_control_status.flags.fake_pos;

-		const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.arsp_thr;
+		const bool is_airspeed_significant = airspeed_sample.true_airspeed > _params.ekf2_arsp_thr;
 		const bool is_airspeed_consistent = (_aid_src_airspeed.test_ratio > 0.f && _aid_src_airspeed.test_ratio < 1.f);
 		const bool starting_conditions_passing = continuing_conditions_passing
 				&& is_airspeed_significant
@@ -151,7 +151,7 @@ void Ekf::controlAirDataFusion(const imuSample &imu_delayed)
 void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src) const
 {
 	// Variance for true airspeed measurement - (m/sec)^2
-	const float R = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) *
+	const float R = sq(math::constrain(_params.ekf2_eas_noise, 0.5f, 5.0f) *
 			   math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));

 	float innov = 0.f;
@@ -165,7 +165,7 @@ void Ekf::updateAirspeed(const airspeedSample &airspeed_sample, estimator_aid_so
 			      R,                                       // observation variance
 			      innov,                                   // innovation
 			      innov_var,                               // innovation variance
-			      math::max(_params.tas_innov_gate, 1.f)); // innovation gate
+			      math::max(_params.ekf2_tas_gate, 1.f)); // innovation gate
 }

 void Ekf::fuseAirspeed(const airspeedSample &airspeed_sample, estimator_aid_source1d_s &aid_src)
@@ -243,7 +243,7 @@ void Ekf::resetWindUsingAirspeed(const airspeedSample &airspeed_sample)
 	constexpr float sideslip_var = sq(math::radians(15.0f));

 	const float euler_yaw = getEulerYaw(_R_to_earth);
-	const float airspeed_var = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f)
+	const float airspeed_var = sq(math::constrain(_params.ekf2_eas_noise, 0.5f, 5.0f)
 				      * math::constrain(airspeed_sample.eas2tas, 0.9f, 10.0f));

 	matrix::SquareMatrix<float, State::wind_vel.dof> P_wind;
@@ -57,7 +57,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 		const float measurement = baro_sample.hgt;
 #endif

-		const float measurement_var = sq(_params.baro_noise);
+		const float measurement_var = sq(_params.ekf2_baro_noise);

 		const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);

@@ -83,14 +83,14 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 						baro_sample.time_us,
 						-(measurement - bias_est.getBias()),      // observation
 						measurement_var + bias_est.getBiasVar(),  // observation variance
-						math::max(_params.baro_innov_gate, 1.f)); // innovation gate
+						math::max(_params.ekf2_baro_gate, 1.f)); // innovation gate

 		// Compensate for positive static pressure transients (negative vertical position innovations)
 		// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
-		if (_control_status.flags.gnd_effect && (_params.gnd_effect_deadzone > 0.f)) {
+		if (_control_status.flags.gnd_effect && (_params.ekf2_gnd_eff_dz > 0.f)) {

 			const float deadzone_start = 0.0f;
-			const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
+			const float deadzone_end = deadzone_start + _params.ekf2_gnd_eff_dz;

 			if (aid_src.innovation < -deadzone_start) {
 				if (aid_src.innovation <= -deadzone_end) {
@@ -111,7 +111,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample)
 		}

 		// determine if we should use height aiding
-		const bool continuing_conditions_passing = (_params.baro_ctrl == 1)
+		const bool continuing_conditions_passing = (_params.ekf2_baro_ctrl == 1)
 				&& measurement_valid
 				&& (_baro_counter > _obs_buffer_length)
 				&& !_control_status.flags.baro_fault;
@@ -222,11 +222,11 @@ float Ekf::compensateBaroForDynamicPressure(const imuSample &imu_sample, const f
 		const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth);

 		const Vector3f K_pstatic_coef(
-			airspeed_body(0) >= 0.f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn,
-			airspeed_body(1) >= 0.f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn,
-			_params.static_pressure_coef_z);
+			airspeed_body(0) >= 0.f ? _params.ekf2_pcoef_xp : _params.ekf2_pcoef_xn,
+			airspeed_body(1) >= 0.f ? _params.ekf2_pcoef_yp : _params.ekf2_pcoef_yn,
+			_params.ekf2_pcoef_z);

-		const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed));
+		const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.ekf2_aspd_max));

 		const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef));

@@ -45,7 +45,7 @@

 void Ekf::controlDragFusion(const imuSample &imu_delayed)
 {
-	if ((_params.drag_ctrl > 0) && _drag_buffer) {
+	if ((_params.ekf2_drag_ctrl > 0) && _drag_buffer) {

 		if (!_control_status.flags.wind && !_control_status.flags.fake_pos && _control_status.flags.in_air) {
 			_control_status.flags.wind = true;
@@ -65,17 +65,19 @@ void Ekf::controlDragFusion(const imuSample &imu_delayed)

 void Ekf::fuseDrag(const dragSample &drag_sample)
 {
-	const float R_ACC = fmaxf(_params.drag_noise, 0.5f); // observation noise variance in specific force drag (m/sec**2)**2
+	const float R_ACC = fmaxf(_params.ekf2_drag_noise,
+				  0.5f); // observation noise variance in specific force drag (m/sec**2)**2
 	const float rho = fmaxf(_air_density, 0.1f); // air density (kg/m**3)

 	// correct rotor momentum drag for increase in required rotor mass flow with altitude
 	// obtained from momentum disc theory
-	const float mcoef_corrrected = fmaxf(_params.mcoef * sqrtf(rho / atmosphere::kAirDensitySeaLevelStandardAtmos), 0.f);
+	const float mcoef_corrrected = fmaxf(_params.ekf2_mcoef * sqrtf(rho / atmosphere::kAirDensitySeaLevelStandardAtmos),
+					     0.f);

 	// drag model parameters
-	const bool using_bcoef_x = _params.bcoef_x > 1.0f;
-	const bool using_bcoef_y = _params.bcoef_y > 1.0f;
-	const bool using_mcoef   = _params.mcoef   > 0.001f;
+	const bool using_bcoef_x = _params.ekf2_bcoef_x > 1.0f;
+	const bool using_bcoef_y = _params.ekf2_bcoef_y > 1.0f;
+	const bool using_mcoef   = _params.ekf2_mcoef   > 0.001f;

 	if (!using_bcoef_x && !using_bcoef_y && !using_mcoef) {
 		return;
@@ -92,11 +94,11 @@ void Ekf::fuseDrag(const dragSample &drag_sample)
 	Vector2f bcoef_inv{0.f, 0.f};

 	if (using_bcoef_x) {
-		bcoef_inv(0) = 1.f / _params.bcoef_x;
+		bcoef_inv(0) = 1.f / _params.ekf2_bcoef_x;
 	}

 	if (using_bcoef_y) {
-		bcoef_inv(1) = 1.f / _params.bcoef_y;
+		bcoef_inv(1) = 1.f / _params.ekf2_bcoef_y;
 	}

 	if (using_bcoef_x && using_bcoef_y) {
@@ -52,14 +52,14 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)
 		bool ev_reset = (ev_sample.reset_counter != _ev_sample_prev.reset_counter);

 		// determine if we should use the horizontal position observations
-		bool quality_sufficient = (_params.ev_quality_minimum <= 0) || (ev_sample.quality >= _params.ev_quality_minimum);
+		bool quality_sufficient = (_params.ekf2_ev_qmin <= 0) || (ev_sample.quality >= _params.ekf2_ev_qmin);

 		const bool starting_conditions_passing = quality_sufficient
 				&& ((ev_sample.time_us - _ev_sample_prev.time_us) < EV_MAX_INTERVAL)
-				&& ((_params.ev_quality_minimum <= 0)
-				    || (_ev_sample_prev.quality >= _params.ev_quality_minimum)) // previous quality sufficient
-				&& ((_params.ev_quality_minimum <= 0)
-				    || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
+				&& ((_params.ekf2_ev_qmin <= 0)
+				    || (_ev_sample_prev.quality >= _params.ekf2_ev_qmin)) // previous quality sufficient
+				&& ((_params.ekf2_ev_qmin <= 0)
+				    || (_ext_vision_buffer->get_newest().quality >= _params.ekf2_ev_qmin)) // newest quality sufficient
 				&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);

 		updateEvAttitudeErrorFilter(ev_sample, ev_reset);
@@ -68,19 +68,19 @@ void Ekf::controlExternalVisionFusion(const imuSample &imu_sample)

 		switch (ev_sample.vel_frame) {
 		case VelocityFrame::BODY_FRAME_FRD: {
-				EvVelBodyFrameFrd ev_vel_body(*this, ev_sample, _params.ev_vel_noise, imu_sample);
+				EvVelBodyFrameFrd ev_vel_body(*this, ev_sample, _params.ekf2_evv_noise, imu_sample);
 				controlEvVelFusion(ev_vel_body, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
 				break;
 			}

 		case VelocityFrame::LOCAL_FRAME_NED: {
-				EvVelLocalFrameNed ev_vel_ned(*this, ev_sample, _params.ev_vel_noise, imu_sample);
+				EvVelLocalFrameNed ev_vel_ned(*this, ev_sample, _params.ekf2_evv_noise, imu_sample);
 				controlEvVelFusion(ev_vel_ned, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
 				break;
 			}

 		case VelocityFrame::LOCAL_FRAME_FRD: {
-				EvVelLocalFrameFrd ev_vel_frd(*this, ev_sample, _params.ev_vel_noise, imu_sample);
+				EvVelLocalFrameFrd ev_vel_frd(*this, ev_sample, _params.ekf2_evv_noise, imu_sample);
 				controlEvVelFusion(ev_vel_frd, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
 				break;
 			}
@@ -75,13 +75,13 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 	}

 	const float measurement = pos(2) - pos_offset_earth(2);
-	float measurement_var = math::max(pos_cov(2, 2), sq(_params.ev_pos_noise), sq(0.01f));
+	float measurement_var = math::max(pos_cov(2, 2), sq(_params.ekf2_evp_noise), sq(0.01f));

 #if defined(CONFIG_EKF2_GNSS)

 	// increase minimum variance if GPS active
 	if (_control_status.flags.gps_hgt) {
-		measurement_var = math::max(measurement_var, sq(_params.gps_pos_noise));
+		measurement_var = math::max(measurement_var, sq(_params.ekf2_gps_p_noise));
 	}

 #endif // CONFIG_EKF2_GNSS
@@ -92,7 +92,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 					ev_sample.time_us,
 					measurement - bias_est.getBias(),           // observation
 					measurement_var + bias_est.getBiasVar(),    // observation variance
-					math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate
+					math::max(_params.ekf2_evp_gate, 1.f)); // innovation gate

 	// update the bias estimator before updating the main filter but after
 	// using its current state to compute the vertical position innovation
@@ -102,7 +102,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
 		bias_est.fuseBias(measurement + _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2));
 	}

-	const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
+	const bool continuing_conditions_passing = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::VPOS))
 			&& measurement_valid;

 	const bool starting_conditions_passing = common_starting_conditions_passing
@@ -49,7 +49,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);

 	// determine if we should use EV position aiding
-	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
+	bool continuing_conditions_passing = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
 					     && _control_status.flags.tilt_align
 					     && PX4_ISFINITE(ev_sample.pos(0))
 					     && PX4_ISFINITE(ev_sample.pos(1));
@@ -131,7 +131,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 	// increase minimum variance if GNSS is active (position reference)
 	if (_control_status.flags.gnss_pos) {
 		for (int i = 0; i < 2; i++) {
-			pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.gps_pos_noise));
+			pos_cov(i, i) = math::max(pos_cov(i, i), sq(_params.ekf2_gps_p_noise));
 		}
 	}

@@ -142,8 +142,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 	const Vector2f measurement{pos(0), pos(1)};

 	const Vector2f measurement_var{
-		math::max(pos_cov(0, 0), sq(_params.ev_pos_noise), sq(0.01f)),
-		math::max(pos_cov(1, 1), sq(_params.ev_pos_noise), sq(0.01f))
+		math::max(pos_cov(0, 0), sq(_params.ekf2_evp_noise), sq(0.01f)),
+		math::max(pos_cov(1, 1), sq(_params.ekf2_evp_noise), sq(0.01f))
 	};

 	const bool measurement_valid = measurement.isAllFinite() && measurement_var.isAllFinite();
@@ -169,7 +169,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
 			      pos_obs_var,                                            // observation variance
 			      position_estimate - position,                           // innovation
 			      Vector2f(getStateVariance<State::pos>()) + pos_obs_var, // innovation variance
-			      math::max(_params.ev_pos_innov_gate, 1.f));             // innovation gate
+			      math::max(_params.ekf2_evp_gate, 1.f));             // innovation gate

 	// update the bias estimator before updating the main filter but after
 	// using its current state to compute the vertical position innovation
@@ -45,10 +45,10 @@
 class ExternalVisionVel
 {
 public:
-	ExternalVisionVel(Ekf &ekf_instance, const extVisionSample &vision_sample, const float ev_vel_noise,
+	ExternalVisionVel(Ekf &ekf_instance, const extVisionSample &vision_sample, const float ekf2_evv_noise,
 			  const imuSample &imu_sample) : _ekf(ekf_instance), _sample(vision_sample)
 	{
-		_min_variance = sq(ev_vel_noise);
+		_min_variance = sq(ekf2_evv_noise);
 		const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _ekf._state.gyro_bias;
 		Vector3f position_offset_body = _ekf._params.ev_pos_body - _ekf._params.imu_pos_body;
 		_velocity_offset_body = angular_velocity % position_offset_body;
@@ -93,9 +93,9 @@ public:
 class EvVelBodyFrameFrd : public ExternalVisionVel
 {
 public:
-	EvVelBodyFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise,
+	EvVelBodyFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ekf2_evv_noise,
 			  const imuSample &imu_sample) :
-		ExternalVisionVel(ekf_instance, vision_sample, ev_vel_noise, imu_sample)
+		ExternalVisionVel(ekf_instance, vision_sample, ekf2_evv_noise, imu_sample)
 	{
 		_measurement = _sample.vel - _velocity_offset_body;
 		_measurement_var = _sample.velocity_var;
@@ -132,9 +132,9 @@ public:
 class EvVelLocalFrameNed : public ExternalVisionVel
 {
 public:
-	EvVelLocalFrameNed(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise,
+	EvVelLocalFrameNed(Ekf &ekf_instance, extVisionSample &vision_sample, const float ekf2_evv_noise,
 			   const imuSample &imu_sample) :
-		ExternalVisionVel(ekf_instance, vision_sample, ev_vel_noise, imu_sample)
+		ExternalVisionVel(ekf_instance, vision_sample, ekf2_evv_noise, imu_sample)
 	{
 		const Vector3f velocity_offset_earth = _ekf._R_to_earth * _velocity_offset_body;

@@ -149,9 +149,9 @@ public:
 class EvVelLocalFrameFrd : public ExternalVisionVel
 {
 public:
-	EvVelLocalFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ev_vel_noise,
+	EvVelLocalFrameFrd(Ekf &ekf_instance, extVisionSample &vision_sample, const float ekf2_evv_noise,
 			   const imuSample &imu_sample) :
-		ExternalVisionVel(ekf_instance,	vision_sample, ev_vel_noise, imu_sample)
+		ExternalVisionVel(ekf_instance,	vision_sample, ekf2_evv_noise, imu_sample)
 	{
 		const Vector3f velocity_offset_earth = _ekf._R_to_earth * _velocity_offset_body;

@@ -51,15 +51,15 @@ void Ekf::controlEvVelFusion(ExternalVisionVel &ev, const bool common_starting_c
 					   || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);

 	// determine if we should use EV velocity aiding
-	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
+	bool continuing_conditions_passing = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
 					     && _control_status.flags.tilt_align
 					     && ev._sample.vel.isAllFinite()
-					     && !ev._sample.vel.longerThan(_params.velocity_limit);
+					     && !ev._sample.vel.longerThan(_params.ekf2_vel_lim);


 	continuing_conditions_passing &= ev._measurement.isAllFinite() && ev._measurement_var.isAllFinite();

-	float gate = math::max(_params.ev_vel_innov_gate, 1.f);
+	float gate = math::max(_params.ekf2_evv_gate, 1.f);

 	if (_control_status.flags.ev_vel) {
 		if (continuing_conditions_passing) {
@@ -45,7 +45,7 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample
 	static constexpr const char *AID_SRC_NAME = "EV yaw";

 	float obs = getEulerYaw(ev_sample.quat);
-	float obs_var = math::max(ev_sample.orientation_var(2), _params.ev_att_noise, sq(0.01f));
+	float obs_var = math::max(ev_sample.orientation_var(2), _params.ekf2_eva_noise, sq(0.01f));

 	float innov = wrap_pi(getEulerYaw(_R_to_earth) - obs);
 	float innov_var = 0.f;
@@ -59,14 +59,14 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample
 			      obs_var,                                     // observation variance
 			      innov,                                       // innovation
 			      innov_var,                                   // innovation variance
-			      math::max(_params.heading_innov_gate, 1.f)); // innovation gate
+			      math::max(_params.ekf2_hdg_gate, 1.f)); // innovation gate

 	if (ev_reset) {
 		_control_status.flags.ev_yaw_fault = false;
 	}

 	// determine if we should use EV yaw aiding
-	bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
+	bool continuing_conditions_passing = (_params.ekf2_ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
 					     && _control_status.flags.tilt_align
 					     && !_control_status.flags.ev_yaw_fault
 					     && PX4_ISFINITE(aid_src.observation)
@@ -48,7 +48,7 @@ void Ekf::controlFakeHgtFusion()

 	if (fake_hgt_data_ready) {

-		const float obs_var = sq(_params.pos_noaid_noise);
+		const float obs_var = sq(_params.ekf2_noaid_noise);
 		const float innov_gate = 3.f;

 		updateVerticalPositionAidStatus(aid_src, _time_delayed_us, -_last_known_gpos.altitude(), obs_var, innov_gate);
@@ -110,7 +110,7 @@ void Ekf::resetHeightToLastKnown()
 {
 	_information_events.flags.reset_pos_to_last_known = true;
 	ECL_INFO("reset height to last known (%.3f)", (double)_last_known_gpos.altitude());
-	resetAltitudeTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise));
+	resetAltitudeTo(_last_known_gpos.altitude(), sq(_params.ekf2_noaid_noise));
 }

 void Ekf::stopFakeHgtFusion()
@@ -52,7 +52,7 @@ void Ekf::controlFakePosFusion()
 		Vector2f obs_var;

 		if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
-			obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, 1.f));
+			obs_var(0) = obs_var(1) = sq(fmaxf(_params.ekf2_noaid_noise, 1.f));

 		} else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
 			// Accelerate tilt fine alignment by fusing more
@@ -113,29 +113,29 @@ bool GnssChecks::runInitialFixChecks(const gnssSample &gnss)
 	_check_fail_status.flags.fix = (gnss.fix_type < 3);

 	// Check the number of satellites
-	_check_fail_status.flags.nsats = (gnss.nsats < _params.req_nsats);
+	_check_fail_status.flags.nsats = (gnss.nsats < _params.ekf2_req_nsats);

 	// Check the position dilution of precision
-	_check_fail_status.flags.pdop = (gnss.pdop > _params.req_pdop);
+	_check_fail_status.flags.pdop = (gnss.pdop > _params.ekf2_req_pdop);

 	// Check the reported horizontal and vertical position accuracy
-	_check_fail_status.flags.hacc = (gnss.hacc > _params.req_hacc);
-	_check_fail_status.flags.vacc = (gnss.vacc > _params.req_vacc);
+	_check_fail_status.flags.hacc = (gnss.hacc > _params.ekf2_req_eph);
+	_check_fail_status.flags.vacc = (gnss.vacc > _params.ekf2_req_epv);

 	// Check the reported speed accuracy
-	_check_fail_status.flags.sacc = (gnss.sacc > _params.req_sacc);
+	_check_fail_status.flags.sacc = (gnss.sacc > _params.ekf2_req_sacc);

 	_check_fail_status.flags.spoofed = gnss.spoofed;

 	runOnGroundGnssChecks(gnss);

 	// force horizontal speed failure if above the limit
-	if (gnss.vel.xy().longerThan(_params.velocity_limit)) {
+	if (gnss.vel.xy().longerThan(_params.ekf2_vel_lim)) {
 		_check_fail_status.flags.hspeed = true;
 	}

 	// force vertical speed failure if above the limit
-	if (fabsf(gnss.vel(2)) > _params.velocity_limit) {
+	if (fabsf(gnss.vel(2)) > _params.ekf2_vel_lim) {
 		_check_fail_status.flags.vspeed = true;
 	}

@@ -198,7 +198,7 @@ void GnssChecks::runOnGroundGnssChecks(const gnssSample &gnss)
 		}

 		// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
-		const Vector3f vel_limit(_params.req_hdrift, _params.req_hdrift, _params.req_vdrift);
+		const Vector3f vel_limit(_params.ekf2_req_hdrift, _params.ekf2_req_hdrift, _params.ekf2_req_vdrift);
 		Vector3f delta_pos(delta_pos_n, delta_pos_e, (_alt_prev - gnss.alt));

 		// Apply a low pass filter
@@ -209,26 +209,26 @@ void GnssChecks::runOnGroundGnssChecks(const gnssSample &gnss)

 		// hdrift: calculate the horizontal drift speed and fail if too high
 		_horizontal_position_drift_rate_m_s = Vector2f(_lat_lon_alt_deriv_filt.xy()).norm();
-		_check_fail_status.flags.hdrift = (_horizontal_position_drift_rate_m_s > _params.req_hdrift);
+		_check_fail_status.flags.hdrift = (_horizontal_position_drift_rate_m_s > _params.ekf2_req_hdrift);

 		// vdrift: fail if the vertical drift speed is too high
 		_vertical_position_drift_rate_m_s = fabsf(_lat_lon_alt_deriv_filt(2));
-		_check_fail_status.flags.vdrift = (_vertical_position_drift_rate_m_s > _params.req_vdrift);
+		_check_fail_status.flags.vdrift = (_vertical_position_drift_rate_m_s > _params.ekf2_req_vdrift);

 		// hspeed: check the magnitude of the filtered horizontal GNSS velocity
 		const Vector2f vel_ne = matrix::constrain(Vector2f(gnss.vel.xy()),
-					-10.0f * _params.req_hdrift,
-					10.0f * _params.req_hdrift);
+					-10.0f * _params.ekf2_req_hdrift,
+					10.0f * _params.ekf2_req_hdrift);
 		_vel_ne_filt = vel_ne * filter_coef + _vel_ne_filt * (1.0f - filter_coef);
 		_filtered_horizontal_velocity_m_s = _vel_ne_filt.norm();
-		_check_fail_status.flags.hspeed = (_filtered_horizontal_velocity_m_s > _params.req_hdrift);
+		_check_fail_status.flags.hspeed = (_filtered_horizontal_velocity_m_s > _params.ekf2_req_hdrift);

 		// vspeed: check the magnitude of the filtered vertical GNSS velocity
-		const float gnss_vz_limit = 10.f * _params.req_vdrift;
+		const float gnss_vz_limit = 10.f * _params.ekf2_req_vdrift;
 		const float gnss_vz = math::constrain(gnss.vel(2), -gnss_vz_limit, gnss_vz_limit);
 		_vel_d_filt = gnss_vz * filter_coef + _vel_d_filt * (1.f - filter_coef);

-		_check_fail_status.flags.vspeed = (fabsf(_vel_d_filt) > _params.req_vdrift);
+		_check_fail_status.flags.vspeed = (fabsf(_vel_d_filt) > _params.ekf2_req_vdrift);

 	} else {
 		// This is the case where the vehicle is on ground and IMU movement is blocking the drift calculation
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Jacob Dahl	febbb176bd	fix rangefinder params	2025-06-24 11:11:02 -08:00
Jacob Dahl	2b6f09cd0f	reset terrain to rng before reset aid source	2025-06-24 11:11:02 -08:00
Jacob Dahl	1386ec2436	fixy	2025-06-24 11:11:02 -08:00
Jacob Dahl	66770812bf	squashed	2025-06-24 11:11:02 -08:00
Marco Hauswirth	079e0a8c3f	remove terrain checks form baroRef unit-tests, vehicle vertical motion is not defined in test	2025-06-24 11:11:02 -08:00
Marco Hauswirth	96c02a262f	always run consistency-KF but limit state change to vertical velocity. this fixes behavior for jumps in fixed wing	2025-06-24 11:11:02 -08:00
Marco Hauswirth	6bc4b03205	minor cleanup	2025-06-24 11:11:02 -08:00
Marco Hauswirth	fa9bfebd17	only run range validator on regular data	2025-06-24 11:11:02 -08:00
Marco Hauswirth	5e6c36a424	adjust IMU falling detection baro-range unittest	2025-06-24 11:11:01 -08:00
Marco Hauswirth	aa00bdcd5d	also run range consistency check while landed	2025-06-24 11:11:01 -08:00
Marco Hauswirth	7cfea9915b	change consistency to only be valid if running for a second	2025-06-24 11:11:01 -08:00
Marco Hauswirth	564b517225	* clean up method calls * move P-init to ekf-derivation script	2025-06-24 11:11:01 -08:00
Marco Hauswirth	30636495c7	fix uncertainty initialization	2025-06-24 11:11:01 -08:00
Marco Hauswirth	9cd8db9680	clean up range-filter fusion step	2025-06-24 11:11:01 -08:00
Marco Hauswirth	6b16b8f906	reset state to unknown on init	2025-06-24 11:11:01 -08:00
Marco Hauswirth	46157114b7	overhaul approach to work on unit tests and snow/blocking/normal flights	2025-06-24 11:11:01 -08:00
Marco Hauswirth	57dd9eb599	adjust threshold unit tests	2025-06-24 11:11:01 -08:00
Marco Hauswirth	844ab9d423	add symforce, adjust smaller things	2025-06-24 11:11:01 -08:00
Marco Hauswirth	79e60c343a	always update lpf	2025-06-24 11:11:01 -08:00
Marco Hauswirth	d686936979	fix bugs, rejection was too sensitive	2025-06-24 11:11:01 -08:00
Marco Hauswirth	18527d1bc0	fix wrong miniEKF init	2025-06-24 11:11:01 -08:00
Marco Hauswirth	c2a4e61c3f	handle noise while hovering	2025-06-24 11:11:01 -08:00
Marco Hauswirth	e441f3d53c	add mini KF to validate range sensor measurements	2025-06-24 11:11:01 -08:00
Alexander Lerach	160ae487ff	auav: use correct sign during calib data reading	2025-06-24 17:58:53 +02:00
Sergei Grichine	256b329aab	Differential: Update 4011_gz_lawnmower to match recent changes in Rover controls (#25079 )	2025-06-24 09:17:10 +02:00
Jacob Dahl	95119027a9	ekf2: variable to parameter name consistency (#25042 ) Rename various EKF2 variable names to match the PX4 parameter names	2025-06-24 09:15:50 +02:00
Jakub Kákona	9e90fd193f	doc: Improve I2C devices links and explanation (#24701 ) * Improve links URL by swithing from GitHub URL to actual documentation. * Update i2c_general.md Clean up and simplify the explanatory text.	2025-06-23 11:06:37 -08:00
Liu1	832a90e07f	cuav_7-nano：use new sensors (#25098 )	2025-06-23 07:24:28 -08:00
Hamish Willee	2e6fd9dd72	Updates from feedback	2025-06-20 08:27:03 +10:00
Hamish Willee	94dc757363	Prettier and add context	2025-06-20 08:27:03 +10:00
Hamish Willee	5cfa0d548c	Update docs/en/peripherals/dshot.md	2025-06-20 08:27:03 +10:00
Jacob Dahl	fa9f8734d0	update warning, add imxrt targets	2025-06-20 08:27:03 +10:00
Jacob Dahl	3d5cb891a6	docs: bidirectional dshot	2025-06-20 08:27:03 +10:00
Beat Küng	afbaa71bfc	fix mavlink: add mutex for mavlink shell (#25082 ) There was a race condition when closing the shell: - the main thread checks if _mavlink_shell is not nullptr (which is true) - the receiver thread closes the shell, which clears _mavlink_shell - the main thread continues with _mavlink_shell->available()	2025-06-19 13:03:01 -08:00
Hamish Willee	873aa89022	[Docs] Update QGC Bootloader instructions (#25072 ) * [Docs] Update QGC Bootloader instructions * Add release note	2025-06-18 09:02:00 -08:00
Daniel Agar	1b3c6f7fd2	drivers/magnetometer: remove Vtrantech VCM1193L from common magnetometer set	2025-06-18 11:56:13 -04:00
Marco Hauswirth	6604c52c98	Commander: Adjust home position altitude after GNSS altitude correction (#25003 ) Within the first 2min of a flight, check if the integrated GNSS vertical velocity and the baro measurements disagree with the reported GNSS altitude, and in that case update the set home position altitude to cancel out GNSS altitude drift. * prevent re-init when reaching negative altitudes * only allow correction during the first 120 second after takeoff * add dependency to COM_HOME_EN parameter. reset vel-integral for multiple takeoffs	2025-06-18 13:26:45 +02:00
Hamish Willee	ef252481a8	Collision mode only works with mpc_pos_mode that is acceleration based	2025-06-18 16:38:37 +10:00
chfriedrich98	f35b92a487	rover: replace RX_MAX_THR_YAW_R with a correction factor RO_YAW_RATE_CORR	2025-06-18 08:35:48 +02:00
chfriedrich98	3e8f054a1c	ackermann: rename DriveModes to AckermannDriveModes	2025-06-18 08:35:48 +02:00
chfriedrich98	eed966a1c6	rover: reduce speed based on course error	2025-06-18 08:35:48 +02:00
chfriedrich98	5a430f0ba6	rover: streamline rover steering setpoint	2025-06-18 08:35:48 +02:00
Hamish Willee	4a5c00d0e4	EKF2_GPS_POS_X/Y/Z have same long description (#25068 )	2025-06-17 21:07:09 -08:00
PX4BuildBot	778f80ca59	CellularStatus - fix to doc standard	2025-06-18 13:26:45 +10:00
Jacob Dahl	339e0f9691	params: srcparser: don't strip newlines (#25058 )	2025-06-17 12:04:33 -08:00
Peter van der Perk	7687e6e33f	dshot: Telem set baudrate before pin-swapping On IMXRT setbaudrate resets swap state	2025-06-17 14:54:48 +02:00
Peter van der Perk	001efc1c0b	Bosch: BMM350: Fix self-test on high ODR rate We've to wait for atleast 6000us before doing self-test	2025-06-17 12:01:45 +02:00
Matthias Grob	eccfb18b51	battery_status message: remove serial_number which is now in battery_info message	2025-06-17 09:05:22 +02:00
Matthias Grob	1d86ede6c6	msg translations: sort headers alphabetically	2025-06-17 09:05:22 +02:00
Matthias Grob	84ce7d2fc6	cyphal: switch to battery_info.serial_number	2025-06-17 09:05:22 +02:00
Matthias Grob	a18453d632	uavcan battery: switch to battery_info.serial_number	2025-06-17 09:05:22 +02:00
Matthias Grob	3ec684153a	batmon: switch to battery_info.serial_number	2025-06-17 09:05:22 +02:00
Matthias Grob	2237bfa9a9	smbus_sbs: switch to battery_info.serial_number	2025-06-17 09:05:22 +02:00
Matthias Grob	7895976a17	batt_smbus: switch to battery_info.serial_number	2025-06-17 09:05:22 +02:00
Matthias Grob	d7ab21b8d6	Add battery_info message with serial number compatible with UAVCAN, MAVLink and drivers I'm starting the separate battery info message because no char[32] should be published and logged at high rate and we need a separate battery info message for static information as discussed.	2025-06-17 09:05:22 +02: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