gnss: move delay compensation to driver level

This allows to specify a delay per receiver and reduces the amount of
unnecessary parameter duplication in multi-ekf mode

.ci/Jenkinsfile-compile

@@ -77,6 +77,7 @@ pipeline {
                      "matek_h743-mini_default",
                      "matek_h743-slim_default",
                      "matek_h743_default",
+                     "micoair_h743_default",
                      "modalai_fc-v1_default",
                      "modalai_fc-v2_default",
                      "mro_ctrl-zero-classic_default",

.github/workflows/build_all_targets.yml

@@ -1,3 +1,8 @@
+# NOTE: this workflow is now running on Dronecode / PX4 AWS account.
+# - If you want to keep the tests running in GitHub Actions you need to uncomment the "runs-on: ubuntu-latest" lines
+#   and comment the "runs-on: [runs-on,runner=..." lines.
+# - If you would like to duplicate this setup try setting up "RunsOn" on your own AWS account try https://runs-on.com
+
 name: Build all targets
 
 on:
@@ -14,7 +19,8 @@ on:
 jobs:
   group_targets:
     name: Scan for Board Targets
-    runs-on: ubuntu-latest
+    # runs-on: ubuntu-latest
+    runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"]
     outputs:
       matrix: ${{ steps.set-matrix.outputs.matrix }}
       timestamp: ${{ steps.set-timestamp.outputs.timestamp }}
@@ -34,7 +40,8 @@ jobs:
 
   setup:
     name: ${{ matrix.group }}
-    runs-on: ubuntu-latest
+    # runs-on: ubuntu-latest
+    runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"]
     needs: group_targets
     strategy:
       matrix: ${{ fromJson(needs.group_targets.outputs.matrix) }}

.github/workflows/sitl_tests.yml

@@ -1,3 +1,8 @@
+# NOTE: this workflow is now running on Dronecode / PX4 AWS account.
+# - If you want to keep the tests running in GitHub Actions you need to uncomment the "runs-on: ubuntu-latest" lines
+#   and comment the "runs-on: [runs-on,runner=..." lines.
+# - If you would like to duplicate this setup try setting up "RunsOn" on your own AWS account try https://runs-on.com
+
 name: SITL Tests
 
 on:
@@ -10,126 +15,139 @@ on:
 
 jobs:
   build:
-    runs-on: ubuntu-latest
+    name: Testing PX4 ${{ matrix.config.model }}
+    runs-on: [runs-on,runner=16cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"]
+    container:
+      image: px4io/px4-dev-simulation-focal:2021-09-08
+      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
     strategy:
       fail-fast: false
       matrix:
         config:
           - {model: "iris",          latitude:  "59.617693", longitude: "-151.145316", altitude:  "48", build_type: "RelWithDebInfo" } # Alaska
-          # - {model: "standard_vtol", latitude: "-38.071235", longitude:  "145.281220", altitude:  "31", build_type: "AddressSanitizer" } # Australia
           - {model: "tailsitter" ,   latitude:  "29.660316", longitude:  "-82.316658", altitude:  "30", build_type: "RelWithDebInfo" } # Florida
           - {model: "standard_vtol", latitude:  "47.397742", longitude:    "8.545594", altitude: "488", build_type: "Coverage" } # Zurich
 
-    container:
-      image: px4io/px4-dev-simulation-focal:2021-09-08
-      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
     steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
 
-    - name: Download MAVSDK
-      run: wget "https://github.com/mavlink/MAVSDK/releases/download/v$(cat test/mavsdk_tests/MAVSDK_VERSION)/libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
-    - name: Install MAVSDK
-      run: dpkg -i "libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
+      - name: Git Ownership Workaround
+        run: git config --system --add safe.directory '*'
 
-    - name: Prepare ccache timestamp
-      id: ccache_cache_timestamp
-      shell: cmake -P {0}
-      run: |
-        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
-        message("::set-output name=timestamp::${current_date}")
-    - name: ccache cache files
-      uses: actions/cache@v2
-      with:
-        path: ~/.ccache
-        key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
-        restore-keys: sitl_tests-${{matrix.config.build_type}}-ccache-
-    - name: setup ccache
-      run: |
-          mkdir -p ~/.ccache
-          echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf
-          echo "compression = true" >> ~/.ccache/ccache.conf
-          echo "compression_level = 6" >> ~/.ccache/ccache.conf
-          echo "max_size = 100M" >> ~/.ccache/ccache.conf
-          echo "hash_dir = false" >> ~/.ccache/ccache.conf
-          ccache -s
-          ccache -z
+      - id: set-timestamp
+        name: Set timestamp for cache
+        run: echo "::set-output name=timestamp::$(date +"%Y%m%d%H%M%S")"
 
-    - name: check environment
-      env:
-        PX4_HOME_LAT: ${{matrix.config.latitude}}
-        PX4_HOME_LON: ${{matrix.config.longitude}}
-        PX4_HOME_ALT: ${{matrix.config.altitude}}
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: |
-          export
-          ulimit -a
-    - name: Build PX4
-      env:
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: make px4_sitl_default
-    - name: ccache post-run px4/firmware
-      run: ccache -s
-    - name: Build SITL Gazebo
-      env:
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: make px4_sitl_default sitl_gazebo-classic
-    - name: ccache post-run sitl_gazebo-classic
-      run: ccache -s
-    - name: Build MAVSDK tests
-      env:
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-        DONT_RUN: 1
-      run: make px4_sitl_default sitl_gazebo-classic mavsdk_tests
-    - name: ccache post-run mavsdk_tests
-      run: ccache -s
+      - name: Cache Key Config
+        uses: actions/cache@v4
+        with:
+          path: ~/.ccache
+          key: sitl-ccache-${{ steps.set-timestamp.outputs.timestamp }}
+          restore-keys: sitl-ccache-${{ steps.set-timestamp.outputs.timestamp }}
 
-    - name: Core dump settings
-      run: |
-          ulimit -c unlimited
-          echo "`pwd`/%e.core" > /proc/sys/kernel/core_pattern
+      - name: Cache Conf Config
+        run: |
+            mkdir -p ~/.ccache
+            echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf
+            echo "compression = true" >> ~/.ccache/ccache.conf
+            echo "compression_level = 6" >> ~/.ccache/ccache.conf
+            echo "max_size = 120M" >> ~/.ccache/ccache.conf
+            echo "hash_dir = false" >> ~/.ccache/ccache.conf
+            ccache -s
+            ccache -z
 
-    - name: Run SITL tests
-      env:
-        PX4_HOME_LAT: ${{matrix.config.latitude}}
-        PX4_HOME_LON: ${{matrix.config.longitude}}
-        PX4_HOME_ALT: ${{matrix.config.altitude}}
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early --model ${{matrix.config.model}} --upload test/mavsdk_tests/configs/sitl.json --verbose
-      timeout-minutes: 45
+      - name: Build PX4
+        env:
+          PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
+        run: make px4_sitl_default
 
-    - name: Look at core files
-      if: failure()
-      run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit"
-    - name: Upload px4 coredump
-      if: failure()
-      uses: actions/upload-artifact@v2-preview
-      with:
-        name: coredump
-        path: px4.core
+      - name: Cache Post-Run [px4_sitl_default]
+        run: ccache -s
 
-    - name: Upload px4 binary
-      if: failure()
-      uses: actions/upload-artifact@v2-preview
-      with:
-        name: binary
-        path: build/px4_sitl_default/bin/px4
+      - name: Build SITL Gazebo
+        env:
+          PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
+        run: make px4_sitl_default sitl_gazebo-classic
 
-    # Report test coverage
-    - name: Upload coverage
-      if: contains(matrix.config.build_type, 'Coverage')
-      run: |
-          git config --global credential.helper "" # disable the keychain credential helper
-          git config --global --add credential.helper store # enable the local store credential helper
-          echo "https://x-access-token:${{ secrets.ACCESS_TOKEN }}@github.com" >> ~/.git-credentials # add credential
-          git config --global url."https://github.com/".insteadof git@github.com: # credentials add credential
-          mkdir -p coverage
-          lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info
-    - name: Upload coverage information to Codecov
-      if: contains(matrix.config.build_type, 'Coverage')
-      uses: codecov/codecov-action@v1
-      with:
-        token: ${{ secrets.CODECOV_TOKEN }}
-        flags: mavsdk
-        file: coverage/lcov.info
+      - name: Cache Post-Run [sitl_gazebo-classic]
+        run: ccache -s
+
+      - name: Download MAVSDK
+        run: wget "https://github.com/mavlink/MAVSDK/releases/download/v$(cat test/mavsdk_tests/MAVSDK_VERSION)/libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
+
+      - name: Install MAVSDK
+        run: dpkg -i "libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
+
+      - name: Check PX4 Environment Variables
+        env:
+          PX4_HOME_LAT: ${{matrix.config.latitude}}
+          PX4_HOME_LON: ${{matrix.config.longitude}}
+          PX4_HOME_ALT: ${{matrix.config.altitude}}
+          PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
+        run: |
+            export
+            ulimit -a
+
+      - name: Build PX4 / MAVSDK tests
+        env:
+          PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
+          DONT_RUN: 1
+        run: make px4_sitl_default sitl_gazebo-classic mavsdk_tests
+
+      - name: Cache Post-Run [px4_sitl_default sitl_gazebo-classic mavsdk_tests]
+        run: ccache -s
+
+      - name: Core Dump Settings
+        run: |
+            ulimit -c unlimited
+            echo "`pwd`/%e.core" > /proc/sys/kernel/core_pattern
+
+      - name: Run SITL / MAVSDK Tests
+        env:
+          PX4_HOME_LAT: ${{matrix.config.latitude}}
+          PX4_HOME_LON: ${{matrix.config.longitude}}
+          PX4_HOME_ALT: ${{matrix.config.altitude}}
+          PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
+        run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 10 --abort-early --model ${{matrix.config.model}} --upload test/mavsdk_tests/configs/sitl.json --verbose
+        timeout-minutes: 45
+
+      - name: Upload failed logs
+        if: failure()
+        uses: actions/upload-artifact@v4
+        with:
+          name: failed-${{matrix.config.model}}-logs.zip
+          path: |
+            logs/**/**/**/*.log
+            logs/**/**/**/*.ulg
+            build/px4_sitl_default/tmp_mavsdk_tests/rootfs/*.ulg
+
+      - name: Look at Core files
+        if: failure() && ${{ hashFiles('px4.core') != '' }}
+        run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit"
+
+      - name: Upload PX4 coredump
+        if: failure() && ${{ hashFiles('px4.core') != '' }}
+        uses: actions/upload-artifact@v4
+        with:
+          name: coredump
+          path: px4.core
+
+      - name: Setup & Generate Coverage Report
+        if: contains(matrix.config.build_type, 'Coverage')
+        run: |
+            git config --global credential.helper "" # disable the keychain credential helper
+            git config --global --add credential.helper store # enable the local store credential helper
+            echo "https://x-access-token:${{ secrets.ACCESS_TOKEN }}@github.com" >> ~/.git-credentials # add credential
+            git config --global url."https://github.com/".insteadof git@github.com: # credentials add credential
+            mkdir -p coverage
+            lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info
+
+      - name: Upload Coverage Information to Codecov
+        if: contains(matrix.config.build_type, 'Coverage')
+        uses: codecov/codecov-action@v4
+        with:
+          token: ${{ secrets.CODECOV_TOKEN }}
+          flags: mavsdk
+          file: coverage/lcov.info

.vscode/cmake-variants.yaml

@@ -286,6 +286,11 @@ CONFIG:
       buildType: MiniSizeRel
       settings:
         CONFIG: matek_gnss-m9n-f4_default
+    micoair_h743_default:
+      short: micoair_h743
+      buildType: MinSizeRel
+      settings:
+        CONFIG: micoair_h743_default
     modalai_fc-v1_default:
       short: modalai_fc-v1
       buildType: MinSizeRel

CMakeLists.txt

@@ -113,12 +113,20 @@ include(px4_parse_function_args)
 include(px4_git)
 
 execute_process(
-	COMMAND git describe --exclude ext/* --always --tags
+	COMMAND git describe --exclude ext/* --tags --match "v[0-9]*"
 	OUTPUT_VARIABLE PX4_GIT_TAG
 	OUTPUT_STRIP_TRAILING_WHITESPACE
+	RESULTS_VARIABLE GIT_DESCRIBE_RESULT
 	WORKING_DIRECTORY ${PX4_SOURCE_DIR}
 	)
 
+# if proper git tag unavilable default to v0.0.0
+if(NOT ${GIT_DESCRIBE_RESULT} MATCHES "0")
+	set(PX4_GIT_TAG "v0.0.0")
+endif()
+
+message(STATUS "PX4_GIT_TAG: ${PX4_GIT_TAG}")
+
 # git describe to X.Y.Z version
 string(REPLACE "." ";" VERSION_LIST ${PX4_GIT_TAG})
 

Makefile

@@ -340,6 +340,7 @@ bootloaders_update: \
 	matek_h743_bootloader \
 	matek_h743-mini_bootloader \
 	matek_h743-slim_bootloader \
+        micoair_h743_bootloader \
 	modalai_fc-v2_bootloader \
 	mro_ctrl-zero-classic_bootloader \
 	mro_ctrl-zero-h7_bootloader \

ROMFS/px4fmu_common/init.d-posix/airframes/1045_gazebo-classic_quadtailsitter

@@ -33,7 +33,7 @@ param set-default PWM_MAIN_FUNC3 103
 param set-default PWM_MAIN_FUNC4 104
 param set-default PWM_MAIN_FUNC5 0
 
-parm set-default FD_FAIL_R 70
+param set-default FD_FAIL_R 70
 
 param set-default FW_P_TC 0.6
 

ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover

@@ -14,15 +14,17 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge
 # Rover parameters
 param set-default RD_WHEEL_TRACK 0.3
 param set-default RD_MAN_YAW_SCALE 0.1
-param set-default RD_YAW_RATE_I 0.1
-param set-default RD_YAW_RATE_P 5
 param set-default RD_MAX_ACCEL 6
 param set-default RD_MAX_JERK 30
 param set-default RD_MAX_SPEED 7
-param set-default RD_HEADING_P 5
-param set-default RD_HEADING_I 0.1
+param set-default RD_YAW_RATE_P 5
+param set-default RD_YAW_RATE_I 0
+param set-default RD_YAW_P 5
+param set-default RD_YAW_I 0
+param set-default RD_SPEED_P 1
+param set-default RD_SPEED_I 0
 param set-default RD_MAX_YAW_RATE 180
-param set-default RD_MISS_SPD_DEF 7
+param set-default RD_MISS_SPD_DEF 5
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533
 

ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower

@@ -27,8 +27,8 @@ 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_HEADING_P 5
-param set-default RD_HEADING_I 0.1
+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_MISS_SPD_DEF 8
 param set-default RD_TRANS_DRV_TRN 0.349066

ROMFS/px4fmu_common/init.d-posix/airframes/4014_gz_x500_mono_cam_down

@@ -0,0 +1,10 @@
+#!/bin/sh
+#
+# @name Gazebo x500 mono cam
+#
+# @type Quadrotor
+#
+
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_mono_cam_down}
+
+. ${R}etc/init.d-posix/airframes/4001_gz_x500

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -85,6 +85,7 @@ px4_add_romfs_files(
 	4011_gz_lawnmower
 	4012_gz_rover_ackermann
 	4013_gz_x500_lidar
+	4014_gz_x500_mono_cam_down
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator

@@ -172,7 +172,8 @@ else
 	# otherwise start simulator (mavlink) module
 
 	# EKF2 specifics
-	param set-default EKF2_GPS_DELAY 10
+	param set-default GPS_1_DELAY 10
+	param set-default GPS_2_DELAY 10
 	param set-default EKF2_MULTI_IMU 3
 	param set-default SENS_IMU_MODE 0
 

ROMFS/px4fmu_common/init.d/airframes/4061_atl_mantis_edu

@@ -47,7 +47,6 @@ param set-default EKF2_BCOEF_Y 25.5
 
 param set-default EKF2_DRAG_CTRL 1
 
-param set-default EKF2_GPS_DELAY 100
 param set-default EKF2_GPS_POS_X 0.06
 param set-default EKF2_GPS_V_NOISE 0.5
 
@@ -79,6 +78,7 @@ param set-default EKF2_RNG_POS_Z 0.033
 
 param set-default EKF2_TERR_NOISE 1
 
+param set-default GPS_1_DELAY 100
 
 # Maximum allowed angle velocity in the landed state
 param set-default LNDMC_ROT_MAX 40

ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover

@@ -20,3 +20,25 @@ param set-default RBCLW_ADDRESS 128
 param set-default RBCLW_FUNC1 101
 param set-default RBCLW_FUNC2 102
 param set-default RBCLW_REV 1 # reverse right wheels
+
+# Rover parameters
+param set-default RD_WHEEL_TRACK 0.3
+param set-default RD_MAN_YAW_SCALE 1
+param set-default RD_MAX_ACCEL 5
+param set-default RD_MAX_JERK 50
+param set-default RD_MAX_SPEED 2
+param set-default RD_YAW_RATE_P 0.1
+param set-default RD_YAW_RATE_I 0
+param set-default RD_YAW_P 5
+param set-default RD_YAW_I 0
+param set-default RD_SPEED_P 0.5
+param set-default RD_SPEED_I 0.1
+param set-default RD_MAX_YAW_RATE 300
+param set-default RD_MISS_SPD_DEF 1.8
+param set-default RD_TRANS_DRV_TRN 0.349066
+param set-default RD_TRANS_TRN_DRV 0.174533
+
+# Pure pursuit parameters
+param set-default PP_LOOKAHD_MAX 10
+param set-default PP_LOOKAHD_MIN 1
+param set-default PP_LOOKAHD_GAIN 1

Tools/setup/ubuntu.sh

@@ -225,7 +225,7 @@ if [[ $INSTALL_SIM == "true" ]]; then
 
 	# Gazebo / Gazebo classic installation
 	if [[ "${UBUNTU_RELEASE}" == "22.04" ]]; then
-		echo "Gazebo (Garden) will be installed"
+		echo "Gazebo (Harmonic) will be installed"
 		echo "Earlier versions will be removed"
 		# Add Gazebo binary repository
 		sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
@@ -233,7 +233,7 @@ if [[ $INSTALL_SIM == "true" ]]; then
 		sudo apt-get update -y --quiet
 
 		# Install Gazebo
-		gazebo_packages="gz-garden"
+		gazebo_packages="gz-harmonic"
 	elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then
 		echo "Gazebo (Garden) will be installed"
 		echo "Earlier versions will be removed"

boards/ark/fmu-v6x/default.px4board

@@ -85,8 +85,6 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_SD_BENCH=y
-CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y

boards/cuav/x7pro/default.px4board

@@ -79,7 +79,6 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
-CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
@@ -91,7 +90,6 @@ CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SD_BENCH=y
-CONFIG_SYSTEMCMDS_SD_STRESS=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
@@ -99,4 +97,3 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_FAKE_GPS=y

boards/px4/fmu-v4pro/test.px4board

@@ -3,6 +3,8 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_BOARD_TESTING=y

boards/px4/fmu-v5/cryptotest.px4board

@@ -1,4 +1,6 @@
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
+CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_BOARD_CRYPTO=y
 CONFIG_DRIVERS_STUB_KEYSTORE=y
 CONFIG_DRIVERS_SW_CRYPTO=y

boards/px4/fmu-v5x/default.px4board

@@ -29,7 +29,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
-CONFIG_COMMON_INS=y
 CONFIG_DRIVERS_IRLOCK=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y

boards/px4/fmu-v6x/default.px4board

@@ -30,7 +30,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
-CONFIG_COMMON_INS=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_OSD_MSP_OSD=y

boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld

@@ -377,7 +377,6 @@
 *(.text._ZN19StickAccelerationXYC1EP12ModuleParams)
 *(.text.imxrt_epsubmit)
 *(.text._ZN15PositionControl6updateEf)
-*(.text._ZN3Ekf29checkVerticalAccelerationBiasERKN9estimator9imuSampleE)
 *(.text._ZN23MavlinkStreamScaledIMU24sendEv)
 *(.text._ZN5PX4IO10io_reg_getEhhPtj)
 *(.text.imxrt_dma_send)

msg/ActionRequest.msg

@@ -11,7 +11,7 @@ uint8 ACTION_VTOL_TRANSITION_TO_MULTICOPTER = 6
 uint8 ACTION_VTOL_TRANSITION_TO_FIXEDWING = 7
 
 uint8 source # how the request was triggered
-uint8 SOURCE_RC_STICK_GESTURE = 0
+uint8 SOURCE_STICK_GESTURE = 0
 uint8 SOURCE_RC_SWITCH = 1
 uint8 SOURCE_RC_BUTTON = 2
 uint8 SOURCE_RC_MODE_SLOT = 3

msg/BatteryStatus.msg

@@ -1,67 +1,67 @@
-uint64 timestamp			# time since system start (microseconds)
-bool connected				# Whether or not a battery is connected, based on a voltage threshold
-float32 voltage_v			# Battery voltage in volts, 0 if unknown
-float32 current_a			# Battery current in amperes, -1 if unknown
-float32 current_average_a	# Battery current average in amperes (for FW average in level flight), -1 if unknown
-float32 discharged_mah		# Discharged amount in mAh, -1 if unknown
-float32 remaining			# From 1 to 0, -1 if unknown
-float32 scale				# Power scaling factor, >= 1, or -1 if unknown
-float32 time_remaining_s	# predicted time in seconds remaining until battery is empty under previous averaged load, NAN if unknown
-float32 temperature			# temperature of the battery. NaN if unknown
-uint8 cell_count			# Number of cells, 0 if unknown
+uint64 timestamp				# time since system start (microseconds)
+bool connected					# Whether or not a battery is connected, based on a voltage threshold
+float32 voltage_v				# Battery voltage in volts, 0 if unknown
+float32 current_a				# Battery current in amperes, -1 if unknown
+float32 current_average_a			# Battery current average in amperes (for FW average in level flight), -1 if unknown
+float32 discharged_mah				# Discharged amount in mAh, -1 if unknown
+float32 remaining				# From 1 to 0, -1 if unknown
+float32 scale					# Power scaling factor, >= 1, or -1 if unknown
+float32 time_remaining_s			# predicted time in seconds remaining until battery is empty under previous averaged load, NAN if unknown
+float32 temperature				# Temperature of the battery in degrees Celcius, NaN if unknown
+uint8 cell_count				# Number of cells, 0 if unknown
 
 uint8 BATTERY_SOURCE_POWER_MODULE = 0
 uint8 BATTERY_SOURCE_EXTERNAL = 1
 uint8 BATTERY_SOURCE_ESCS = 2
-uint8 source				# Battery source
-uint8 priority				# Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
-uint16 capacity				# actual capacity of the battery
-uint16 cycle_count			# number of discharge cycles the battery has experienced
-uint16 average_time_to_empty	# predicted remaining battery capacity based on the average rate of discharge in min
-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		# state of health. FullChargeCapacity/DesignCapacity, 0-100%.
-uint16 max_error			# max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100%
+uint8 source					# Battery source
+uint8 priority					# Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
+uint16 capacity					# actual capacity of the battery
+uint16 cycle_count				# number of discharge cycles the battery has experienced
+uint16 average_time_to_empty			# predicted remaining battery capacity based on the average rate of discharge in min
+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				# state of health. FullChargeCapacity/DesignCapacity, 0-100%.
+uint16 max_error				# max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100%
 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
+uint16 interface_error				# interface error counter
 
-float32[14] voltage_cell_v		# Battery individual cell voltages, 0 if unknown
-float32 max_cell_voltage_delta	# Max difference between individual cell voltages
+float32[14] voltage_cell_v			# Battery individual cell voltages, 0 if unknown
+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
+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 BATTERY_WARNING_NONE = 0		# no battery low voltage warning active
-uint8 BATTERY_WARNING_LOW = 1		# warning of low voltage
-uint8 BATTERY_WARNING_CRITICAL = 2	# critical voltage, return / abort immediately
-uint8 BATTERY_WARNING_EMERGENCY = 3	# immediate landing required
-uint8 BATTERY_WARNING_FAILED = 4	# the battery has failed completely
-uint8 BATTERY_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 BATTERY_STATE_CHARGING = 7 # Battery is charging
+uint8 BATTERY_WARNING_NONE = 0			# no battery low voltage warning active
+uint8 BATTERY_WARNING_LOW = 1			# warning of low voltage
+uint8 BATTERY_WARNING_CRITICAL = 2		# critical voltage, return / abort immediately
+uint8 BATTERY_WARNING_EMERGENCY = 3		# immediate landing required
+uint8 BATTERY_WARNING_FAILED = 4		# the battery has failed completely
+uint8 BATTERY_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 BATTERY_STATE_CHARGING = 7 		# Battery is charging
 
-uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged
-uint8 BATTERY_FAULT_SPIKES = 1 # Voltage spikes
-uint8 BATTERY_FAULT_CELL_FAIL= 2 # One or more cells have failed
-uint8 BATTERY_FAULT_OVER_CURRENT = 3 # Over-current
-uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 # Over-temperature
-uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault
-uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with battery one
-uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware
-uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system
-uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 # hardware problem
-uint8 BATTERY_FAULT_FAILED_TO_ARM = 10 # Battery had a problem while arming
-uint8 BATTERY_FAULT_COUNT = 11 # Counter - keep it as last element!
+uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 		# Battery has deep discharged
+uint8 BATTERY_FAULT_SPIKES = 1 			# Voltage spikes
+uint8 BATTERY_FAULT_CELL_FAIL= 2 		# One or more cells have failed
+uint8 BATTERY_FAULT_OVER_CURRENT = 3 		# Over-current
+uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 	# Over-temperature
+uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 	# Under-temperature fault
+uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 	# Vehicle voltage is not compatible with battery one
+uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 	# Battery firmware is not compatible with current autopilot firmware
+uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 	# Battery model is not supported by the system
+uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 	# hardware problem
+uint8 BATTERY_FAULT_FAILED_TO_ARM = 10 		# Battery had a problem while arming
+uint8 BATTERY_FAULT_COUNT = 11 			# Counter - keep it as last element!
 
-uint16 faults		# Smart battery supply status/fault flags (bitmask) for health indication.
-uint8 warning		# Current battery warning
+uint16 faults					# Smart battery supply status/fault flags (bitmask) for health indication.
+uint8 warning					# Current battery warning
 
 uint8 MAX_INSTANCES = 4
 
-float32 full_charge_capacity_wh     # The compensated battery capacity
-float32 remaining_capacity_wh       # The compensated battery capacity remaining
-uint16 over_discharge_count         # Number of battery overdischarge
-float32 nominal_voltage             # Nominal voltage of the battery pack
+float32 full_charge_capacity_wh     		# The compensated battery capacity
+float32 remaining_capacity_wh       		# The compensated battery capacity remaining
+uint16 over_discharge_count         		# Number of battery overdischarge
+float32 nominal_voltage             		# Nominal voltage of the battery pack
 
 float32 internal_resistance_estimate            # [Ohm] Internal resistance per cell estimate
 float32 ocv_estimate                            # [V] Open circuit voltage estimate

msg/CMakeLists.txt

@@ -152,6 +152,10 @@ set(msg_files
 	ObstacleDistance.msg
 	OffboardControlMode.msg
 	OnboardComputerStatus.msg
+	OpenDroneIdArmStatus.msg
+	OpenDroneIdOperatorId.msg
+	OpenDroneIdSelfId.msg
+	OpenDroneIdSystem.msg
 	OrbitStatus.msg
 	OrbTest.msg
 	OrbTestLarge.msg
@@ -182,6 +186,7 @@ set(msg_files
 	RoverAckermannGuidanceStatus.msg
 	RoverAckermannStatus.msg
 	RoverDifferentialGuidanceStatus.msg
+	RoverDifferentialSetpoint.msg
 	RoverDifferentialStatus.msg
 	Rpm.msg
 	RtlStatus.msg

msg/DistanceSensor.msg

@@ -40,3 +40,8 @@ uint8 ROTATION_UPWARD_FACING   = 24 # MAV_SENSOR_ROTATION_PITCH_90
 uint8 ROTATION_DOWNWARD_FACING = 25 # MAV_SENSOR_ROTATION_PITCH_270
 
 uint8 ROTATION_CUSTOM          = 100 # MAV_SENSOR_ROTATION_CUSTOM
+
+uint8 mode
+uint8 MODE_UNKNOWN  = 0
+uint8 MODE_ENABLED  = 1
+uint8 MODE_DISABLED = 2

msg/EstimatorStatusFlags.msg

@@ -45,6 +45,8 @@ bool cs_mag_heading_consistent  # 37 - true when the heading obtained from mag d
 bool cs_aux_gpos                # 38 - true if auxiliary global position measurement fusion is intended
 bool cs_rng_terrain             # 39 - true if we are fusing range finder data for terrain
 bool cs_opt_flow_terrain        # 40 - true if we are fusing flow data for terrain
+bool cs_valid_fake_pos          # 41 - true if a valid constant position is being fused
+bool cs_constant_pos            # 42 - true if the vehicle is at a constant position
 
 # fault status
 uint32 fault_status_changes   # number of filter fault status (fs) changes
@@ -57,7 +59,6 @@ bool fs_bad_airspeed          #  5 - true if fusion of the airspeed has encounte
 bool fs_bad_sideslip          #  6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
 bool fs_bad_optflow_x         #  7 - true if fusion of the optical flow X axis has encountered a numerical error
 bool fs_bad_optflow_y         #  8 - true if fusion of the optical flow Y axis has encountered a numerical error
-bool fs_bad_acc_bias          #  9 - true if bad delta velocity bias estimates have been detected
 bool fs_bad_acc_vertical      # 10 - true if bad vertical accelerometer data has been detected
 bool fs_bad_acc_clipping      # 11 - true if delta velocity data contains clipping (asymmetric railing)
 

msg/OpenDroneIdArmStatus.msg

@@ -0,0 +1,3 @@
+uint64 timestamp
+uint8 status
+char[50] error

msg/OpenDroneIdOperatorId.msg

@@ -0,0 +1,4 @@
+uint64 timestamp
+uint8[20] id_or_mac
+uint8 operator_id_type
+char[20] operator_id

msg/OpenDroneIdSelfId.msg

@@ -0,0 +1,4 @@
+uint64 timestamp
+uint8[20] id_or_mac
+uint8 description_type
+char[23] description

msg/OpenDroneIdSystem.msg

@@ -0,0 +1,13 @@
+uint64 timestamp
+uint8[20] id_or_mac
+uint8 operator_location_type
+uint8 classification_type
+int32 operator_latitude
+int32 operator_longitude
+uint16 area_count
+uint16 area_radius
+float32 area_ceiling
+float32 area_floor
+uint8 category_eu
+uint8 class_eu
+float32 operator_altitude_geo

msg/RoverDifferentialGuidanceStatus.msg

@@ -1,10 +1,7 @@
 uint64 timestamp # time since system start (microseconds)
 
-float32 desired_speed 	      # [m/s] Desired forward speed for the rover
 float32 lookahead_distance    # [m] Lookahead distance of pure the pursuit controller
 float32 heading_error_deg 	      # [deg] Heading error of the pure pursuit controller
-float32 pid_heading_integral  # Integral of the PID for the desired yaw rate during missions
-float32 pid_throttle_integral # Integral of the PID for the throttle during missions
 uint8   state_machine         # Driving state of the rover [0: SPOT_TURNING, 1: DRIVING, 2: GOAL_REACHED]
 
 # TOPICS rover_differential_guidance_status

msg/RoverDifferentialSetpoint.msg

@@ -0,0 +1,9 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 forward_speed_setpoint 		  # [m/s] Desired forward speed for the rover
+float32 forward_speed_setpoint_normalized # [-1, 1] Normalized forward speed for the rover
+float32 yaw_rate_setpoint      		  # [rad/s] Desired yaw rate for the rover (Overriden by yaw controller if yaw_setpoint is used)
+float32 yaw_rate_setpoint_normalized      # [-1, 1] Normalized yaw rate for the rover
+float32 yaw_setpoint 	       		  # [rad] Desired yaw (heading) for the rover
+
+# TOPICS rover_differential_setpoint

msg/RoverDifferentialStatus.msg

@@ -1,8 +1,11 @@
 uint64 timestamp # time since system start (microseconds)
 
 float32 actual_speed	       # [m/s] Actual forward speed of the rover
-float32 desired_yaw_rate_deg_s # [deg/s] Desired yaw rate
+float32 actual_yaw_deg         # [deg] Actual yaw of the rover
 float32 actual_yaw_rate_deg_s  # [deg/s] Actual yaw rate of the rover
-float32 pid_yaw_rate_integral  # Integral of the PID for the desired yaw rate controller
+float32 desired_yaw_rate_deg_s # [deg/s] Yaw rate setpoint for the closed loop yaw rate controller
+float32 pid_yaw_integral       # Integral of the PID for the closed loop yaw controller
+float32 pid_yaw_rate_integral  # Integral of the PID for the closed loop yaw rate controller
+float32 pid_throttle_integral  # Integral of the PID for the closed loop speed controller
 
 # TOPICS rover_differential_status

msg/SensorAirflow.msg

@@ -1,5 +1,5 @@
 uint64 timestamp		# time since system start (microseconds)
 uint32 device_id                # unique device ID for the sensor that does not change between power cycles
 float32 speed			# the speed being reported by the wind / airflow sensor
-float32 direction		# the direction bein report by the wind / airflow sensor
+float32 direction		# the direction being reported by the wind / airflow sensor
 uint8 status			# Status code from the sensor

msg/VehicleOpticalFlowVel.msg

@@ -4,6 +4,9 @@ uint64 timestamp_sample                # the timestamp of the raw data (microsec
 float32[2] vel_body                    # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
 float32[2] vel_ne                      # same as vel_body but in local frame (m/s)
 
+float32[2] vel_body_filtered           # filtered velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s)
+float32[2] vel_ne_filtered             # filtered same as vel_body_filtered but in local frame (m/s)
+
 float32[2] flow_rate_uncompensated     # integrated optical flow measurement (rad/s)
 float32[2] flow_rate_compensated       # integrated optical flow measurement compensated for angular motion (rad/s)
 

msg/VehicleStatus.msg

@@ -12,7 +12,7 @@ uint8 ARMING_STATE_ARMED    = 2
 uint8 latest_arming_reason
 uint8 latest_disarming_reason
 uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0
-uint8 ARM_DISARM_REASON_RC_STICK = 1
+uint8 ARM_DISARM_REASON_STICK_GESTURE = 1
 uint8 ARM_DISARM_REASON_RC_SWITCH = 2
 uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3
 uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4

platforms/common/pab_manifest.c

@@ -357,6 +357,7 @@ static const px4_hw_mft_item_t base_configuration_17[] = {
 };
 
 // BASE ID 0x100 Holybro Pixhawk Jetson Baseboard Alaised to ID 0
+// BASE ID 0x150 ZeroOne Pixhawk Baseboard Alaised to ID 0
 
 static px4_hw_mft_list_entry_t mft_lists[] = {
 //  ver_rev
@@ -372,6 +373,7 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{HW_BASE_ID(16),     base_configuration_0, arraySize(base_configuration_0)},   // Auterion Skynode ver 16
 	{HW_BASE_ID(17),     base_configuration_17, arraySize(base_configuration_17)}, // Auterion Skynode ver 17
 	{HW_BASE_ID(0x100),  base_configuration_0, arraySize(base_configuration_0)},   // Holybro Pixhawk Jetson Baseboard ver 0x100 Alaised to ID 0
+	{HW_BASE_ID(0x150),  base_configuration_0, arraySize(base_configuration_0)},   // ZeroOne Pixhawk Baseboard ver 0x150
 };
 
 /************************************************************************************

platforms/nuttx/init/stm32f7/rc.board_arch_defaults

@@ -13,4 +13,4 @@ param set-default -s MC_AT_EN 1
 
 param set-default -s UAVCAN_ENABLE 2
 
-set LOGGER_BUF 64
+set LOGGER_BUF 40

src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp

@@ -145,6 +145,7 @@ private:
 	typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_UNKNOWN};
 	bool _restriction{false};
 	bool _auto_restriction{false};
+	bool _prev_restriction{false};
 };
 
 LightwareLaser::LightwareLaser(const I2CSPIDriverConfig &config) :
@@ -438,7 +439,7 @@ int LightwareLaser::updateRestriction()
 		updateParams();
 	}
 
-	bool _prev_restriction{_restriction};
+	_prev_restriction = _restriction;
 
 	switch (_param_sf1xx_mode.get()) {
 	case 0: // Sensor disabled
@@ -498,6 +499,8 @@ void LightwareLaser::RunImpl()
 
 	case State::Running:
 		if (!_restriction) {
+			_px4_rangefinder.set_mode(distance_sensor_s::MODE_ENABLED);
+
 			if (PX4_OK != collect()) {
 				PX4_DEBUG("collection error");
 
@@ -506,6 +509,14 @@ void LightwareLaser::RunImpl()
 					_consecutive_errors = 0;
 				}
 			}
+
+		} else {
+			_px4_rangefinder.set_mode(distance_sensor_s::MODE_DISABLED);
+
+			if (!_prev_restriction) { // Publish disabled status once
+				_px4_rangefinder.update(hrt_absolute_time(), -1.f, 0);
+			}
+
 		}
 
 		ScheduleDelayed(_conversion_interval);

src/drivers/gps/gps.cpp

@@ -221,24 +221,11 @@ private:
 
 	px4::atomic<int> _scheduled_reset{(int)GPSRestartType::None};
 
-	/**
-	 * Publish the gps struct
-	 */
-	void 				publish();
+	void publish();
+	uint64_t get_delay_us() const;
 
-	/**
-	 * Publish the satellite info
-	 */
 	void 				publishSatelliteInfo();
-
-	/**
-	 * Publish RTCM corrections
-	 */
 	void 				publishRTCMCorrections(uint8_t *data, size_t len);
-
-	/**
-	 * Publish RTCM corrections
-	 */
 	void 				publishRelativePosition(sensor_gnss_relative_s &gnss_relative);
 
 	/**
@@ -1179,6 +1166,14 @@ void
 GPS::publish()
 {
 	if (_instance == Instance::Main || _is_gps_main_advertised.load()) {
+		if ((_report_gps_pos.timestamp_sample == 0) || (_report_gps_pos.timestamp_sample == _report_gps_pos.timestamp)) {
+			const uint64_t delay_us = get_delay_us();
+
+			if (_report_gps_pos.timestamp > delay_us) {
+				_report_gps_pos.timestamp_sample = _report_gps_pos.timestamp - get_delay_us();
+			}
+		}
+
 		_report_gps_pos.device_id = get_device_id();
 
 		_report_gps_pos.selected_rtcm_instance = _selected_rtcm_instance;
@@ -1211,6 +1206,33 @@ GPS::publish()
 	}
 }
 
+uint64_t
+GPS::get_delay_us() const
+{
+	float delay_ms;
+
+	switch (_instance) {
+	case Instance::Main:
+		param_get(param_find("GPS_1_DELAY"), &delay_ms);
+		break;
+
+	case Instance::Secondary:
+		param_get(param_find("GPS_2_DELAY"), &delay_ms);
+		break;
+
+	default:
+		delay_ms = 0.f;
+		break;
+	}
+
+	if (delay_ms < -FLT_EPSILON) {
+		// If not specified (< 0), use generic delay as this is more realistic than 0
+		delay_ms = 110.f;
+	}
+
+	return static_cast<uint64_t>(delay_ms * 1e3f);
+}
+
 void
 GPS::publishSatelliteInfo()
 {

src/drivers/gps/params.c

@@ -276,3 +276,33 @@ PARAM_DEFINE_INT32(GPS_1_GNSS, 0);
  * @group GPS
  */
 PARAM_DEFINE_INT32(GPS_2_GNSS, 0);
+
+/**
+ * GPS measurement delay relative to IMU measurements
+ *
+ * Set to -1 if unknown
+ *
+ * @min -1
+ * @max 300
+ * @unit ms
+ * @decimal 1
+ *
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_1_DELAY, -1);
+
+/**
+ * GPS measurement delay relative to IMU measurements
+ *
+ * Set to -1 if unknown
+ *
+ * @min -1
+ * @max 300
+ * @unit ms
+ * @decimal 1
+ *
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_2_DELAY, -1);

src/drivers/power_monitor/ina228/ina228.cpp

@@ -88,6 +88,7 @@ INA228::INA228(const I2CSPIDriverConfig &config, int battery_index) :
 	_battery.setConnected(false);
 	_battery.updateVoltage(0.f);
 	_battery.updateCurrent(0.f);
+	_battery.updateTemperature(0.f);
 	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
 }
 
@@ -306,6 +307,7 @@ INA228::collect()
 	// success = success && (read(INA228_REG_POWER, _power) == PX4_OK);
 	success = success && (read(INA228_REG_CURRENT, _current) == PX4_OK);
 	//success = success && (read(INA228_REG_VSHUNT, _shunt) == PX4_OK);
+	success = success && (read(INA228_REG_DIETEMP, _temperature) == PX4_OK);
 
 	if (!success) {
 		PX4_DEBUG("error reading from sensor");
@@ -315,6 +317,7 @@ INA228::collect()
 	_battery.setConnected(success);
 	_battery.updateVoltage(static_cast<float>(_bus_voltage * INA228_VSCALE));
 	_battery.updateCurrent(static_cast<float>(_current * _current_lsb));
+	_battery.updateTemperature(static_cast<float>(_temperature * INA228_TSCALE));
 	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
 
 	perf_end(_sample_perf);
@@ -381,6 +384,7 @@ INA228::RunImpl()
 		_battery.setConnected(false);
 		_battery.updateVoltage(0.f);
 		_battery.updateCurrent(0.f);
+		_battery.updateTemperature(0.f);
 		_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
 
 		if (init() != PX4_OK) {

src/drivers/power_monitor/ina228/ina228.h

@@ -291,6 +291,7 @@ using namespace time_literals;
 #define INA228_CONST                         13107.2e6f  /* is an internal fixed value used to ensure scaling is maintained properly  */
 #define INA228_SHUNT                         0.0005f   /* Shunt is 500 uOhm */
 #define INA228_VSCALE                        1.95e-04f  /* LSB of voltage is 195.3125 uV/LSB */
+#define INA228_TSCALE                        7.8125e-03f /* LSB of temperature is 7.8125 mDegC/LSB */
 
 #define swap16(w)                            __builtin_bswap16((w))
 #define swap32(d)                            __builtin_bswap32((d))
@@ -339,6 +340,7 @@ private:
 	int32_t           _bus_voltage{0};
 	int64_t           _power{0};
 	int32_t           _current{0};
+	int16_t           _temperature{0};
 	int32_t           _shunt{0};
 	int16_t           _cal{0};
 	int16_t           _range{INA228_ADCRANGE_HIGH};

src/drivers/power_monitor/ina238/ina238.cpp

@@ -80,6 +80,7 @@ INA238::INA238(const I2CSPIDriverConfig &config, int battery_index) :
 	_battery.setConnected(false);
 	_battery.updateVoltage(0.f);
 	_battery.updateCurrent(0.f);
+	_battery.updateTemperature(0.f);
 	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
 }
 
@@ -224,9 +225,11 @@ int INA238::collect()
 	bool success{true};
 	int16_t bus_voltage{0};
 	int16_t current{0};
+	int16_t temperature{0};
 
 	success = (RegisterRead(Register::VS_BUS, (uint16_t &)bus_voltage) == PX4_OK);
 	success = success && (RegisterRead(Register::CURRENT, (uint16_t &)current) == PX4_OK);
+	success = success && (RegisterRead(Register::DIETEMP, (uint16_t &)temperature) == PX4_OK);
 
 	if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) {
 		// check configuration registers periodically or immediately following any failure
@@ -250,6 +253,7 @@ int INA238::collect()
 	_battery.setConnected(success);
 	_battery.updateVoltage(static_cast<float>(bus_voltage * INA238_VSCALE));
 	_battery.updateCurrent(static_cast<float>(current * _current_lsb));
+	_battery.updateTemperature(static_cast<float>(temperature * INA238_TSCALE));
 	_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
 
 	perf_end(_sample_perf);
@@ -309,6 +313,7 @@ void INA238::RunImpl()
 		_battery.setConnected(false);
 		_battery.updateVoltage(0.f);
 		_battery.updateCurrent(0.f);
+		_battery.updateTemperature(0.f);
 		_battery.updateAndPublishBatteryStatus(hrt_absolute_time());
 
 		if (init() != PX4_OK) {

src/drivers/power_monitor/ina238/ina238.h

@@ -73,6 +73,7 @@ using namespace ina238;
 #define INA238_DN_MAX                        32768.0f   /* 2^15 */
 #define INA238_CONST                         819.2e6f  /* is an internal fixed value used to ensure scaling is maintained properly  */
 #define INA238_VSCALE                        3.125e-03f  /* LSB of voltage is 3.1255 mV/LSB */
+#define INA238_TSCALE                        7.8125e-03f /* LSB of temperature is 7.8125 mDegC/LSB */
 
 #define DEFAULT_MAX_CURRENT                  327.68f    /* Amps */
 #define DEFAULT_SHUNT                        0.0003f   /* Shunt is 300 uOhm */

src/drivers/power_monitor/ina238/ina238_registers.hpp

@@ -14,6 +14,7 @@ enum class Register : uint8_t {
 	ADCCONFIG = 0x01, // ADC Configuration Register
 	SHUNT_CAL = 0x02, // Shunt Calibration Register
 	VS_BUS = 0x05,
+	DIETEMP = 0x06,
 	CURRENT = 0x07,
 	MANUFACTURER_ID = 0x3e,
 	DEVICE_ID = 0x3f

src/drivers/uavcan/CMakeLists.txt

@@ -147,6 +147,8 @@ px4_add_module(
 		arming_status.hpp
 		beep.cpp
 		beep.hpp
+		remoteid.cpp
+		remoteid.hpp
 		rgbled.cpp
 		rgbled.hpp
 		safety_state.cpp

src/drivers/uavcan/Kconfig

@@ -26,6 +26,10 @@ if DRIVERS_UAVCAN
         bool "Include safety state controller"
         default y
 
+    config UAVCAN_REMOTEID_CONTROLLER
+        bool "Include remote ID controller"
+        default y
+
     config UAVCAN_RGB_CONTROLLER
         bool "Include rgb controller"
         default y

src/drivers/uavcan/remoteid.cpp

@@ -0,0 +1,356 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "remoteid.hpp"
+#include <modules/mavlink/open_drone_id_translations.hpp>
+#include <drivers/drv_hrt.h>
+
+using namespace time_literals;
+
+
+UavcanRemoteIDController::UavcanRemoteIDController(uavcan::INode &node) :
+	ModuleParams(nullptr),
+	_timer(node),
+	_node(node),
+	_uavcan_pub_remoteid_basicid(node),
+	_uavcan_pub_remoteid_location(node),
+	_uavcan_pub_remoteid_self_id(node),
+	_uavcan_pub_remoteid_system(node),
+	_uavcan_pub_remoteid_operator_id(node),
+	_uavcan_sub_arm_status(node)
+{
+}
+
+int UavcanRemoteIDController::init()
+{
+	// Setup timer and call back function for periodic updates
+	_timer.setCallback(TimerCbBinder(this, &UavcanRemoteIDController::periodic_update));
+	_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / MAX_RATE_HZ));
+
+	int res = _uavcan_sub_arm_status.start(ArmStatusBinder(this, &UavcanRemoteIDController::arm_status_sub_cb));
+
+	if (res < 0) {
+		PX4_WARN("ArmStatus sub failed %i", res);
+		return res;
+	}
+
+	return 0;
+}
+
+void UavcanRemoteIDController::periodic_update(const uavcan::TimerEvent &)
+{
+	_vehicle_status.update();
+
+	send_basic_id();
+	send_location();
+	send_self_id();
+	send_system();
+	send_operator_id();
+}
+
+void UavcanRemoteIDController::send_basic_id()
+{
+	dronecan::remoteid::BasicID basic_id {};
+	// basic_id.id_or_mac // supposedly only used for drone ID data from other UAs
+	basic_id.id_type = dronecan::remoteid::BasicID::ODID_ID_TYPE_SERIAL_NUMBER;
+	basic_id.ua_type = static_cast<uint8_t>(open_drone_id_translations::odidTypeForMavType(
+			_vehicle_status.get().system_type));
+
+	// uas_id: UAS (Unmanned Aircraft System) ID following the format specified by id_type
+	// TODO: MAV_ODID_ID_TYPE_SERIAL_NUMBER needs to be ANSI/CTA-2063 format
+
+	char uas_id[20] = {};
+	board_get_px4_guid_formated((char *)(uas_id), sizeof(uas_id));
+	basic_id.uas_id = uas_id;
+
+	_uavcan_pub_remoteid_basicid.broadcast(basic_id);
+}
+
+void UavcanRemoteIDController::send_location()
+{
+	dronecan::remoteid::Location msg {};
+
+	// initialize all fields to unknown
+	msg.status = MAV_ODID_STATUS_UNDECLARED;
+	msg.direction = 36100; // If unknown: 36100 centi-degrees
+	msg.speed_horizontal = 25500; // If unknown: 25500 cm/s
+	msg.speed_vertical = 6300; // If unknown: 6300 cm/s
+	msg.latitude = 0; // If unknown: 0
+	msg.longitude = 0; // If unknown: 0
+	msg.altitude_geodetic = -1000; // If unknown: -1000 m
+	msg.altitude_geodetic = -1000; // If unknown: -1000 m
+	msg.height = -1000; // If unknown: -1000 m
+	msg.horizontal_accuracy = MAV_ODID_HOR_ACC_UNKNOWN;
+	msg.vertical_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
+	msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
+	msg.speed_accuracy = MAV_ODID_SPEED_ACC_UNKNOWN;
+	msg.timestamp = 0xFFFF; // If unknown: 0xFFFF
+	msg.timestamp_accuracy = MAV_ODID_TIME_ACC_UNKNOWN;
+
+	bool updated = false;
+
+	if (_vehicle_land_detected_sub.advertised()) {
+		vehicle_land_detected_s vehicle_land_detected{};
+
+		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)
+		    && (hrt_elapsed_time(&vehicle_land_detected.timestamp) < 10_s)) {
+			if (vehicle_land_detected.landed) {
+				msg.status = MAV_ODID_STATUS_GROUND;
+
+			} else {
+				msg.status = MAV_ODID_STATUS_AIRBORNE;
+			}
+
+			updated = true;
+		}
+	}
+
+	if (hrt_elapsed_time(&_vehicle_status.get().timestamp) < 10_s) {
+		if (_vehicle_status.get().failsafe && (_vehicle_status.get().arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+			msg.status = MAV_ODID_STATUS_EMERGENCY;
+			updated = true;
+		}
+	}
+
+	if (_vehicle_gps_position_sub.advertised()) {
+		sensor_gps_s vehicle_gps_position{};
+
+		if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)
+		    && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 10_s)) {
+
+			if (vehicle_gps_position.vel_ned_valid) {
+				const matrix::Vector3f vel_ned{vehicle_gps_position.vel_n_m_s, vehicle_gps_position.vel_e_m_s, vehicle_gps_position.vel_d_m_s};
+
+				// direction: calculate GPS course over ground angle
+				const float course = atan2f(vel_ned(1), vel_ned(0));
+				const int course_deg = roundf(math::degrees(matrix::wrap_2pi(course)));
+				msg.direction = math::constrain(100 * course_deg, 0, 35999); // 0 - 35999 centi-degrees
+
+				// speed_horizontal: If speed is larger than 25425 cm/s, use 25425 cm/s.
+				const int speed_horizontal_cm_s = matrix::Vector2f(vel_ned).length() * 100.f;
+				msg.speed_horizontal = math::constrain(speed_horizontal_cm_s, 0, 25425);
+
+				// speed_vertical: Up is positive, If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s.
+				const int speed_vertical_cm_s = roundf(-vel_ned(2) * 100.f);
+				msg.speed_vertical = math::constrain(speed_vertical_cm_s, -6200, 6200);
+
+				msg.speed_accuracy = open_drone_id_translations::odidSpeedAccForVariance(vehicle_gps_position.s_variance_m_s);
+
+				updated = true;
+			}
+
+			if (vehicle_gps_position.fix_type >= 2) {
+				msg.latitude = static_cast<int32_t>(round(vehicle_gps_position.latitude_deg * 1e7));
+				msg.longitude = static_cast<int32_t>(round(vehicle_gps_position.longitude_deg * 1e7));
+
+				// altitude_geodetic
+				if (vehicle_gps_position.fix_type >= 3) {
+					msg.altitude_geodetic = static_cast<float>(round(vehicle_gps_position.altitude_msl_m)); // [m]
+				}
+
+				msg.horizontal_accuracy = open_drone_id_translations::odidHorAccForEph(vehicle_gps_position.eph);
+
+				msg.vertical_accuracy = open_drone_id_translations::odidVerAccForEpv(vehicle_gps_position.epv);
+
+				updated = true;
+			}
+
+			if (vehicle_gps_position.time_utc_usec != 0) {
+				// timestamp: UTC then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000
+				uint64_t utc_time_msec = vehicle_gps_position.time_utc_usec / 1000;
+				msg.timestamp = ((float)(utc_time_msec % (60 * 60 * 1000))) / 1000;
+
+				msg.timestamp_accuracy = open_drone_id_translations::odidTimeForElapsed(hrt_elapsed_time(
+								 &vehicle_gps_position.timestamp));
+
+				updated = true;
+			}
+		}
+	}
+
+	// altitude_barometric: The altitude calculated from the barometric pressue
+	if (_vehicle_air_data_sub.advertised()) {
+		vehicle_air_data_s vehicle_air_data{};
+
+		if (_vehicle_air_data_sub.copy(&vehicle_air_data) && (hrt_elapsed_time(&vehicle_air_data.timestamp) < 10_s)) {
+			msg.altitude_barometric = vehicle_air_data.baro_alt_meter;
+			msg.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN; // We just don't without calibration.
+			updated = true;
+		}
+	}
+
+	// height: The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference
+	if (_home_position_sub.advertised() && _vehicle_local_position_sub.updated()) {
+		home_position_s home_position{};
+		vehicle_local_position_s vehicle_local_position{};
+
+		if (_home_position_sub.copy(&home_position)
+		    && _vehicle_local_position_sub.copy(&vehicle_local_position)
+		    && (hrt_elapsed_time(&vehicle_local_position.timestamp) < 1_s)
+		   ) {
+
+			if (home_position.valid_alt && vehicle_local_position.z_valid && vehicle_local_position.z_global) {
+				float altitude = (-vehicle_local_position.z + vehicle_local_position.ref_alt);
+
+				msg.height = altitude - home_position.alt;
+				msg.height_reference = MAV_ODID_HEIGHT_REF_OVER_TAKEOFF;
+				updated = true;
+			}
+		}
+	}
+
+	if (updated) {
+		_uavcan_pub_remoteid_location.broadcast(msg);
+	}
+}
+
+void UavcanRemoteIDController::send_system()
+{
+	open_drone_id_system_s system;
+
+	if (_open_drone_id_system.advertised() && _open_drone_id_system.copy(&system)) {
+
+		// Use what ground station sends us.
+
+		dronecan::remoteid::System msg {};
+		msg.timestamp = system.timestamp;
+
+		for (unsigned i = 0; i < sizeof(system.id_or_mac); ++i) {
+			msg.id_or_mac.push_back(system.id_or_mac[i]);
+		}
+
+		msg.operator_location_type = system.operator_location_type;
+		msg.classification_type = system.classification_type;
+		msg.operator_latitude = system.operator_latitude;
+		msg.operator_longitude = system.operator_longitude;
+		msg.area_count = system.area_count;
+		msg.area_radius = system.area_radius;
+		msg.area_ceiling = system.area_ceiling;
+		msg.area_floor = system.area_floor;
+		msg.category_eu = system.category_eu;
+		msg.class_eu = system.class_eu;
+		msg.operator_altitude_geo = system.operator_altitude_geo;
+
+		_uavcan_pub_remoteid_system.broadcast(msg);
+
+	} else {
+		// And otherwise, send our home/takeoff location.
+
+		sensor_gps_s vehicle_gps_position;
+		home_position_s home_position;
+
+		if (_vehicle_gps_position_sub.copy(&vehicle_gps_position) && _home_position_sub.copy(&home_position)) {
+			if (vehicle_gps_position.fix_type >= 3
+			    && home_position.valid_alt && home_position.valid_hpos) {
+
+				dronecan::remoteid::System msg {};
+
+				// msg.id_or_mac // Only used for drone ID data received from other UAs.
+				msg.operator_location_type = MAV_ODID_OPERATOR_LOCATION_TYPE_TAKEOFF;
+				msg.classification_type = MAV_ODID_CLASSIFICATION_TYPE_UNDECLARED;
+				msg.operator_latitude = home_position.lat * 1e7;
+				msg.operator_longitude = home_position.lon * 1e7;
+				msg.area_count = 1;
+				msg.area_radius = 0;
+				msg.area_ceiling = -1000;
+				msg.area_floor = -1000;
+				msg.category_eu = MAV_ODID_CATEGORY_EU_UNDECLARED;
+				msg.class_eu = MAV_ODID_CLASS_EU_UNDECLARED;
+				float wgs84_amsl_offset = vehicle_gps_position.altitude_ellipsoid_m - vehicle_gps_position.altitude_msl_m;
+				msg.operator_altitude_geo = home_position.alt + wgs84_amsl_offset;
+
+				// timestamp: 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019.
+				static uint64_t utc_offset_s = 1'546'300'800; // UTC seconds since 00:00:00 01/01/2019
+				msg.timestamp = vehicle_gps_position.time_utc_usec / 1e6 - utc_offset_s;
+
+				_uavcan_pub_remoteid_system.broadcast(msg);
+			}
+		}
+	}
+}
+
+void UavcanRemoteIDController::send_self_id()
+{
+	open_drone_id_self_id_s self_id;
+
+	if (_open_drone_id_self_id.copy(&self_id)) {
+
+		dronecan::remoteid::SelfID msg {};
+
+		for (unsigned i = 0; i < sizeof(self_id.id_or_mac); ++i) {
+			msg.id_or_mac.push_back(self_id.id_or_mac[i]);
+		}
+
+		msg.description_type = self_id.description_type;
+
+		for (unsigned i = 0; i < sizeof(self_id.description); ++i) {
+			msg.description.push_back(self_id.description[i]);
+		}
+
+		_uavcan_pub_remoteid_self_id.broadcast(msg);
+	}
+}
+
+void UavcanRemoteIDController::send_operator_id()
+{
+	open_drone_id_operator_id_s operator_id;
+
+	if (_open_drone_id_operator_id.copy(&operator_id)) {
+
+		dronecan::remoteid::OperatorID msg {};
+
+		for (unsigned i = 0; i < sizeof(operator_id.id_or_mac); ++i) {
+			msg.id_or_mac.push_back(operator_id.id_or_mac[i]);
+		}
+
+		msg.operator_id_type = operator_id.operator_id_type;
+
+		for (unsigned i = 0; i < sizeof(operator_id.operator_id); ++i) {
+			msg.operator_id.push_back(operator_id.operator_id[i]);
+		}
+
+		_uavcan_pub_remoteid_operator_id.broadcast(msg);
+	}
+}
+
+void
+UavcanRemoteIDController::arm_status_sub_cb(const uavcan::ReceivedDataStructure<dronecan::remoteid::ArmStatus> &msg)
+{
+	open_drone_id_arm_status_s arm_status{};
+
+	arm_status.timestamp = hrt_absolute_time();
+	arm_status.status = msg.status;
+	memcpy(arm_status.error, msg.error.c_str(), sizeof(arm_status.error));
+
+	_open_drone_id_arm_status_pub.publish(arm_status);
+}

src/drivers/uavcan/remoteid.hpp

@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/open_drone_id_operator_id.h>
+#include <uORB/topics/open_drone_id_arm_status.h>
+#include <uORB/topics/open_drone_id_self_id.h>
+#include <uORB/topics/open_drone_id_system.h>
+
+#include <uavcan/uavcan.hpp>
+#include <dronecan/remoteid/BasicID.hpp>
+#include <dronecan/remoteid/Location.hpp>
+#include <dronecan/remoteid/SelfID.hpp>
+#include <dronecan/remoteid/System.hpp>
+#include <dronecan/remoteid/ArmStatus.hpp>
+#include <dronecan/remoteid/OperatorID.hpp>
+
+#include <px4_platform_common/module_params.h>
+
+class UavcanRemoteIDController : public ModuleParams
+{
+public:
+	UavcanRemoteIDController(uavcan::INode &node);
+	~UavcanRemoteIDController() = default;
+
+	int init();
+
+private:
+	typedef uavcan::MethodBinder<UavcanRemoteIDController *, void (UavcanRemoteIDController::*)(const uavcan::TimerEvent &)>
+	TimerCbBinder;
+
+	static constexpr unsigned MAX_RATE_HZ = 1;
+	uavcan::TimerEventForwarder<TimerCbBinder> _timer;
+
+	void periodic_update(const uavcan::TimerEvent &);
+
+	void send_basic_id();
+	void send_location();
+	void send_self_id();
+	void send_system();
+	void send_operator_id();
+
+	void arm_status_sub_cb(const uavcan::ReceivedDataStructure<dronecan::remoteid::ArmStatus> &msg);
+
+	uavcan::INode &_node;
+
+	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+	uORB::SubscriptionData<vehicle_status_s> _vehicle_status{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
+	uORB::Subscription _open_drone_id_operator_id{ORB_ID(open_drone_id_operator_id)};
+	uORB::Subscription _open_drone_id_self_id{ORB_ID(open_drone_id_self_id)};
+	uORB::Subscription _open_drone_id_system{ORB_ID(open_drone_id_system)};
+	uORB::Publication<open_drone_id_arm_status_s> _open_drone_id_arm_status_pub{ORB_ID(open_drone_id_arm_status)};
+
+	uavcan::Publisher<dronecan::remoteid::BasicID> _uavcan_pub_remoteid_basicid;
+	uavcan::Publisher<dronecan::remoteid::Location> _uavcan_pub_remoteid_location;
+	uavcan::Publisher<dronecan::remoteid::SelfID> _uavcan_pub_remoteid_self_id;
+	uavcan::Publisher<dronecan::remoteid::System> _uavcan_pub_remoteid_system;
+	uavcan::Publisher<dronecan::remoteid::OperatorID> _uavcan_pub_remoteid_operator_id;
+
+	using ArmStatusBinder = uavcan::MethodBinder < UavcanRemoteIDController *,
+	      void (UavcanRemoteIDController::*)(const uavcan::ReceivedDataStructure<dronecan::remoteid::ArmStatus> &) >;
+
+	uavcan::Subscriber<dronecan::remoteid::ArmStatus, ArmStatusBinder> _uavcan_sub_arm_status;
+};

src/drivers/uavcan/rgbled.hpp

@@ -51,7 +51,7 @@ public:
 	int init();
 
 private:
-	// Max update rate to avoid exessive bus traffic
+	// Max update rate to avoid excessive bus traffic
 	static constexpr unsigned MAX_RATE_HZ = 20;
 
 	void periodic_update(const uavcan::TimerEvent &);

src/drivers/uavcan/uavcan_main.cpp

@@ -96,6 +96,9 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 #if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER)
 	_safety_state_controller(_node),
 #endif
+#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER)
+	_remoteid_controller(_node),
+#endif
 #if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
 	_rgbled_controller(_node),
 #endif
@@ -558,6 +561,16 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 		return ret;
 	}
 
+#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER)
+	ret = _remoteid_controller.init();
+
+	if (ret < 0) {
+		return ret;
+	}
+
+#endif
+
+
 #if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
 	ret = _rgbled_controller.init();
 

src/drivers/uavcan/uavcan_main.hpp

@@ -68,6 +68,10 @@
 
 #include "logmessage.hpp"
 
+#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER)
+#include "remoteid.hpp"
+#endif
+
 #if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
 #include "rgbled.hpp"
 #endif
@@ -269,6 +273,9 @@ private:
 #if defined(CONFIG_UAVCAN_SAFETY_STATE_CONTROLLER)
 	UavcanSafetyState         	_safety_state_controller;
 #endif
+#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER)
+	UavcanRemoteIDController _remoteid_controller;
+#endif
 #if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
 	UavcanRGBController             _rgbled_controller;
 #endif

src/lib/battery/battery.cpp

@@ -106,6 +106,11 @@ void Battery::updateCurrent(const float current_a)
 	_current_a = current_a;
 }
 
+void Battery::updateTemperature(const float temperature_c)
+{
+	_temperature_c = temperature_c;
+}
+
 void Battery::updateBatteryStatus(const hrt_abstime &timestamp)
 {
 	// Require minimum voltage otherwise override connected status
@@ -149,7 +154,7 @@ battery_status_s Battery::getBatteryStatus()
 	battery_status.remaining = _state_of_charge;
 	battery_status.scale = _scale;
 	battery_status.time_remaining_s = computeRemainingTime(_current_a);
-	battery_status.temperature = NAN;
+	battery_status.temperature = _temperature_c;
 	battery_status.cell_count = _params.n_cells;
 	battery_status.connected = _connected;
 	battery_status.source = _source;

src/lib/battery/battery.h

@@ -91,6 +91,7 @@ public:
 	void setStateOfCharge(const float soc) { _state_of_charge = soc; _external_state_of_charge = true; }
 	void updateVoltage(const float voltage_v);
 	void updateCurrent(const float current_a);
+	void updateTemperature(const float temperature_c);
 
 	/**
 	 * Update state of charge calculations
@@ -168,6 +169,7 @@ private:
 	float _current_a{-1};
 	AlphaFilter<float>
 	_current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight.
+	float _temperature_c{NAN};
 	float _discharged_mah{0.f};
 	float _discharged_mah_loop{0.f};
 	float _state_of_charge_volt_based{-1.f}; // [0,1]

src/lib/drivers/rangefinder/PX4Rangefinder.cpp

@@ -39,7 +39,8 @@ PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_or
 {
 	set_device_id(device_id);
 	set_orientation(device_orientation);
-	set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); // Default to type LASER
+	set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
+	set_mode(distance_sensor_s::MODE_UNKNOWN);
 }
 
 PX4Rangefinder::~PX4Rangefinder()

src/lib/drivers/rangefinder/PX4Rangefinder.hpp

@@ -60,6 +60,8 @@ public:
 
 	void set_orientation(const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
 
+	void set_mode(const uint8_t mode) { _distance_sensor_pub.get().mode = mode; }
+
 	void update(const hrt_abstime &timestamp_sample, const float distance, const int8_t quality = -1);
 
 	int get_instance() { return _distance_sensor_pub.get_instance(); };

src/lib/events/enums.json

@@ -447,12 +447,12 @@
                             "description": "Transition to standby"
                         },
                         "1": {
-                            "name": "rc_stick",
-                            "description": "RC"
+                            "name": "stick_gesture",
+                            "description": "Stick gesture"
                         },
                         "2": {
                             "name": "rc_switch",
-                            "description": "RC (switch)"
+                            "description": "RC switch"
                         },
                         "3": {
                             "name": "command_internal",

src/lib/mixer_module/mixer_module.cpp

@@ -67,7 +67,7 @@ static const FunctionProvider all_function_providers[] = {
 };
 
 MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, OutputModuleInterface &interface,
-			   SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up) :
+			   SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up, const uint8_t instance_start) :
 	ModuleParams(&interface),
 	_output_ramp_up(ramp_up),
 	_scheduling_policy(scheduling_policy),
@@ -87,7 +87,7 @@ MixingOutput::MixingOutput(const char *param_prefix, uint8_t max_num_outputs, Ou
 
 	px4_sem_init(&_lock, 0, 1);
 
-	initParamHandles();
+	initParamHandles(instance_start);
 
 	for (unsigned i = 0; i < MAX_ACTUATORS; i++) {
 		_failsafe_value[i] = UINT16_MAX;
@@ -108,20 +108,20 @@ MixingOutput::~MixingOutput()
 	_outputs_pub.unadvertise();
 }
 
-void MixingOutput::initParamHandles()
+void MixingOutput::initParamHandles(const uint8_t instance_start)
 {
 	char param_name[17];
 
 	for (unsigned i = 0; i < _max_num_outputs; ++i) {
-		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FUNC", i + 1);
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FUNC", i + instance_start);
 		_param_handles[i].function = param_find(param_name);
-		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "DIS", i + 1);
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "DIS", i + instance_start);
 		_param_handles[i].disarmed = param_find(param_name);
-		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MIN", i + 1);
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MIN", i + instance_start);
 		_param_handles[i].min = param_find(param_name);
-		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MAX", i + 1);
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MAX", i + instance_start);
 		_param_handles[i].max = param_find(param_name);
-		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FAIL", i + 1);
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FAIL", i + instance_start);
 		_param_handles[i].failsafe = param_find(param_name);
 	}
 

src/lib/mixer_module/mixer_module.hpp

@@ -120,7 +120,7 @@ public:
 	 */
 	MixingOutput(const char *param_prefix, uint8_t max_num_outputs, OutputModuleInterface &interface,
 		     SchedulingPolicy scheduling_policy,
-		     bool support_esc_calibration, bool ramp_up = true);
+		     bool support_esc_calibration, bool ramp_up = true, const uint8_t instance_start = 1);
 
 	~MixingOutput();
 
@@ -221,7 +221,7 @@ private:
 
 	void cleanupFunctions();
 
-	void initParamHandles();
+	void initParamHandles(const uint8_t instance_start);
 
 	void limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates);
 

src/lib/version/CMakeLists.txt

@@ -53,7 +53,8 @@ endif()
 
 set(px4_git_ver_header ${CMAKE_CURRENT_BINARY_DIR}/build_git_version.h)
 add_custom_command(OUTPUT ${px4_git_ver_header}
-	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py ${px4_git_ver_header} --validate
+	COMMAND
+		${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py ${px4_git_ver_header} --validate --git_tag '${PX4_GIT_TAG}'
 	DEPENDS
 		${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py
 		${git_dir_path}/HEAD

src/lib/version/px_update_git_header.py

@@ -10,6 +10,7 @@ parser = argparse.ArgumentParser(description="""Extract version info from git an
 generate a version header file. The working directory is expected to be
 the root of Firmware.""")
 parser.add_argument('filename', metavar='version.h', help='Header output file')
+parser.add_argument('--git_tag', help='git tag string')
 parser.add_argument('-v', '--verbose', dest='verbose', action='store_true',
                     help='Verbose output', default=False)
 parser.add_argument('--validate', dest='validate', action='store_true',
@@ -36,8 +37,11 @@ header = """
 
 
 # PX4
-git_describe_cmd = 'git describe --exclude ext/* --always --tags --dirty'
-git_tag = subprocess.check_output(git_describe_cmd.split(),
+if args.git_tag:
+    git_tag = args.git_tag
+else:
+    git_describe_cmd = 'git describe --exclude ext/* --always --tags --dirty'
+    git_tag = subprocess.check_output(git_describe_cmd.split(),
                                   stderr=subprocess.STDOUT).decode('utf-8').strip()
 
 try:
@@ -57,17 +61,7 @@ if validate:
     # now check the version format
     m = re.match(r'v([0-9]+)\.([0-9]+)\.[0-9]+(((-dev)|(-alpha[0-9]+)|(-beta[0-9]+)|(-rc[0-9]+))|'\
                  r'(-[0-9]+\.[0-9]+\.[0-9]+((-dev)|(-alpha[0-9]+)|(-beta[0-9]+)|([-]?rc[0-9]+))?))?$', git_tag_test)
-    if m:
-        # format matches, check the major and minor numbers
-        major = int(m.group(1))
-        minor = int(m.group(2))
-        if major < 1 or (major == 1 and minor < 9):
-            print("")
-            print("Error: PX4 version too low, expected at least v1.9.0")
-            print("Check the git tag (current tag: '{:}')".format(git_tag_test))
-            print("")
-            sys.exit(1)
-    else:
+    if not m:
         print("")
         print("Error: the git tag '{:}' does not match the expected format.".format(git_tag_test))
         print("")
@@ -103,9 +97,9 @@ except:
 if tag_or_branch is None:
     # replace / so it can be used as directory name
     tag_or_branch = git_branch_name.replace('/', '-')
-    # either a release or master branch (used for metadata)
+    # either a release or main branch (used for metadata)
     if not tag_or_branch.startswith('release-'):
-        tag_or_branch = 'master'
+        tag_or_branch = 'main'
 
 header += f"""
 #define PX4_GIT_VERSION_STR "{git_version}"
@@ -115,7 +109,7 @@ header += f"""
 
 #define PX4_GIT_OEM_VERSION_STR  "{oem_tag}"
 
-#define PX4_GIT_TAG_OR_BRANCH_NAME "{tag_or_branch}" // special variable: git tag, release or master branch
+#define PX4_GIT_TAG_OR_BRANCH_NAME "{tag_or_branch}" // special variable: git tag, release or main branch
 """
 
 

src/modules/commander/Commander.cpp

@@ -503,9 +503,9 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
 	switch (calling_reason) {
 	case arm_disarm_reason_t::transition_to_standby: return "";
 
-	case arm_disarm_reason_t::rc_stick: return "RC";
+	case arm_disarm_reason_t::stick_gesture: return "Stick gesture";
 
-	case arm_disarm_reason_t::rc_switch: return "RC (switch)";
+	case arm_disarm_reason_t::rc_switch: return "RC switch";
 
 	case arm_disarm_reason_t::command_internal: return "internal command";
 
@@ -583,7 +583,7 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 				return TRANSITION_DENIED;
 			}
 
-		} else if (calling_reason == arm_disarm_reason_t::rc_stick
+		} else if (calling_reason == arm_disarm_reason_t::stick_gesture
 			   || calling_reason == arm_disarm_reason_t::rc_switch
 			   || calling_reason == arm_disarm_reason_t::rc_button) {
 
@@ -634,12 +634,12 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 		const bool mc_manual_thrust_mode = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 						   && _vehicle_control_mode.flag_control_manual_enabled
 						   && !_vehicle_control_mode.flag_control_climb_rate_enabled;
-		const bool commanded_by_rc = (calling_reason == arm_disarm_reason_t::rc_stick)
+		const bool commanded_by_rc = (calling_reason == arm_disarm_reason_t::stick_gesture)
 					     || (calling_reason == arm_disarm_reason_t::rc_switch)
 					     || (calling_reason == arm_disarm_reason_t::rc_button);
 
 		if (!landed && !(mc_manual_thrust_mode && commanded_by_rc && _param_com_disarm_man.get())) {
-			if (calling_reason != arm_disarm_reason_t::rc_stick) {
+			if (calling_reason != arm_disarm_reason_t::stick_gesture) {
 				mavlink_log_critical(&_mavlink_log_pub, "Disarming denied: not landed\t");
 				events::send(events::ID("commander_disarm_denied_not_landed"),
 				{events::Log::Critical, events::LogInternal::Info},
@@ -698,11 +698,23 @@ Commander::Commander() :
 	// default for vtol is rotary wing
 	_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
 
-	_param_mav_comp_id = param_find("MAV_COMP_ID");
-	_param_mav_sys_id = param_find("MAV_SYS_ID");
+	param_t param_mav_comp_id = param_find("MAV_COMP_ID");
+	param_t param_mav_sys_id = param_find("MAV_SYS_ID");
 	_param_mav_type = param_find("MAV_TYPE");
 	_param_rc_map_fltmode = param_find("RC_MAP_FLTMODE");
 
+	int32_t value_int32 = 0;
+
+	// MAV_SYS_ID => vehicle_status.system_id
+	if ((param_mav_sys_id != PARAM_INVALID) && (param_get(param_mav_sys_id, &value_int32) == PX4_OK)) {
+		_vehicle_status.system_id = value_int32;
+	}
+
+	// MAV_COMP_ID => vehicle_status.component_id
+	if ((param_mav_comp_id != PARAM_INVALID) && (param_get(param_mav_comp_id, &value_int32) == PX4_OK)) {
+		_vehicle_status.component_id = value_int32;
+	}
+
 	updateParameters();
 }
 
@@ -875,7 +887,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			}
 
 			if (desired_nav_state != vehicle_status_s::NAVIGATION_STATE_MAX) {
-				if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) {
+
+				// Special handling for LAND mode: always allow to switch into it such that if used
+				// as emergency mode it is always available. When triggering it the user generally wants
+				// the vehicle to descend immediately, and if that means to switch to DESCEND it is fine.
+
+				const bool force = desired_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND;
+
+				if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd), false, force)) {
 					main_ret = TRANSITION_CHANGED;
 
 				} else {
@@ -1050,7 +1069,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
-			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd))) {
+			// Special handling for LAND mode: always allow to switch into it such that if used
+			// as emergency mode it is always available. When triggering it the user generally wants
+			// the vehicle to descend immediately, and if that means to switch to DESCEND it is fine.
+			const bool force = true;
+
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd), false,
+							force)) {
 				mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t");
 				events::send(events::ID("commander_landing_current_pos"), events::Log::Info,
 					     "Landing at current position");
@@ -1487,6 +1512,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE:
 	case vehicle_command_s::VEHICLE_CMD_DO_WINCH:
 	case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:
+	case vehicle_command_s::VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 
@@ -1603,7 +1629,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 
 	// Silently ignore RC actions during RC calibration
 	if (_vehicle_status.rc_calibration_in_progress
-	    && (action_request.source == action_request_s::SOURCE_RC_STICK_GESTURE
+	    && (action_request.source == action_request_s::SOURCE_STICK_GESTURE
 		|| action_request.source == action_request_s::SOURCE_RC_SWITCH
 		|| action_request.source == action_request_s::SOURCE_RC_BUTTON
 		|| action_request.source == action_request_s::SOURCE_RC_MODE_SLOT)) {
@@ -1611,7 +1637,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 	}
 
 	switch (action_request.source) {
-	case action_request_s::SOURCE_RC_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::rc_stick; break;
+	case action_request_s::SOURCE_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::stick_gesture; break;
 
 	case action_request_s::SOURCE_RC_SWITCH: arm_disarm_reason = arm_disarm_reason_t::rc_switch; break;
 
@@ -1682,22 +1708,11 @@ void Commander::updateParameters()
 
 	int32_t value_int32 = 0;
 
-	// MAV_SYS_ID => vehicle_status.system_id
-	if ((_param_mav_sys_id != PARAM_INVALID) && (param_get(_param_mav_sys_id, &value_int32) == PX4_OK)) {
-		_vehicle_status.system_id = value_int32;
-	}
-
-	// MAV_COMP_ID => vehicle_status.component_id
-	if ((_param_mav_comp_id != PARAM_INVALID) && (param_get(_param_mav_comp_id, &value_int32) == PX4_OK)) {
-		_vehicle_status.component_id = value_int32;
-	}
-
 	// MAV_TYPE -> vehicle_status.system_type
 	if ((_param_mav_type != PARAM_INVALID) && (param_get(_param_mav_type, &value_int32) == PX4_OK)) {
 		_vehicle_status.system_type = value_int32;
 	}
 
-
 	_vehicle_status.avoidance_system_required = _param_com_obs_avoid.get();
 
 	_auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s);

src/modules/commander/Commander.hpp

@@ -324,8 +324,6 @@ private:
 	perf_counter_t _preflight_check_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": preflight check")};
 
 	// optional parameters
-	param_t _param_mav_comp_id{PARAM_INVALID};
-	param_t _param_mav_sys_id{PARAM_INVALID};
 	param_t _param_mav_type{PARAM_INVALID};
 	param_t _param_rc_map_fltmode{PARAM_INVALID};
 

src/modules/commander/HealthAndArmingChecks/checks/distanceSensorChecks.cpp

@@ -48,7 +48,8 @@ void DistanceSensorChecks::checkAndReport(const Context &context, Report &report
 
 		if (exists) {
 			distance_sensor_s dist_sens;
-			valid = _distance_sensor_sub[instance].copy(&dist_sens) && hrt_elapsed_time(&dist_sens.timestamp) < 1_s;
+			valid = _distance_sensor_sub[instance].copy(&dist_sens) && ((hrt_elapsed_time(&dist_sens.timestamp) < 1_s)
+					|| (dist_sens.mode == distance_sensor_s::MODE_DISABLED));
 			reporter.setIsPresent(health_component_t::distance_sensor);
 		}
 

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

@@ -273,12 +273,27 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 		}
 
 		if (!context.isArmed() && ekf_gps_check_fail) {
-			NavModes required_groups_gps = required_groups;
-			events::Log log_level = events::Log::Error;
+			NavModes required_groups_gps;
+			events::Log log_level;
 
-			if (_param_com_arm_wo_gps.get()) {
+			switch (static_cast<GnssArmingCheck>(_param_com_arm_wo_gps.get())) {
+			default:
+
+			/* FALLTHROUGH */
+			case GnssArmingCheck::DenyArming:
+				required_groups_gps = required_groups;
+				log_level = events::Log::Error;
+				break;
+
+			case GnssArmingCheck::WarningOnly:
 				required_groups_gps = NavModes::None; // optional
 				log_level = events::Log::Warning;
+				break;
+
+			case GnssArmingCheck::Disabled:
+				required_groups_gps = NavModes::None;
+				log_level = events::Log::Disabled;
+				break;
 			}
 
 			// Only report the first failure to avoid spamming
@@ -438,11 +453,20 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 			}
 
 			if (message && reporter.mavlink_log_pub()) {
-				if (!_param_com_arm_wo_gps.get()) {
-					mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail");
+				switch (static_cast<GnssArmingCheck>(_param_com_arm_wo_gps.get())) {
+				default:
 
-				} else {
-					mavlink_log_critical(reporter.mavlink_log_pub(), message, "");
+				/* FALLTHROUGH */
+				case GnssArmingCheck::DenyArming:
+					mavlink_log_critical(reporter.mavlink_log_pub(), message, " Fail");
+					break;
+
+				case GnssArmingCheck::WarningOnly:
+					mavlink_log_warning(reporter.mavlink_log_pub(), message, "");
+					break;
+
+				case GnssArmingCheck::Disabled:
+					break;
 				}
 			}
 		}

src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

@@ -59,6 +59,12 @@ public:
 	void checkAndReport(const Context &context, Report &reporter) override;
 
 private:
+	enum class GnssArmingCheck : uint8_t {
+		DenyArming = 0,
+		WarningOnly = 1,
+		Disabled = 2
+	};
+
 	void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
 				  NavModes required_groups);
 	void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups);
@@ -108,7 +114,7 @@ private:
 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
 					(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
 					(ParamInt<px4::params::COM_ARM_MAG_STR>) _param_com_arm_mag_str,
-					(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
+					(ParamInt<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
 					(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
 					(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
 					(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,

src/modules/commander/commander_params.c

@@ -225,11 +225,14 @@ PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f);
 PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f);
 
 /**
- * Allow arming without GPS
+ * GPS preflight check
+ *
+ * Measures taken when a check defined by EKF2_GPS_CHECK is failing.
  *
  * @group Commander
- * @value 0 Require GPS lock to arm
- * @value 1 Allow arming without GPS
+ * @value 0 Deny arming
+ * @value 1 Warning only
+ * @value 2 Disabled
  */
 PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);
 
@@ -285,7 +288,7 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
  * @unit s
  * @min 0.0
  * @max 25.0
- * @decimal 3
+ * @decimal 1
  */
 PARAM_DEFINE_FLOAT(COM_FAIL_ACT_T, 5.f);
 
@@ -335,14 +338,14 @@ PARAM_DEFINE_INT32(COM_QC_ACT, 0);
  * The offboard loss failsafe will only be entered after a timeout,
  * set by COM_OF_LOSS_T in seconds.
  *
- * @value  0 Position mode
- * @value  1 Altitude mode
- * @value  2 Manual
- * @value  3 Return mode
- * @value  4 Land mode
- * @value  5 Hold mode
- * @value  6 Terminate
- * @value  7 Disarm
+ * @value 0 Position mode
+ * @value 1 Altitude mode
+ * @value 2 Stabilized
+ * @value 3 Return mode
+ * @value 4 Land mode
+ * @value 5 Hold mode
+ * @value 6 Terminate
+ * @value 7 Disarm
  * @group Commander
  */
 PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);
@@ -450,16 +453,12 @@ PARAM_DEFINE_FLOAT(COM_RC_STICK_OV, 30.0f);
 PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0);
 
 /**
- * Position control navigation loss response.
+ * Position mode navigation loss response
  *
- * This sets the flight mode that will be used if navigation accuracy is no longer adequate for position control.
+ * This sets the flight mode that will be used if navigation accuracy is no longer adequate for position control in manual Position mode.
  *
- * If Altitude/Manual is selected: assume use of remote control after fallback. Switch to Altitude mode if a height estimate is available, else switch to MANUAL.
- *
- * If Land/Descend is selected: assume no use of remote control after fallback. Switch to Land mode if a height estimate is available, else switch to Descend.
- *
- * @value 0 Altitude/Manual
- * @value 1 Land/Descend
+ * @value 0 Altitude mode
+ * @value 1 Land mode (descend)
  *
  * @group Commander
  */

src/modules/commander/failsafe/failsafe.cpp

@@ -60,6 +60,7 @@ FailsafeBase::ActionOptions Failsafe::fromNavDllOrRclActParam(int param_value)
 
 	case gcs_connection_loss_failsafe_mode::Land_mode:
 		options.action = Action::Land;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
 		break;
 
 	case gcs_connection_loss_failsafe_mode::Terminate:
@@ -113,6 +114,7 @@ FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value)
 
 	case geofence_violation_action::Land_mode:
 		options.action = Action::Land;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
 		break;
 
 	default:
@@ -290,7 +292,7 @@ FailsafeBase::Action Failsafe::fromOffboardLossActParam(int param_value, uint8_t
 		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
 		break;
 
-	case offboard_loss_failsafe_mode::Manual:
+	case offboard_loss_failsafe_mode::Stabilized:
 		action = Action::FallbackStab;
 		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
 		break;
@@ -355,6 +357,7 @@ FailsafeBase::ActionOptions Failsafe::fromHighWindLimitActParam(int param_value)
 
 	case command_after_high_wind_failsafe::Land_mode:
 		options.action = Action::Land;
+		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
 		break;
 
 	default:

src/modules/commander/failsafe/failsafe.h

@@ -70,7 +70,7 @@ private:
 	enum class offboard_loss_failsafe_mode : int32_t {
 		Position_mode = 0,
 		Altitude_mode = 1,
-		Manual = 2,
+		Stabilized = 2,
 		Return_mode = 3,
 		Land_mode = 4,
 		Hold_mode = 5,

src/modules/ekf2/CMakeLists.txt

@@ -111,6 +111,7 @@ if(EKF2_SYMFORCE_GEN AND (${PYTHON_SYMFORCE_EXIT_CODE} EQUAL 0))
 	)
 endif()
 
+set(EKF_MODULE_PARAMS)
 set(EKF_LIBS)
 set(EKF_SRCS)
 list(APPEND EKF_SRCS
@@ -135,24 +136,27 @@ list(APPEND EKF_SRCS
 
 if(CONFIG_EKF2_AIRSPEED)
 	list(APPEND EKF_SRCS EKF/aid_sources/airspeed/airspeed_fusion.cpp)
+	list(APPEND EKF_MODULE_PARAMS params_airspeed.yaml)
 endif()
 
 if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
 	list(APPEND EKF_SRCS EKF/aid_sources/aux_global_position/aux_global_position.cpp)
+	list(APPEND EKF_MODULE_PARAMS params_aux_global_position.yaml)
 endif()
 
 if(CONFIG_EKF2_AUXVEL)
 	list(APPEND EKF_SRCS EKF/aid_sources/auxvel/auxvel_fusion.cpp)
+	list(APPEND EKF_MODULE_PARAMS params_aux_velocity.yaml)
 endif()
 
 if(CONFIG_EKF2_BAROMETER)
-	list(APPEND EKF_SRCS
-		EKF/aid_sources/barometer/baro_height_control.cpp
-	)
+	list(APPEND EKF_SRCS EKF/aid_sources/barometer/baro_height_control.cpp)
+	list(APPEND EKF_MODULE_PARAMS params_barometer.yaml)
 endif()
 
 if(CONFIG_EKF2_DRAG_FUSION)
 	list(APPEND EKF_SRCS EKF/aid_sources/drag/drag_fusion.cpp)
+	list(APPEND EKF_MODULE_PARAMS params_drag.yaml)
 endif()
 
 if(CONFIG_EKF2_EXTERNAL_VISION)
@@ -163,6 +167,7 @@ if(CONFIG_EKF2_EXTERNAL_VISION)
 		EKF/aid_sources/external_vision/ev_vel_control.cpp
 		EKF/aid_sources/external_vision/ev_yaw_control.cpp
 	)
+	list(APPEND EKF_MODULE_PARAMS params_external_vision.yaml)
 endif()
 
 if(CONFIG_EKF2_GNSS)
@@ -177,10 +182,13 @@ if(CONFIG_EKF2_GNSS)
 	endif()
 
 	list(APPEND EKF_LIBS yaw_estimator)
+
+	list(APPEND EKF_MODULE_PARAMS params_gnss.yaml)
 endif()
 
 if(CONFIG_EKF2_GRAVITY_FUSION)
 	list(APPEND EKF_SRCS EKF/aid_sources/gravity/gravity_fusion.cpp)
+	list(APPEND EKF_MODULE_PARAMS params_gravity.yaml)
 endif()
 
 if(CONFIG_EKF2_MAGNETOMETER)
@@ -188,6 +196,7 @@ if(CONFIG_EKF2_MAGNETOMETER)
 		EKF/aid_sources/magnetometer/mag_control.cpp
 		EKF/aid_sources/magnetometer/mag_fusion.cpp
 	)
+	list(APPEND EKF_MODULE_PARAMS params_magnetometer.yaml)
 endif()
 
 if(CONFIG_EKF2_OPTICAL_FLOW)
@@ -195,6 +204,7 @@ if(CONFIG_EKF2_OPTICAL_FLOW)
 		EKF/aid_sources/optical_flow/optical_flow_control.cpp
 		EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
 	)
+	list(APPEND EKF_MODULE_PARAMS params_optical_flow.yaml)
 endif()
 
 if(CONFIG_EKF2_RANGE_FINDER)
@@ -204,18 +214,22 @@ if(CONFIG_EKF2_RANGE_FINDER)
 		EKF/aid_sources/range_finder/range_height_fusion.cpp
 		EKF/aid_sources/range_finder/sensor_range_finder.cpp
 	)
+	list(APPEND EKF_MODULE_PARAMS params_range_finder.yaml)
 endif()
 
 if(CONFIG_EKF2_SIDESLIP)
 	list(APPEND EKF_SRCS EKF/aid_sources/sideslip/sideslip_fusion.cpp)
+	list(APPEND EKF_MODULE_PARAMS params_sideslip.yaml)
 endif()
 
 if(CONFIG_EKF2_TERRAIN)
 	list(APPEND EKF_SRCS EKF/terrain_control.cpp)
+	list(APPEND EKF_MODULE_PARAMS params_terrain.yaml)
 endif()
 
 if(CONFIG_EKF2_WIND)
 	list(APPEND EKF_SRCS EKF/wind.cpp)
+	list(APPEND EKF_MODULE_PARAMS params_wind.yaml)
 endif ()
 
 add_subdirectory(EKF)
@@ -248,9 +262,12 @@ px4_add_module(
 
 	MODULE_CONFIG
 		module.yaml
+		params_gyro_bias.yaml
+		params_accel_bias.yaml
 		params_multi.yaml
 		params_volatile.yaml
 		params_selector.yaml
+		${EKF_MODULE_PARAMS}
 
 	DEPENDS
 		geo

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp

@@ -113,7 +113,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 
 				} else {
 					// Try to initialize using measurement
-					if (ekf.setEkfGlobalOrigin(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, sample.epv)) {
+					if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
+							sample.epv)) {
 						ekf.enableControlStatusAuxGpos();
 						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
 						_state = State::active;

src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp

@@ -75,55 +75,20 @@ void Ekf::controlFakePosFusion()
 				      Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
 				      innov_gate);                                        // innovation gate
 
-		const bool enable_conditions_passing = !isHorizontalAidingActive()
-						       && ((getTiltVariance() > sq(math::radians(3.f))) || _control_status.flags.vehicle_at_rest)
-						       && (!(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector)) || _control_status.flags.vehicle_at_rest)
-						       && _horizontal_deadreckon_time_exceeded;
+		const bool enable_valid_fake_pos = _control_status.flags.constant_pos || _control_status.flags.vehicle_at_rest;
+		const bool enable_fake_pos = !enable_valid_fake_pos
+					     && (getTiltVariance() > sq(math::radians(3.f)))
+					     && !(_params.imu_ctrl & static_cast<int32_t>(ImuCtrl::GravityVector))
+					     && _horizontal_deadreckon_time_exceeded;
 
-		if (_control_status.flags.fake_pos) {
-			if (enable_conditions_passing) {
+		_control_status.flags.fake_pos = runFakePosStateMachine(enable_fake_pos, _control_status.flags.fake_pos, aid_src);
+		_control_status.flags.valid_fake_pos = runFakePosStateMachine(enable_valid_fake_pos,
+						       _control_status.flags.valid_fake_pos, aid_src);
 
-				// always protect against extreme values that could result in a NaN
-				if ((aid_src.test_ratio[0] < sq(100.0f / innov_gate))
-				    && (aid_src.test_ratio[1] < sq(100.0f / innov_gate))
-				   ) {
-					if (!aid_src.innovation_rejected
-					    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
-									  State::pos.idx + 0)
-					    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
-									  State::pos.idx + 1)
-					   ) {
-						aid_src.fused = true;
-						aid_src.time_last_fuse = _time_delayed_us;
-					}
-				}
-
-				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);
-
-				if (is_fusion_failing) {
-					ECL_WARN("fake position fusion failing, resetting");
-					resetFakePosFusion();
-				}
-
-			} else {
-				stopFakePosFusion();
-			}
-
-		} else {
-			if (enable_conditions_passing) {
-				ECL_INFO("start fake position fusion");
-				_control_status.flags.fake_pos = true;
-				resetFakePosFusion();
-
-				if (_control_status.flags.tilt_align) {
-					// The fake position fusion is not started for initial alignement
-					ECL_WARN("stopping navigation");
-				}
-			}
-		}
-
-	} else if (_control_status.flags.fake_pos && isHorizontalAidingActive()) {
-		stopFakePosFusion();
+	} else if ((_control_status.flags.fake_pos || _control_status.flags.valid_fake_pos) && isHorizontalAidingActive()) {
+		ECL_INFO("stop fake position fusion");
+		_control_status.flags.fake_pos = false;
+		_control_status.flags.valid_fake_pos = false;
 	}
 }
 
@@ -138,10 +103,41 @@ void Ekf::resetFakePosFusion()
 	_aid_src_fake_pos.time_last_fuse = _time_delayed_us;
 }
 
-void Ekf::stopFakePosFusion()
+bool Ekf::runFakePosStateMachine(const bool enable_conditions_passing, bool status_flag,
+				 estimator_aid_source2d_s &aid_src)
 {
-	if (_control_status.flags.fake_pos) {
-		ECL_INFO("stop fake position fusion");
-		_control_status.flags.fake_pos = false;
+	if (status_flag) {
+		if (enable_conditions_passing) {
+			if (!aid_src.innovation_rejected
+			    && fuseDirectStateMeasurement(aid_src.innovation[0], aid_src.innovation_variance[0], aid_src.observation_variance[0],
+							  State::pos.idx + 0)
+			    && fuseDirectStateMeasurement(aid_src.innovation[1], aid_src.innovation_variance[1], aid_src.observation_variance[1],
+							  State::pos.idx + 1)
+			   ) {
+				aid_src.fused = true;
+				aid_src.time_last_fuse = _time_delayed_us;
+			}
+
+			const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);
+
+			if (is_fusion_failing) {
+				ECL_WARN("fake position fusion failing, resetting");
+				resetFakePosFusion();
+			}
+
+		} else {
+			ECL_INFO("stop fake position fusion");
+			status_flag = false;
+		}
+
+	} else {
+		if (enable_conditions_passing) {
+			ECL_INFO("start fake position fusion");
+			status_flag = true;
+
+			resetFakePosFusion();
+		}
 	}
+
+	return status_flag;
 }

src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp

@@ -41,10 +41,6 @@
 
 #include "ekf.h"
 
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-# include <lib/world_magnetic_model/geo_mag_declination.h>
-#endif // CONFIG_EKF2_MAGNETOMETER
-
 #include <mathlib/mathlib.h>
 
 // GPS pre-flight check bit locations
@@ -63,46 +59,27 @@ void Ekf::collect_gps(const gnssSample &gps)
 {
 	if (_filter_initialised && !_NED_origin_initialised && _gps_checks_passed) {
 		// If we have good GPS data set the origin's WGS-84 position to the last gps fix
-		const double lat = gps.lat;
-		const double lon = gps.lon;
-
 		if (!_pos_ref.isInitialized()) {
-			_pos_ref.initReference(lat, lon, gps.time_us);
-
-			// if we are already doing aiding, correct for the change in position since the EKF started navigating
-			if (isHorizontalAidingActive()) {
-				double est_lat;
-				double est_lon;
-				_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
-				_pos_ref.initReference(est_lat, est_lon, gps.time_us);
-			}
+			setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc);
 		}
 
 		// Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin
 		if (!PX4_ISFINITE(_gps_alt_ref)) {
-			_gps_alt_ref = gps.alt + _state.pos(2);
+			setAltOriginFromCurrentPos(gps.alt, gps.vacc);
 		}
 
-		_NED_origin_initialised = true;
-
-		// save the horizontal and vertical position uncertainty of the origin
-		_gpos_origin_eph = gps.hacc;
-		_gpos_origin_epv = gps.vacc;
-
 		_information_events.flags.gps_checks_passed = true;
-		ECL_INFO("GPS checks passed");
-	}
 
-	if ((isTimedOut(_wmm_gps_time_last_checked, 1e6)) || (_wmm_gps_time_last_set == 0)) {
-		// a rough 2D fix is sufficient to lookup declination
+		ECL_INFO("GPS origin set to lat=%.6f, lon=%.6f",
+			 _pos_ref.getProjectionReferenceLat(), _pos_ref.getProjectionReferenceLon());
+
+	} else {
+		// a rough 2D fix is sufficient to lookup earth spin rate
 		const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000);
 
 		if (gps_rough_2d_fix && (_gps_checks_passed || !_NED_origin_initialised)) {
-			updateWmm(gps.lat, gps.lon);
 			_earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat));
 		}
-
-		_wmm_gps_time_last_checked = _time_delayed_us;
 	}
 }
 

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -39,6 +39,8 @@
 #include "ekf.h"
 #include <mathlib/mathlib.h>
 
+#include <lib/world_magnetic_model/geo_mag_declination.h>
+
 #include <ekf_derivation/generated/compute_mag_innov_innov_var_and_hx.h>
 
 void Ekf::controlMagFusion(const imuSample &imu_sample)
@@ -66,7 +68,9 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 			_control_status.flags.mag_fault = false;
 
 			_state.mag_B.zero();
-			resetMagCov();
+			resetMagBiasCov();
+
+			stopMagFusion();
 
 			_mag_lpf.reset(mag_sample.mag);
 			_mag_counter = 1;
@@ -76,15 +80,43 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 			_mag_counter++;
 		}
 
+		// check for WMM update periodically or if global origin has changed
+		bool wmm_updated = false;
+
+		if (global_origin().isInitialized()) {
+
+			bool origin_newer_than_last_mag = (global_origin().getProjectionReferenceTimestamp() > aid_src.time_last_fuse);
+
+			if (global_origin_valid()
+			    && (origin_newer_than_last_mag || (local_position_is_valid() && isTimedOut(_wmm_mag_time_last_checked, 10e6)))
+			   ) {
+				// position of local NED origin in GPS / WGS84 frame
+				double latitude_deg;
+				double longitude_deg;
+				global_origin().reproject(_state.pos(0), _state.pos(1), latitude_deg, longitude_deg);
+
+				if (updateWorldMagneticModel(latitude_deg, longitude_deg)) {
+					wmm_updated = true;
+				}
+
+				_wmm_mag_time_last_checked = _time_delayed_us;
+
+			} else if (origin_newer_than_last_mag) {
+				// use global origin to update WMM
+				if (updateWorldMagneticModel(global_origin().getProjectionReferenceLat(),
+							     global_origin().getProjectionReferenceLon())
+				   ) {
+					wmm_updated = true;
+				}
+			}
+		}
+
 		// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
 		// this is useful if there is a lot of interference on the sensor measurement.
 		if (_params.synthesize_mag_z && (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
-		    && (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps))
+		    && (_wmm_earth_field_gauss.isAllFinite() && _wmm_earth_field_gauss.longerThan(0.f))
 		   ) {
-			const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
-							* Vector3f(_mag_strength_gps, 0, 0);
-
-			mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred);
+			mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, _wmm_earth_field_gauss);
 
 			_control_status.flags.synthetic_mag_z = true;
 
@@ -140,8 +172,6 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 
 		checkMagHeadingConsistency(mag_sample);
 
-		// WMM update can occur on the last epoch, just after mag fusion
-		const bool wmm_updated = (_wmm_gps_time_last_set >= aid_src.time_last_fuse);
 		const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
 
 
@@ -175,14 +205,14 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 
 		// if we are using 3-axis magnetometer fusion, but without external NE aiding,
 		// then the declination must be fused as an observation to prevent long term heading drift
-		const bool no_ne_aiding_or_pre_takeoff = !using_ne_aiding || !_control_status.flags.in_air;
-		_control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_pre_takeoff;
+		const bool no_ne_aiding_or_not_moving = !using_ne_aiding || _control_status.flags.vehicle_at_rest;
+		_control_status.flags.mag_dec = _control_status.flags.mag && no_ne_aiding_or_not_moving;
 
 		if (_control_status.flags.mag) {
 
 			if (continuing_conditions_passing && _control_status.flags.yaw_align) {
 
-				if (mag_sample.reset || checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) {
+				if (checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_not_moving)) {
 					ECL_INFO("reset to %s", AID_SRC_NAME);
 					resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
 					aid_src.time_last_fuse = imu_sample.time_us;
@@ -203,14 +233,35 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 					}
 
 					if (_control_status.flags.mag_dec) {
-						fuseDeclination(0.5f);
+
+						// observation variance (rad**2)
+						const float R_DECL = sq(0.5f);
+
+						if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
+						    && PX4_ISFINITE(_wmm_declination_rad)
+						   ) {
+							// using declination from the world magnetic model
+							fuseDeclination(_wmm_declination_rad, 0.5f, update_all_states);
+
+						} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
+							   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
+							  ) {
+							// using previously saved declination
+							fuseDeclination(math::radians(_params.mag_declination_deg), R_DECL, update_all_states);
+
+						} else {
+							// if there is no aiding coming from an inertial frame we need to fuse some declination
+							// even if we don't know the value, it's better to fuse 0 than nothing
+							float declination_rad = 0.f;
+							fuseDeclination(declination_rad, R_DECL);
+						}
 					}
 				}
 
 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);
 
 				if (is_fusion_failing) {
-					if (no_ne_aiding_or_pre_takeoff) {
+					if (no_ne_aiding_or_not_moving) {
 						ECL_WARN("%s fusion failing, resetting", AID_SRC_NAME);
 						resetMagStates(_mag_lpf.getState(), _control_status.flags.mag_hdg || _control_status.flags.mag_3D);
 						aid_src.time_last_fuse = imu_sample.time_us;
@@ -271,6 +322,9 @@ void Ekf::stopMagFusion()
 	if (_control_status.flags.mag) {
 		ECL_INFO("stopping mag fusion");
 
+		resetMagEarthCov();
+		resetMagBiasCov();
+
 		if (_control_status.flags.yaw_align && (_control_status.flags.mag_3D || _control_status.flags.mag_hdg)) {
 			// reset yaw alignment from mag unless using GNSS aiding
 			const bool using_ne_aiding = _control_status.flags.gps || _control_status.flags.aux_gpos;
@@ -329,29 +383,36 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 	const Vector3f mag_I_before_reset = _state.mag_I;
 	const Vector3f mag_B_before_reset = _state.mag_B;
 
-	// reset covariances to default
-	resetMagCov();
+	static constexpr float kMagEarthMinGauss = 0.01f; // minimum difference in mag earth field strength for reset (Gauss)
 
 	// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
-	if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
-
+	if (_wmm_earth_field_gauss.longerThan(0.f) && _wmm_earth_field_gauss.isAllFinite()) {
 		// use expected earth field to reset states
-		const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps))
-						* Vector3f(_mag_strength_gps, 0, 0);
 
-		// mag_B: reset
+		// mag_I: reset, skipped if negligible change in state
+		const Vector3f mag_I = _wmm_earth_field_gauss;
+		bool mag_I_reset = false;
+
+		if ((_state.mag_I - mag_I).longerThan(kMagEarthMinGauss)) {
+			_state.mag_I = mag_I;
+			resetMagEarthCov();
+			mag_I_reset = true;
+		}
+
+		// mag_B: reset, skipped if mag_I didn't change
 		if (!reset_heading && _control_status.flags.yaw_align) {
-			// mag_B: reset using WMM
-			const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
-			_state.mag_B = mag - (R_to_body * mag_earth_pred);
+			if (mag_I_reset) {
+				// mag_B: reset using WMM
+				const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
+				_state.mag_B = mag - (R_to_body * _wmm_earth_field_gauss);
+				resetMagBiasCov();
+			} // otherwise keep existing mag_B state (!mag_I_reset)
 
 		} else {
 			_state.mag_B.zero();
+			resetMagBiasCov();
 		}
 
-		// mag_I: reset, skipped if no change in state and variance good
-		_state.mag_I = mag_earth_pred;
-
 		if (reset_heading) {
 			resetMagHeading(mag);
 		}
@@ -359,34 +420,32 @@ void Ekf::resetMagStates(const Vector3f &mag, bool reset_heading)
 	} else {
 		// mag_B: reset
 		_state.mag_B.zero();
+		resetMagBiasCov();
 
-		// Use the magnetometer measurement to reset the field states
+		// Use the magnetometer measurement to reset the heading
 		if (reset_heading) {
 			resetMagHeading(mag);
 		}
 
-		// mag_I: use the last magnetometer measurements to reset the field states
-		_state.mag_I = _R_to_earth * mag;
+		// mag_I: use the last magnetometer measurement to reset the field states
+		const Vector3f mag_I = _R_to_earth * mag;
+
+		if ((_state.mag_I - mag_I).longerThan(kMagEarthMinGauss)) {
+			_state.mag_I = mag_I;
+			resetMagEarthCov();
+		}
 	}
 
-	if (!mag_I_before_reset.longerThan(0.f)) {
-		ECL_INFO("initializing mag I [%.3f, %.3f, %.3f], mag B [%.3f, %.3f, %.3f]",
-			 (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
-			 (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
-			);
-
-	} else {
+	if ((_state.mag_I - mag_I_before_reset).longerThan(0.f)) {
 		ECL_INFO("resetting mag I [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
 			 (double)mag_I_before_reset(0), (double)mag_I_before_reset(1), (double)mag_I_before_reset(2),
-			 (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2)
-			);
+			 (double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2));
+	}
 
-		if (mag_B_before_reset.longerThan(0.f) || _state.mag_B.longerThan(0.f)) {
-			ECL_INFO("resetting mag B [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
-				 (double)mag_B_before_reset(0), (double)mag_B_before_reset(1), (double)mag_B_before_reset(2),
-				 (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2)
-				);
-		}
+	if ((_state.mag_B - mag_B_before_reset).longerThan(0.f)) {
+		ECL_INFO("resetting mag B [%.3f, %.3f, %.3f] -> [%.3f, %.3f, %.3f]",
+			 (double)mag_B_before_reset(0), (double)mag_B_before_reset(1), (double)mag_B_before_reset(2),
+			 (double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2));
 	}
 
 	// record the start time for the magnetic field alignment
@@ -451,8 +510,8 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
 	_mag_strength = mag_sample.length();
 
 	if (_params.mag_check & static_cast<int32_t>(MagCheckMask::STRENGTH)) {
-		if (PX4_ISFINITE(_mag_strength_gps)) {
-			if (!isMeasuredMatchingExpected(_mag_strength, _mag_strength_gps, _params.mag_check_strength_tolerance_gs)) {
+		if (PX4_ISFINITE(_wmm_field_strength_gauss)) {
+			if (!isMeasuredMatchingExpected(_mag_strength, _wmm_field_strength_gauss, _params.mag_check_strength_tolerance_gs)) {
 				_control_status.flags.mag_field_disturbed = true;
 				is_check_failing = true;
 			}
@@ -475,9 +534,9 @@ bool Ekf::checkMagField(const Vector3f &mag_sample)
 	_mag_inclination = asinf(mag_earth(2) / fmaxf(mag_earth.norm(), 1e-4f));
 
 	if (_params.mag_check & static_cast<int32_t>(MagCheckMask::INCLINATION)) {
-		if (PX4_ISFINITE(_mag_inclination_gps)) {
+		if (PX4_ISFINITE(_wmm_inclination_rad)) {
 			const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
-			const float inc_error_rad = wrap_pi(_mag_inclination - _mag_inclination_gps);
+			const float inc_error_rad = wrap_pi(_mag_inclination - _wmm_inclination_rad);
 
 			if (fabsf(inc_error_rad) > inc_tol_rad) {
 				_control_status.flags.mag_field_disturbed = true;
@@ -549,13 +608,60 @@ float Ekf::getMagDeclination()
 		// Use value consistent with earth field state
 		return atan2f(_state.mag_I(1), _state.mag_I(0));
 
-	} else if (_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL) {
-		// use parameter value until GPS is available, then use value returned by geo library
-		if (PX4_ISFINITE(_mag_declination_gps)) {
-			return _mag_declination_gps;
+	} else if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
+		   && PX4_ISFINITE(_wmm_declination_rad)
+		  ) {
+		// if available use value returned by geo library
+		return _wmm_declination_rad;
+
+	} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
+		   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
+		  ) {
+		// using saved mag declination
+		return math::radians(_params.mag_declination_deg);
+	}
+
+	// otherwise unavailable
+	return 0.f;
+}
+
+bool Ekf::updateWorldMagneticModel(const double latitude_deg, const double longitude_deg)
+{
+	// set the magnetic field data returned by the geo library using the current GPS position
+	const float declination_rad = math::radians(get_mag_declination_degrees(latitude_deg, longitude_deg));
+	const float inclination_rad = math::radians(get_mag_inclination_degrees(latitude_deg, longitude_deg));
+	const float strength_gauss = get_mag_strength_gauss(latitude_deg, longitude_deg);
+
+	if (PX4_ISFINITE(declination_rad) && PX4_ISFINITE(inclination_rad) && PX4_ISFINITE(strength_gauss)) {
+
+		const bool declination_changed = (fabsf(declination_rad - _wmm_declination_rad) > math::radians(1.f));
+		const bool inclination_changed = (fabsf(inclination_rad - _wmm_inclination_rad) > math::radians(1.f));
+		const bool strength_changed = (fabsf(strength_gauss - _wmm_field_strength_gauss) > 0.01f);
+
+		if (!PX4_ISFINITE(_wmm_declination_rad)
+		    || !PX4_ISFINITE(_wmm_inclination_rad)
+		    || !PX4_ISFINITE(_wmm_field_strength_gauss)
+		    || !_wmm_earth_field_gauss.longerThan(0.f)
+		    || !_wmm_earth_field_gauss.isAllFinite()
+		    || declination_changed
+		    || inclination_changed
+		    || strength_changed
+		   ) {
+
+			ECL_DEBUG("WMM declination updated %.3f -> %.3f deg (lat=%.6f, lon=%.6f)",
+				  (double)math::degrees(_wmm_declination_rad), (double)math::degrees(declination_rad),
+				  (double)latitude_deg, (double)longitude_deg
+				 );
+
+			_wmm_declination_rad = declination_rad;
+			_wmm_inclination_rad = inclination_rad;
+			_wmm_field_strength_gauss = strength_gauss;
+
+			_wmm_earth_field_gauss = Dcmf(Eulerf(0, -inclination_rad, declination_rad)) * Vector3f(strength_gauss, 0, 0);
+
+			return true;
 		}
 	}
 
-	// otherwise use the parameter value
-	return math::radians(_params.mag_declination_deg);
+	return false;
 }

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_fusion.cpp

@@ -99,7 +99,8 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 				resetQuatCov(_params.mag_heading_noise);
 			}
 
-			resetMagCov();
+			resetMagEarthCov();
+			resetMagBiasCov();
 
 			return false;
 		}
@@ -150,51 +151,44 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 	return false;
 }
 
-bool Ekf::fuseDeclination(float decl_sigma)
+bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_states)
 {
-	float decl_measurement = NAN;
+	VectorState H;
+	float decl_pred;
+	float innovation_variance;
 
-	if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
-	    && PX4_ISFINITE(_mag_declination_gps)
-	   ) {
-		decl_measurement = _mag_declination_gps;
+	sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R, FLT_EPSILON,
+			&decl_pred, &innovation_variance, &H);
 
-	} else if ((_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)
-		   && PX4_ISFINITE(_params.mag_declination_deg) && (fabsf(_params.mag_declination_deg) > 0.f)
-		  ) {
-		decl_measurement = math::radians(_params.mag_declination_deg);
+	const float innovation = wrap_pi(decl_pred - decl_measurement_rad);
+
+	if (innovation_variance < R) {
+		// variance calculation is badly conditioned
+		_fault_status.flags.bad_mag_decl = true;
+		return false;
 	}
 
-	if (PX4_ISFINITE(decl_measurement)) {
+	// Calculate the Kalman gains
+	VectorState Kfusion = P * H / innovation_variance;
 
-		// observation variance (rad**2)
-		const float R_DECL = sq(decl_sigma);
+	if (!update_all_states) {
+		// zero non-mag Kalman gains if not updating all states
 
-		VectorState H;
-		float decl_pred;
-		float innovation_variance;
+		// copy mag_I and mag_B Kalman gains
+		const Vector3f K_mag_I = Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0);
+		const Vector3f K_mag_B = Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0);
 
-		sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R_DECL, FLT_EPSILON, &decl_pred, &innovation_variance,
-				&H);
-
-		const float innovation = wrap_pi(decl_pred - decl_measurement);
-
-		if (innovation_variance < R_DECL) {
-			// variance calculation is badly conditioned
-			return false;
-		}
-
-		// Calculate the Kalman gains
-		VectorState Kfusion = P * H / innovation_variance;
-
-		const bool is_fused = measurementUpdate(Kfusion, H, R_DECL, innovation);
-
-		_fault_status.flags.bad_mag_decl = !is_fused;
-
-		return is_fused;
+		// zero all Kalman gains, then restore mag
+		Kfusion.setZero();
+		Kfusion.slice<State::mag_I.dof, 1>(State::mag_I.idx, 0) = K_mag_I;
+		Kfusion.slice<State::mag_B.dof, 1>(State::mag_B.idx, 0) = K_mag_B;
 	}
 
-	return false;
+	const bool is_fused = measurementUpdate(Kfusion, H, R, innovation);
+
+	_fault_status.flags.bad_mag_decl = !is_fused;
+
+	return is_fused;
 }
 
 float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp

@@ -122,8 +122,16 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		const float range = predictFlowRange();
 		_flow_vel_body(0) = -flow_compensated(1) * range;
 		_flow_vel_body(1) =  flow_compensated(0) * range;
-		_flow_vel_ne = Vector2f(_R_to_earth * Vector3f(_flow_vel_body(0), _flow_vel_body(1), 0.f));
 
+		if (_flow_counter == 0) {
+			_flow_vel_body_lpf.reset(_flow_vel_body);
+			_flow_counter = 1;
+
+		} else {
+
+			_flow_vel_body_lpf.update(_flow_vel_body);
+			_flow_counter++;
+		}
 
 		// Check if we are in-air and require optical flow to control position drift
 		bool is_flow_required = _control_status.flags.in_air
@@ -144,6 +152,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				&& is_quality_good
 				&& is_magnitude_good
 				&& is_tilt_good
+				&& (_flow_counter > 10)
 				&& (isTerrainEstimateValid() || isHorizontalAidingActive())
 				&& isTimedOut(_aid_src_optical_flow.time_last_fuse, (uint64_t)2e6); // Prevent rapid switching
 
@@ -160,7 +169,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				// handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period
 				if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max)) {
 					if (is_flow_required && is_quality_good && is_magnitude_good) {
-						resetFlowFusion();
+						resetFlowFusion(flow_sample);
 
 						if (_control_status.flags.opt_flow_terrain && !isTerrainEstimateValid()) {
 							resetTerrainToFlow();
@@ -194,7 +203,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				} else {
 					if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) {
 						ECL_INFO("starting optical flow, resetting");
-						resetFlowFusion();
+						resetFlowFusion(flow_sample);
 						_control_status.flags.opt_flow = true;
 
 					} else if (_control_status.flags.opt_flow_terrain) {
@@ -213,11 +222,13 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 	}
 }
 
-void Ekf::resetFlowFusion()
+void Ekf::resetFlowFusion(const flowSample &flow_sample)
 {
 	ECL_INFO("reset velocity to flow");
 	_information_events.flags.reset_vel_to_flow = true;
-	resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));
+
+	const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(flow_sample);
+	resetHorizontalVelocityTo(getFilteredFlowVelNE(), flow_vel_var);
 
 	// reset position, estimate is relative to initial position in this mode, so we start with zero error
 	if (!_control_status.flags.in_air) {
@@ -271,6 +282,8 @@ void Ekf::stopFlowFusion()
 
 		_innov_check_fail_status.flags.reject_optflow_X = false;
 		_innov_check_fail_status.flags.reject_optflow_Y = false;
+
+		_flow_counter = 0;
 	}
 }
 

src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp

@@ -54,6 +54,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
 		_range_sensor.setPitchOffset(_params.rng_sens_pitch);
 		_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 		_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+		_range_sensor.setMaxFogDistance(_params.rng_fog);
 
 		_range_sensor.runChecks(imu_sample.time_us, _R_to_earth);
 

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.cpp

@@ -84,8 +84,9 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)
 
 			if (isTiltOk() && isDataInRange()) {
 				updateStuckCheck();
+				updateFogCheck(getDistBottom(), _sample.time_us);
 
-				if (!_is_stuck) {
+				if (!_is_stuck && !_is_blocked) {
 					_is_sample_valid = true;
 					_time_last_valid_us = _sample.time_us;
 				}
@@ -146,5 +147,23 @@ void SensorRangeFinder::updateStuckCheck()
 	}
 }
 
+void SensorRangeFinder::updateFogCheck(const float dist_bottom, const uint64_t time_us)
+{
+	if (_max_fog_dist > 0.f && time_us - _time_last_valid_us < 1e6) {
+
+		const float median_dist = _median_dist.apply(dist_bottom);
+		const float factor = 2.f; // magic hardcoded factor
+
+		if (!_is_blocked && median_dist < _max_fog_dist && _prev_median_dist - median_dist > factor * _max_fog_dist) {
+			_is_blocked = true;
+
+		} else if (_is_blocked && median_dist > factor * _max_fog_dist) {
+			_is_blocked = false;
+		}
+
+		_prev_median_dist = median_dist;
+	}
+}
+
 } // namespace sensor
 } // namespace estimator

src/modules/ekf2/EKF/aid_sources/range_finder/sensor_range_finder.hpp

@@ -44,6 +44,7 @@
 #include "Sensor.hpp"
 
 #include <matrix/math.hpp>
+#include <lib/mathlib/math/filter/MedianFilter.hpp>
 
 namespace estimator
 {
@@ -99,6 +100,8 @@ public:
 		_rng_valid_max_val = max_distance;
 	}
 
+	void setMaxFogDistance(float max_fog_dist) { _max_fog_dist = max_fog_dist; }
+
 	void setQualityHysteresis(float valid_quality_threshold_s)
 	{
 		_quality_hyst_us = uint64_t(valid_quality_threshold_s * 1e6f);
@@ -129,6 +132,7 @@ private:
 	bool isTiltOk() const { return _cos_tilt_rng_to_earth > _range_cos_max_tilt; }
 	bool isDataInRange() const;
 	void updateStuckCheck();
+	void updateFogCheck(const float dist_bottom, const uint64_t time_us);
 
 	rangeSample _sample{};
 
@@ -172,6 +176,14 @@ private:
 	 */
 	uint64_t _time_bad_quality_us{};	///< timestamp at which range finder signal quality was 0 (used for hysteresis)
 	uint64_t _quality_hyst_us{};		///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (us)
+
+	/*
+	 * Fog check
+	 */
+	bool _is_blocked{false};
+	float _max_fog_dist{0.f};	//< maximum distance at which the range finder could detect fog (m)
+	math::MedianFilter<float, 5> _median_dist{};
+	float _prev_median_dist{0.f};
 };
 
 } // namespace sensor

src/modules/ekf2/EKF/common.h

@@ -261,6 +261,7 @@ struct systemFlagUpdate {
 	bool in_air{true};
 	bool is_fixed_wing{false};
 	bool gnd_effect{false};
+	bool constant_pos{false};
 };
 
 struct parameters {
@@ -319,7 +320,6 @@ struct parameters {
 
 #if defined(CONFIG_EKF2_GNSS)
 	int32_t gnss_ctrl {static_cast<int32_t>(GnssCtrl::HPOS) | static_cast<int32_t>(GnssCtrl::VEL)};
-	float gps_delay_ms{110.0f};             ///< GPS measurement delay relative to the IMU (mSec)
 
 	Vector3f gps_pos_body{};                ///< xyz position of the GPS antenna in body frame (m)
 
@@ -420,6 +420,7 @@ struct parameters {
 	float range_valid_quality_s{1.0f};      ///< minimum duration during which the reported range finder signal quality needs to be non-zero in order to be declared valid (s)
 	float range_cos_max_tilt{0.7071f};      ///< cosine of the maximum tilt angle from the vertical that permits use of range finder and flow data
 	float range_kin_consistency_gate{1.0f}; ///< gate size used by the range finder kinematic consistency check
+	float rng_fog{0.f};                 	///< max distance which a blocked range sensor measures (fog, dirt) [m]
 
 	Vector3f rng_pos_body{};                ///< xyz position of range sensor in body frame (m)
 #endif // CONFIG_EKF2_RANGE_FINDER
@@ -512,7 +513,7 @@ bool bad_sideslip      :
 		1; ///< 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error
 		bool bad_optflow_X     : 1; ///< 7 - true if fusion of the optical flow X axis has encountered a numerical error
 		bool bad_optflow_Y     : 1; ///< 8 - true if fusion of the optical flow Y axis has encountered a numerical error
-		bool bad_acc_bias      : 1; ///< 9 - true if bad delta velocity bias estimates have been detected
+		bool __UNUSED          : 1; ///< 9 -
 		bool bad_acc_vertical  : 1; ///< 10 - true if bad vertical accelerometer data has been detected
 		bool bad_acc_clipping  : 1; ///< 11 - true if delta velocity data contains clipping (asymmetric railing)
 	} flags;
@@ -614,6 +615,8 @@ uint64_t mag_heading_consistent  :
 		uint64_t aux_gpos                : 1; ///< 38 - true if auxiliary global position measurement fusion is intended
 		uint64_t rng_terrain             : 1; ///< 39 - true if we are fusing range finder data for terrain
 		uint64_t opt_flow_terrain        : 1; ///< 40 - true if we are fusing flow data for terrain
+		uint64_t valid_fake_pos          : 1; ///< 41 - true if a valid constant position is being fused
+		uint64_t constant_pos            : 1; ///< 42 - true if the vehicle is at a constant position
 
 	} flags;
 	uint64_t value;

src/modules/ekf2/EKF/control.cpp

@@ -62,6 +62,8 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
 			if (system_flags_delayed.gnd_effect) {
 				set_gnd_effect();
 			}
+
+			set_constant_pos(system_flags_delayed.constant_pos);
 		}
 	}
 

src/modules/ekf2/EKF/covariance.cpp

@@ -96,7 +96,8 @@ void Ekf::initialiseCovariance()
 	resetAccelBiasCov();
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	resetMagCov();
+	resetMagEarthCov();
+	resetMagBiasCov();
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_WIND)
@@ -340,11 +341,17 @@ void Ekf::resetAccelBiasCov()
 }
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-void Ekf::resetMagCov()
+void Ekf::resetMagEarthCov()
 {
-	ECL_INFO("reset mag covariance");
+	ECL_INFO("reset mag earth covariance");
 
 	P.uncorrelateCovarianceSetVariance<State::mag_I.dof>(State::mag_I.idx, sq(_params.mag_noise));
+}
+
+void Ekf::resetMagBiasCov()
+{
+	ECL_INFO("reset mag bias covariance");
+
 	P.uncorrelateCovarianceSetVariance<State::mag_B.dof>(State::mag_B.idx, sq(_params.mag_noise));
 }
 #endif // CONFIG_EKF2_MAGNETOMETER

src/modules/ekf2/EKF/ekf.cpp

@@ -81,6 +81,7 @@ void Ekf::reset()
 	_range_sensor.setPitchOffset(_params.rng_sens_pitch);
 	_range_sensor.setCosMaxTilt(_params.range_cos_max_tilt);
 	_range_sensor.setQualityHysteresis(_params.range_valid_quality_s);
+	_range_sensor.setMaxFogDistance(_params.rng_fog);
 #endif // CONFIG_EKF2_RANGE_FINDER
 
 	_control_status.value = 0;
@@ -114,8 +115,6 @@ void Ekf::reset()
 
 	_last_known_pos.setZero();
 
-	_time_acc_bias_check = 0;
-
 #if defined(CONFIG_EKF2_BAROMETER)
 	_baro_counter = 0;
 #endif // CONFIG_EKF2_BAROMETER
@@ -276,68 +275,100 @@ void Ekf::predictState(const imuSample &imu_delayed)
 	_height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf;
 }
 
-bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy,
-		uint64_t timestamp_observation)
+bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const double longitude, const float altitude,
+		const float eph,
+		const float epv, uint64_t timestamp_observation)
 {
-	if (!_pos_ref.isInitialized()) {
-		ECL_WARN("unable to reset global position, position reference not initialized");
+	if (!checkLatLonValidity(latitude, longitude)) {
 		return false;
 	}
 
-	Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg);
+	if (!_pos_ref.isInitialized()) {
+		if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
+			return false;
+		}
+
+		if (!PX4_ISFINITE(_gps_alt_ref)) {
+			setAltOriginFromCurrentPos(altitude, epv);
+		}
+
+		return true;
+	}
+
+	Vector3f pos_correction;
 
 	// apply a first order correction using velocity at the delayed time horizon and the delta time
-	if ((timestamp_observation > 0) && (isHorizontalAidingActive() || !_horizontal_deadreckon_time_exceeded)) {
+	if ((timestamp_observation > 0) && local_position_is_valid()) {
 
 		timestamp_observation = math::min(_time_latest_us, timestamp_observation);
 
-		float diff_us = 0.f;
+		float dt_us;
 
 		if (_time_delayed_us >= timestamp_observation) {
-			diff_us = static_cast<float>(_time_delayed_us - timestamp_observation);
+			dt_us = static_cast<float>(_time_delayed_us - timestamp_observation);
 
 		} else {
-			diff_us = -static_cast<float>(timestamp_observation - _time_delayed_us);
+			dt_us = -static_cast<float>(timestamp_observation - _time_delayed_us);
 		}
 
-		const float dt_s = diff_us * 1e-6f;
-		pos_corrected += _state.vel.xy() * dt_s;
+		const float dt_s = dt_us * 1e-6f;
+		pos_correction = _state.vel * dt_s;
 	}
 
-	const float obs_var = math::max(sq(accuracy), sq(0.01f));
+	{
+		const Vector2f hpos = _pos_ref.project(latitude, longitude) + pos_correction.xy();
 
-	const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected;
-	const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
+		const float obs_var = math::max(sq(eph), sq(0.01f));
 
-	const float sq_gate = sq(5.f); // magic hardcoded gate
-	const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)),
-				  sq(innov(1)) / (sq_gate * innov_var(1))};
+		const Vector2f innov = Vector2f(_state.pos.xy()) - hpos;
+		const Vector2f innov_var = Vector2f(getStateVariance<State::pos>()) + obs_var;
 
-	const bool innov_rejected = (test_ratio.max() > 1.f);
+		const float sq_gate = sq(5.f); // magic hardcoded gate
+		const float test_ratio = sq(innov(0)) / (sq_gate * innov_var(0)) + sq(innov(1)) / (sq_gate * innov_var(1));
 
-	if (!_control_status.flags.in_air || (accuracy > 0.f && accuracy < 1.f) || innov_rejected) {
-		// when on ground or accuracy chosen to be very low, we hard reset position
-		// this allows the user to still send hard resets at any time
-		ECL_INFO("reset position to external observation");
-		_information_events.flags.reset_pos_to_ext_obs = true;
+		const bool innov_rejected = (test_ratio > 1.f);
 
-		resetHorizontalPositionTo(pos_corrected, obs_var);
-		_last_known_pos.xy() = _state.pos.xy();
-		return true;
+		if (!_control_status.flags.in_air || (eph > 0.f && eph < 1.f) || innov_rejected) {
+			// when on ground or accuracy chosen to be very low, we hard reset position
+			// this allows the user to still send hard resets at any time
+			ECL_INFO("reset position to external observation");
+			_information_events.flags.reset_pos_to_ext_obs = true;
 
-	} else {
-		if (fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0)
-		    && fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1)
-		   ) {
-			ECL_INFO("fused external observation as position measurement");
+			resetHorizontalPositionTo(hpos, obs_var);
+			_last_known_pos.xy() = _state.pos.xy();
+
+		} else {
+			ECL_INFO("fuse external observation as position measurement");
+			fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0);
+			fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1);
+
+			// Use the reset counters to inform the controllers about a potential large position jump
+			// TODO: compute the actual position change
 			_state_reset_status.reset_count.posNE++;
+			_state_reset_status.posNE_change.zero();
+
 			_time_last_hor_pos_fuse = _time_delayed_us;
 			_last_known_pos.xy() = _state.pos.xy();
-			return true;
 		}
 	}
 
-	return false;
+	if (checkAltitudeValidity(altitude)) {
+		const float altitude_corrected = altitude - pos_correction(2);
+
+		if (!PX4_ISFINITE(_gps_alt_ref)) {
+			setAltOriginFromCurrentPos(altitude_corrected, epv);
+
+		} else {
+			const float vpos = -(altitude_corrected - _gps_alt_ref);
+			const float obs_var = math::max(sq(epv), sq(0.01f));
+
+			ECL_INFO("reset height to external observation");
+			resetVerticalPositionTo(vpos, obs_var);
+			_last_known_pos(2) = _state.pos(2);
+		}
+	}
+
+	return true;
 }
 
 void Ekf::updateParameters()

src/modules/ekf2/EKF/ekf.h

@@ -122,7 +122,10 @@ public:
 	const auto &aid_src_optical_flow() const { return _aid_src_optical_flow; }
 
 	const Vector2f &getFlowVelBody() const { return _flow_vel_body; }
-	const Vector2f &getFlowVelNE() const { return _flow_vel_ne; }
+	Vector2f getFlowVelNE() const { return Vector2f(_R_to_earth * Vector3f(getFlowVelBody()(0), getFlowVelBody()(1), 0.f)); }
+
+	const Vector2f &getFilteredFlowVelBody() const { return _flow_vel_body_lpf.getState(); }
+	Vector2f getFilteredFlowVelNE() const { return Vector2f(_R_to_earth * Vector3f(getFilteredFlowVelBody()(0), getFilteredFlowVelBody()(1), 0.f)); }
 
 	const Vector2f &getFlowCompensated() const { return _flow_rate_compensated; }
 	const Vector2f &getFlowUncompensated() const { return _flow_sample_delayed.flow_rate; }
@@ -258,8 +261,11 @@ public:
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	// return true if the origin is valid
 	bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
-	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = 0.f, float epv = 0.f);
-	void updateWmm(double lat, double lon);
+	bool checkLatLonValidity(double latitude, double longitude);
+	bool checkAltitudeValidity(float altitude);
+	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN);
+	bool setEkfGlobalOriginFromCurrentPos(double latitude, double longitude, float altitude, float eph = NAN,
+					      float epv = NAN);
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
@@ -294,17 +300,17 @@ public:
 	// return true if the local position estimate is valid
 	bool local_position_is_valid() const
 	{
-		return (!_horizontal_deadreckon_time_exceeded && !_control_status.flags.fake_pos);
+		return !_horizontal_deadreckon_time_exceeded;
 	}
 
 	bool isLocalVerticalPositionValid() const
 	{
-		return !_vertical_position_deadreckon_time_exceeded && !_control_status.flags.fake_hgt;
+		return !_vertical_position_deadreckon_time_exceeded;
 	}
 
 	bool isLocalVerticalVelocityValid() const
 	{
-		return !_vertical_velocity_deadreckon_time_exceeded && !_control_status.flags.fake_hgt;
+		return !_vertical_velocity_deadreckon_time_exceeded;
 	}
 
 	bool isYawFinalAlignComplete() const
@@ -466,6 +472,9 @@ public:
 #endif // CONFIG_EKF2_GNSS
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
+	// set the magnetic field data returned by the geo library using position
+	bool updateWorldMagneticModel(const double latitude_deg, const double longitude_deg);
+
 	const auto &aid_src_mag() const { return _aid_src_mag; }
 #endif // CONFIG_EKF2_MAGNETOMETER
 
@@ -524,7 +533,7 @@ public:
 		return true;
 	}
 
-	bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy,
+	bool resetGlobalPosToExternalObservation(double latitude, double longitude, float altitude, float eph, float epv,
 			uint64_t timestamp_observation);
 
 	/**
@@ -603,8 +612,6 @@ private:
 
 	Vector3f _last_known_pos{};		///< last known local position vector (m)
 
-	uint64_t _time_acc_bias_check{0};	///< last time the  accel bias check passed (uSec)
-
 	Vector3f _earth_rate_NED{};	///< earth rotation vector (NED) in rad/s
 
 	Dcmf _R_to_earth{};	///< transformation matrix from body frame to earth frame from last EKF prediction
@@ -632,10 +639,12 @@ private:
 
 	// optical flow processing
 	Vector3f _flow_gyro_bias{};	///< bias errors in optical flow sensor rate gyro outputs (rad/sec)
-	Vector2f _flow_vel_body{};	///< velocity from corrected flow measurement (body frame)(m/s)
-	Vector2f _flow_vel_ne{};		///< velocity from corrected flow measurement (local frame) (m/s)
 	Vector3f _ref_body_rate{};
 
+	Vector2f _flow_vel_body{};                      ///< velocity from corrected flow measurement (body frame)(m/s)
+	AlphaFilter<Vector2f> _flow_vel_body_lpf{0.1f}; ///< filtered velocity from corrected flow measurement (body frame)(m/s)
+	uint32_t _flow_counter{0};                      ///< number of flow samples read for initialization
+
 	Vector2f _flow_rate_compensated{}; ///< measured angular rate of the image about the X and Y body axes after removal of body rotation (rad/s), RH rotation is positive
 #endif // CONFIG_EKF2_OPTICAL_FLOW
 
@@ -758,6 +767,12 @@ private:
 		P.slice<S.dof, S.dof>(S.idx, S.idx) = cov;
 	}
 
+	bool setLatLonOrigin(double latitude, double longitude, float eph = NAN);
+	bool setAltOrigin(float altitude, float epv = NAN);
+
+	bool setLatLonOriginFromCurrentPos(double latitude, double longitude, float eph = NAN);
+	bool setAltOriginFromCurrentPos(float altitude, float epv = NAN);
+
 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
 	bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW);
 	void computeYawInnovVarAndH(float variance, float &innovation_variance, VectorState &H_YAW) const;
@@ -770,8 +785,8 @@ private:
 		     bool update_all_states = false, bool update_tilt = false);
 
 	// fuse magnetometer declination measurement
-	// argument passed in is the declination uncertainty in radians
-	bool fuseDeclination(float decl_sigma);
+	//  R: declination observation variance (rad**2)
+	bool fuseDeclination(const float decl_measurement_rad, const float R, bool update_all_states = false);
 
 #endif // CONFIG_EKF2_MAGNETOMETER
 
@@ -871,7 +886,7 @@ private:
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 	// control fusion of optical flow observations
 	void controlOpticalFlowFusion(const imuSample &imu_delayed);
-	void resetFlowFusion();
+	void resetFlowFusion(const flowSample &flow_sample);
 	void stopFlowFusion();
 
 	void updateOnGroundMotionForOpticalFlowChecks();
@@ -1077,7 +1092,6 @@ private:
 	void stopAuxVelFusion();
 #endif // CONFIG_EKF2_AUXVEL
 
-	void checkVerticalAccelerationBias(const imuSample &imu_delayed);
 	void checkVerticalAccelerationHealth(const imuSample &imu_delayed);
 	Likelihood estimateInertialNavFallingLikelihood() const;
 
@@ -1106,7 +1120,8 @@ private:
 	void resetQuatCov(const Vector3f &rot_var_ned);
 
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	void resetMagCov();
+	void resetMagEarthCov();
+	void resetMagBiasCov();
 #endif // CONFIG_EKF2_MAGNETOMETER
 
 #if defined(CONFIG_EKF2_WIND)
@@ -1132,7 +1147,7 @@ private:
 	}
 
 	void resetFakePosFusion();
-	void stopFakePosFusion();
+	bool runFakePosStateMachine(bool enable_condition_passing, bool status_flag, estimator_aid_source2d_s &aid_src);
 
 	// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
 	// yaw : Euler yaw angle (rad)

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -72,97 +72,151 @@ bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &lo
 	return _NED_origin_initialised;
 }
 
+bool Ekf::checkLatLonValidity(const double latitude, const double longitude)
+{
+	const bool lat_valid = (PX4_ISFINITE(latitude) && (abs(latitude) <= 90));
+	const bool lon_valid = (PX4_ISFINITE(longitude) && (abs(longitude) <= 180));
+
+	return (lat_valid && lon_valid);
+}
+
+bool Ekf::checkAltitudeValidity(const float altitude)
+{
+	// sanity check valid altitude anywhere between the Mariana Trench and edge of Space
+	return (PX4_ISFINITE(altitude) && ((altitude > -12'000.f) && (altitude < 100'000.f)));
+}
+
 bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph,
 			     const float epv)
 {
-	// sanity check valid latitude/longitude and altitude anywhere between the Mariana Trench and edge of Space
-	if (PX4_ISFINITE(latitude) && (abs(latitude) <= 90)
-	    && PX4_ISFINITE(longitude) && (abs(longitude) <= 180)
-	    && PX4_ISFINITE(altitude) && (altitude > -12'000.f) && (altitude < 100'000.f)
-	   ) {
-		bool current_pos_available = false;
-		double current_lat = static_cast<double>(NAN);
-		double current_lon = static_cast<double>(NAN);
-
-		// if we are already doing aiding, correct for the change in position since the EKF started navigating
-		if (_pos_ref.isInitialized() && isHorizontalAidingActive()) {
-			_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
-			current_pos_available = true;
-		}
-
-		const float gps_alt_ref_prev = _gps_alt_ref;
-
-		// reinitialize map projection to latitude, longitude, altitude, and reset position
-		_pos_ref.initReference(latitude, longitude, _time_delayed_us);
-		_gps_alt_ref = altitude;
-
-		updateWmm(current_lat, current_lon);
-
-		_gpos_origin_eph = eph;
-		_gpos_origin_epv = epv;
-
-		_NED_origin_initialised = true;
-
-		if (current_pos_available) {
-			// reset horizontal position if we already have a global origin
-			Vector2f position = _pos_ref.project(current_lat, current_lon);
-			resetHorizontalPositionTo(position);
-		}
-
-		if (PX4_ISFINITE(gps_alt_ref_prev) && isVerticalPositionAidingActive()) {
-			// determine current z
-			const float z_prev = _state.pos(2);
-			const float current_alt = -z_prev + gps_alt_ref_prev;
-#if defined(CONFIG_EKF2_GNSS)
-			const float gps_hgt_bias = _gps_hgt_b_est.getBias();
-#endif // CONFIG_EKF2_GNSS
-			resetVerticalPositionTo(_gps_alt_ref - current_alt);
-			ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev,
-				  (double)_state.pos(2));
-#if defined(CONFIG_EKF2_GNSS)
-			// adjust existing GPS height bias
-			_gps_hgt_b_est.setBias(gps_hgt_bias);
-#endif // CONFIG_EKF2_GNSS
-		}
-
-		return true;
+	if (!setLatLonOrigin(latitude, longitude, eph)) {
+		return false;
 	}
 
-	return false;
+	// altitude is optional
+	setAltOrigin(altitude, epv);
+
+	return true;
 }
 
-void Ekf::updateWmm(const double lat, const double lon)
+bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph)
 {
-#if defined(CONFIG_EKF2_MAGNETOMETER)
-
-	// set the magnetic field data returned by the geo library using the current GPS position
-	const float mag_declination_gps = math::radians(get_mag_declination_degrees(lat, lon));
-	const float mag_inclination_gps = math::radians(get_mag_inclination_degrees(lat, lon));
-	const float mag_strength_gps = get_mag_strength_gauss(lat, lon);
-
-	if (PX4_ISFINITE(mag_declination_gps) && PX4_ISFINITE(mag_inclination_gps) && PX4_ISFINITE(mag_strength_gps)) {
-
-		const bool mag_declination_changed = (fabsf(mag_declination_gps - _mag_declination_gps) > math::radians(1.f));
-		const bool mag_inclination_changed = (fabsf(mag_inclination_gps - _mag_inclination_gps) > math::radians(1.f));
-
-		if ((_wmm_gps_time_last_set == 0)
-		    || !PX4_ISFINITE(_mag_declination_gps)
-		    || !PX4_ISFINITE(_mag_inclination_gps)
-		    || !PX4_ISFINITE(_mag_strength_gps)
-		    || mag_declination_changed
-		    || mag_inclination_changed
-		   ) {
-			_mag_declination_gps = mag_declination_gps;
-			_mag_inclination_gps = mag_inclination_gps;
-			_mag_strength_gps = mag_strength_gps;
-
-			_wmm_gps_time_last_set = _time_delayed_us;
-		}
+	if (!checkLatLonValidity(latitude, longitude)) {
+		return false;
 	}
 
-#endif // CONFIG_EKF2_MAGNETOMETER
+	bool current_pos_available = false;
+	double current_lat = static_cast<double>(NAN);
+	double current_lon = static_cast<double>(NAN);
+
+	// if we are already doing aiding, correct for the change in position since the EKF started navigating
+	if (_pos_ref.isInitialized() && local_position_is_valid()) {
+		_pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon);
+		current_pos_available = true;
+	}
+
+	// reinitialize map projection to latitude, longitude, altitude, and reset position
+	_pos_ref.initReference(latitude, longitude, _time_delayed_us);
+
+	if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
+		_gpos_origin_eph = eph;
+	}
+
+	_NED_origin_initialised = true;
+
+	if (current_pos_available) {
+		// reset horizontal position if we already have a global origin
+		Vector2f position = _pos_ref.project(current_lat, current_lon);
+		resetHorizontalPositionTo(position);
+	}
+
+	return true;
 }
 
+bool Ekf::setAltOrigin(const float altitude, const float epv)
+{
+	if (!checkAltitudeValidity(altitude)) {
+		return false;
+	}
+
+	const float gps_alt_ref_prev = _gps_alt_ref;
+	_gps_alt_ref = altitude;
+
+	if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
+		_gpos_origin_epv = epv;
+	}
+
+	if (PX4_ISFINITE(gps_alt_ref_prev) && isLocalVerticalPositionValid()) {
+		// determine current z
+		const float z_prev = _state.pos(2);
+		const float current_alt = -z_prev + gps_alt_ref_prev;
+#if defined(CONFIG_EKF2_GNSS)
+		const float gps_hgt_bias = _gps_hgt_b_est.getBias();
+#endif // CONFIG_EKF2_GNSS
+		resetVerticalPositionTo(_gps_alt_ref - current_alt);
+		ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev,
+			  (double)_state.pos(2));
+#if defined(CONFIG_EKF2_GNSS)
+		// adjust existing GPS height bias
+		_gps_hgt_b_est.setBias(gps_hgt_bias);
+#endif // CONFIG_EKF2_GNSS
+	}
+
+	return true;
+}
+
+bool Ekf::setEkfGlobalOriginFromCurrentPos(const double latitude, const double longitude, const float altitude,
+		const float eph, const float epv)
+{
+	if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) {
+		return false;
+	}
+
+	// altitude is optional
+	setAltOriginFromCurrentPos(altitude, epv);
+
+	return true;
+}
+
+bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double longitude, const float eph)
+{
+	if (!checkLatLonValidity(latitude, longitude)) {
+		return false;
+	}
+
+	_pos_ref.initReference(latitude, longitude, _time_delayed_us);
+
+	// if we are already doing aiding, correct for the change in position since the EKF started navigating
+	if (local_position_is_valid()) {
+		double est_lat;
+		double est_lon;
+		_pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon);
+		_pos_ref.initReference(est_lat, est_lon, _time_delayed_us);
+	}
+
+	if (PX4_ISFINITE(eph) && (eph >= 0.f)) {
+		_gpos_origin_eph = eph;
+	}
+
+	_NED_origin_initialised = true;
+
+	return true;
+}
+
+bool Ekf::setAltOriginFromCurrentPos(const float altitude, const float epv)
+{
+	if (!checkAltitudeValidity(altitude)) {
+		return false;
+	}
+
+	_gps_alt_ref = altitude + _state.pos(2);
+
+	if (PX4_ISFINITE(epv) && (epv >= 0.f)) {
+		_gpos_origin_epv = epv;
+	}
+
+	return true;
+}
 
 void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const
 {
@@ -642,7 +696,8 @@ uint16_t Ekf::get_ekf_soln_status() const
 #endif // CONFIG_EKF2_TERRAIN
 
 	// 128	ESTIMATOR_CONST_POS_MODE	True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow)
-	soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.vehicle_at_rest;
+	soln_status.flags.const_pos_mode = _control_status.flags.fake_pos || _control_status.flags.valid_fake_pos
+					   || _control_status.flags.vehicle_at_rest;
 
 	// 256	ESTIMATOR_PRED_POS_HORIZ_REL	True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate
 	soln_status.flags.pred_pos_horiz_rel = isHorizontalAidingActive();
@@ -793,6 +848,13 @@ void Ekf::updateHorizontalDeadReckoningstatus()
 		}
 	}
 
+	if (_control_status.flags.valid_fake_pos && isRecent(_aid_src_fake_pos.time_last_fuse, _params.no_aid_timeout_max)) {
+		// only respect as a valid aiding source now if we expect to have another valid source once in air
+		if (aiding_expected_in_air) {
+			inertial_dead_reckoning = false;
+		}
+	}
+
 	if (inertial_dead_reckoning) {
 		if (isTimedOut(_time_last_horizontal_aiding, (uint64_t)_params.valid_timeout_max)) {
 			// deadreckon time exceeded
@@ -890,17 +952,21 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
 	// inhibit learning of imu accel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
 	// xy accel bias learning is also disabled on ground as those states are poorly observable when perpendicular to the gravity vector
 	{
+		const Vector3f gyro_corrected = imu_delayed.delta_ang / imu_delayed.delta_ang_dt - _state.gyro_bias;
+
 		const float alpha = math::constrain((imu_delayed.delta_ang_dt / _params.acc_bias_learn_tc), 0.f, 1.f);
 		const float beta = 1.f - alpha;
-		_ang_rate_magnitude_filt = fmaxf(imu_delayed.delta_ang.norm() / imu_delayed.delta_ang_dt,
-						 beta * _ang_rate_magnitude_filt);
+
+		_ang_rate_magnitude_filt = fmaxf(gyro_corrected.norm(), beta * _ang_rate_magnitude_filt);
 	}
 
 	{
+		const Vector3f accel_corrected = imu_delayed.delta_vel / imu_delayed.delta_vel_dt - _state.accel_bias;
+
 		const float alpha = math::constrain((imu_delayed.delta_vel_dt / _params.acc_bias_learn_tc), 0.f, 1.f);
 		const float beta = 1.f - alpha;
 
-		_accel_magnitude_filt = fmaxf(imu_delayed.delta_vel.norm() / imu_delayed.delta_vel_dt, beta * _accel_magnitude_filt);
+		_accel_magnitude_filt = fmaxf(accel_corrected.norm(), beta * _accel_magnitude_filt);
 	}
 
 

src/modules/ekf2/EKF/estimator_interface.cpp

@@ -178,7 +178,6 @@ void EstimatorInterface::setGpsData(const gnssSample &gnss_sample)
 	}
 
 	const int64_t time_us = gnss_sample.time_us
-				- static_cast<int64_t>(_params.gps_delay_ms * 1000)
 				- static_cast<int64_t>(_dt_ekf_avg * 5e5f); // seconds to microseconds divided by 2
 
 	if (time_us >= static_cast<int64_t>(_gps_buffer->get_newest().time_us + _min_obs_interval_us)) {

src/modules/ekf2/EKF/estimator_interface.h

@@ -179,6 +179,8 @@ public:
 		_control_status.flags.vehicle_at_rest = at_rest;
 	}
 
+	void set_constant_pos(bool constant_pos) { _control_status.flags.constant_pos = constant_pos; }
+
 	// return true if the attitude is usable
 	bool attitude_valid() const { return _control_status.flags.tilt_align; }
 
@@ -248,7 +250,7 @@ public:
 	bool get_mag_decl_deg(float &val) const
 	{
 		if (_NED_origin_initialised && (_params.mag_declination_source & GeoDeclinationMask::SAVE_GEO_DECL)) {
-			val = math::degrees(_mag_declination_gps);
+			val = math::degrees(_wmm_declination_rad);
 			return true;
 
 		} else {
@@ -259,7 +261,7 @@ public:
 	bool get_mag_inc_deg(float &val) const
 	{
 		if (_NED_origin_initialised) {
-			val = math::degrees(_mag_inclination_gps);
+			val = math::degrees(_wmm_inclination_rad);
 			return true;
 
 		} else {
@@ -270,9 +272,9 @@ public:
 	void get_mag_checks(float &inc_deg, float &inc_ref_deg, float &strength_gs, float &strength_ref_gs) const
 	{
 		inc_deg = math::degrees(_mag_inclination);
-		inc_ref_deg = math::degrees(_mag_inclination_gps);
+		inc_ref_deg = math::degrees(_wmm_inclination_rad);
 		strength_gs = _mag_strength;
-		strength_ref_gs = _mag_strength_gps;
+		strength_ref_gs = _wmm_field_strength_gauss;
 	}
 #endif // CONFIG_EKF2_MAGNETOMETER
 
@@ -449,13 +451,14 @@ protected:
 	// allocate data buffers and initialize interface variables
 	bool initialise_interface(uint64_t timestamp);
 
-	uint64_t _wmm_gps_time_last_checked{0};  // time WMM last checked
-	uint64_t _wmm_gps_time_last_set{0};      // time WMM last set
-
 #if defined(CONFIG_EKF2_MAGNETOMETER)
-	float _mag_declination_gps {NAN};        // magnetic declination returned by the geo library using the last valid GPS position (rad)
-	float _mag_inclination_gps{NAN};	  // magnetic inclination returned by the geo library using the last valid GPS position (rad)
-	float _mag_strength_gps{NAN};	          // magnetic strength returned by the geo library using the last valid GPS position (T)
+	uint64_t _wmm_mag_time_last_checked {0}; // time WMM update last checked by mag control
+
+	float _wmm_declination_rad{NAN};        // magnetic declination returned by the geo library using the last valid GPS position (rad)
+	float _wmm_inclination_rad{NAN};        // magnetic inclination returned by the geo library using the last valid GPS position (rad)
+	float _wmm_field_strength_gauss{NAN};   // magnetic strength returned by the geo library using the last valid GPS position (Gauss)
+
+	Vector3f _wmm_earth_field_gauss{};      // expected magnetic field vector from the last valid GPS position (Gauss)
 
 	float _mag_inclination{NAN};
 	float _mag_strength{NAN};

src/modules/ekf2/EKF/height_control.cpp

@@ -39,7 +39,6 @@
 
 void Ekf::controlHeightFusion(const imuSample &imu_delayed)
 {
-	checkVerticalAccelerationBias(imu_delayed);
 	checkVerticalAccelerationHealth(imu_delayed);
 
 #if defined(CONFIG_EKF2_BAROMETER)
@@ -120,106 +119,6 @@ void Ekf::checkHeightSensorRefFallback()
 	}
 }
 
-void Ekf::checkVerticalAccelerationBias(const imuSample &imu_delayed)
-{
-	// Run additional checks to see if the delta velocity bias has hit limits in a direction that is clearly wrong
-	// calculate accel bias term aligned with the gravity vector
-	const float dVel_bias_lim = 0.9f * _params.acc_bias_lim * _dt_ekf_avg;
-	const Vector3f delta_vel_bias = _state.accel_bias * _dt_ekf_avg;
-	const float down_dvel_bias = delta_vel_bias.dot(Vector3f(_R_to_earth.row(2)));
-
-	// check that the vertical component of accel bias is consistent with both the vertical position and velocity innovation
-	bool bad_acc_bias = false;
-
-	if (fabsf(down_dvel_bias) > dVel_bias_lim) {
-
-		bool bad_vz = false;
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-
-		if (_control_status.flags.ev_hgt) {
-			if (down_dvel_bias * _aid_src_ev_vel.innovation[2] < 0.f) {
-				bad_vz = true;
-			}
-		}
-
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
-#if defined(CONFIG_EKF2_GNSS)
-
-		if (_control_status.flags.gps) {
-			if (down_dvel_bias * _aid_src_gnss_vel.innovation[2] < 0.f) {
-				bad_vz = true;
-			}
-		}
-
-#endif // CONFIG_EKF2_GNSS
-
-		if (bad_vz) {
-#if defined(CONFIG_EKF2_BAROMETER)
-
-			if (_control_status.flags.baro_hgt) {
-				if (down_dvel_bias * _aid_src_baro_hgt.innovation < 0.f) {
-					bad_acc_bias = true;
-				}
-			}
-
-#endif // CONFIG_EKF2_BAROMETER
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-
-			if (_control_status.flags.ev_hgt) {
-				if (down_dvel_bias * _aid_src_ev_hgt.innovation < 0.f) {
-					bad_acc_bias = true;
-				}
-			}
-
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
-#if defined(CONFIG_EKF2_GNSS)
-
-			if (_control_status.flags.gps_hgt) {
-				if (down_dvel_bias * _aid_src_gnss_hgt.innovation < 0.f) {
-					bad_acc_bias = true;
-				}
-			}
-
-#endif // CONFIG_EKF2_GNSS
-
-#if defined(CONFIG_EKF2_RANGE_FINDER)
-
-			if (_control_status.flags.rng_hgt) {
-				if (down_dvel_bias * _aid_src_rng_hgt.innovation < 0.f) {
-					bad_acc_bias = true;
-				}
-			}
-
-#endif // CONFIG_EKF2_RANGE_FINDER
-		}
-	}
-
-	// record the pass/fail
-	if (!bad_acc_bias) {
-		_fault_status.flags.bad_acc_bias = false;
-		_time_acc_bias_check = _time_delayed_us;
-
-	} else {
-		_fault_status.flags.bad_acc_bias = true;
-	}
-
-	// if we have failed for 7 seconds continuously, reset the accel bias covariances to fix bad conditioning of
-	// the covariance matrix but preserve the variances (diagonals) to allow bias learning to continue
-	if (_fault_status.flags.bad_acc_bias && isTimedOut(_time_acc_bias_check, (uint64_t)7e6)) {
-
-		resetAccelBiasCov();
-
-		_time_acc_bias_check = imu_delayed.time_us;
-
-		_fault_status.flags.bad_acc_bias = false;
-		ECL_WARN("invalid accel bias - covariance reset");
-	}
-}
-
 void Ekf::checkVerticalAccelerationHealth(const imuSample &imu_delayed)
 {
 	// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical

src/modules/ekf2/EKF/velocity_fusion.cpp

@@ -91,6 +91,9 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel, const Vector2f
 		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 1, math::max(sq(0.01f), new_horz_vel_var(1)));
 	}
 
+	P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, P(State::pos.idx, State::pos.idx));
+	P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, P(State::pos.idx + 1, State::pos.idx + 1));
+
 	_output_predictor.resetHorizontalVelocityTo(delta_horz_vel);
 
 	// record the state change
@@ -117,6 +120,8 @@ void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
 		P.uncorrelateCovarianceSetVariance<1>(State::vel.idx + 2, math::max(sq(0.01f), new_vert_vel_var));
 	}
 
+	P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, P(State::pos.idx + 2, State::pos.idx + 2));
+
 	_output_predictor.resetVerticalVelocityTo(delta_vert_vel);
 
 	// record the state change

src/modules/ekf2/EKF2.cpp

@@ -79,7 +79,6 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_noaid_noise(_params->pos_noaid_noise),
 #if defined(CONFIG_EKF2_GNSS)
 	_param_ekf2_gps_ctrl(_params->gnss_ctrl),
-	_param_ekf2_gps_delay(_params->gps_delay_ms),
 	_param_ekf2_gps_pos_x(_params->gps_pos_body(0)),
 	_param_ekf2_gps_pos_y(_params->gps_pos_body(1)),
 	_param_ekf2_gps_pos_z(_params->gps_pos_body(2)),
@@ -162,6 +161,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_rng_a_igate(_params->range_aid_innov_gate),
 	_param_ekf2_rng_qlty_t(_params->range_valid_quality_s),
 	_param_ekf2_rng_k_gate(_params->range_kin_consistency_gate),
+	_param_ekf2_rng_fog(_params->rng_fog),
 	_param_ekf2_rng_pos_x(_params->rng_pos_body(0)),
 	_param_ekf2_rng_pos_y(_params->rng_pos_body(1)),
 	_param_ekf2_rng_pos_z(_params->rng_pos_body(2)),
@@ -545,9 +545,8 @@ void EKF2::Run()
 						accuracy = vehicle_command.param3;
 					}
 
-					// TODO add check for lat and long validity
-					if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6,
-							accuracy, timestamp_observation)
+					if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6, vehicle_command.param7,
+							accuracy, accuracy, timestamp_observation)
 					   ) {
 						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
@@ -903,14 +902,6 @@ void EKF2::VerifyParams()
 
 #endif // CONFIG_EKF2_RANGE_FINDER
 
-#if defined(CONFIG_EKF2_GNSS)
-
-	if (_param_ekf2_gps_delay.get() > delay_max) {
-		delay_max = _param_ekf2_gps_delay.get();
-	}
-
-#endif // CONFIG_EKF2_GNSS
-
 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
 
 	if (_param_ekf2_of_delay.get() > delay_max) {
@@ -1925,6 +1916,8 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.cs_aux_gpos                = _ekf.control_status_flags().aux_gpos;
 		status_flags.cs_rng_terrain    = _ekf.control_status_flags().rng_terrain;
 		status_flags.cs_opt_flow_terrain    = _ekf.control_status_flags().opt_flow_terrain;
+		status_flags.cs_valid_fake_pos      = _ekf.control_status_flags().valid_fake_pos;
+		status_flags.cs_constant_pos        = _ekf.control_status_flags().constant_pos;
 
 		status_flags.fault_status_changes     = _filter_fault_status_changes;
 		status_flags.fs_bad_mag_x             = _ekf.fault_status_flags().bad_mag_x;
@@ -1936,7 +1929,6 @@ void EKF2::PublishStatusFlags(const hrt_abstime &timestamp)
 		status_flags.fs_bad_sideslip          = _ekf.fault_status_flags().bad_sideslip;
 		status_flags.fs_bad_optflow_x         = _ekf.fault_status_flags().bad_optflow_X;
 		status_flags.fs_bad_optflow_y         = _ekf.fault_status_flags().bad_optflow_Y;
-		status_flags.fs_bad_acc_bias          = _ekf.fault_status_flags().bad_acc_bias;
 		status_flags.fs_bad_acc_vertical      = _ekf.fault_status_flags().bad_acc_vertical;
 		status_flags.fs_bad_acc_clipping      = _ekf.fault_status_flags().bad_acc_clipping;
 
@@ -2025,6 +2017,9 @@ void EKF2::PublishOpticalFlowVel(const hrt_abstime &timestamp)
 		_ekf.getFlowVelBody().copyTo(flow_vel.vel_body);
 		_ekf.getFlowVelNE().copyTo(flow_vel.vel_ne);
 
+		_ekf.getFilteredFlowVelBody().copyTo(flow_vel.vel_body_filtered);
+		_ekf.getFilteredFlowVelNE().copyTo(flow_vel.vel_ne_filtered);
+
 		_ekf.getFlowUncompensated().copyTo(flow_vel.flow_rate_uncompensated);
 		_ekf.getFlowCompensated().copyTo(flow_vel.flow_rate_compensated);
 
@@ -2425,7 +2420,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 		}
 
 		gnssSample gnss_sample{
-			.time_us = vehicle_gps_position.timestamp,
+			.time_us = vehicle_gps_position.timestamp_sample,
 			.lat = vehicle_gps_position.latitude_deg,
 			.lon = vehicle_gps_position.longitude_deg,
 			.alt = static_cast<float>(vehicle_gps_position.altitude_msl_m),
@@ -2592,6 +2587,15 @@ void EKF2::UpdateSystemFlagsSample(ekf2_timestamps_s &ekf2_timestamps)
 			flags.gnd_effect = vehicle_land_detected.in_ground_effect;
 		}
 
+		launch_detection_status_s launch_detection_status;
+
+		if (_launch_detection_status_sub.copy(&launch_detection_status)
+		    && (ekf2_timestamps.timestamp < launch_detection_status.timestamp + 3_s)) {
+
+			flags.constant_pos = (launch_detection_status.launch_detection_state ==
+					      launch_detection_status_s::STATE_WAITING_FOR_LAUNCH);
+		}
+
 		_ekf.setSystemFlagData(flags);
 	}
 }
@@ -2653,7 +2657,6 @@ void EKF2::UpdateAccelCalibration(const hrt_abstime &timestamp)
 	const bool bias_valid = (_param_ekf2_imu_ctrl.get() & static_cast<int32_t>(ImuCtrl::AccelBias))
 				&& _ekf.control_status_flags().tilt_align
 				&& (_ekf.fault_status().value == 0)
-				&& !_ekf.fault_status_flags().bad_acc_bias
 				&& !_ekf.fault_status_flags().bad_acc_clipping
 				&& !_ekf.fault_status_flags().bad_acc_vertical;
 

src/modules/ekf2/EKF2.hpp

@@ -72,6 +72,7 @@
 #include <uORB/topics/estimator_states.h>
 #include <uORB/topics/estimator_status.h>
 #include <uORB/topics/estimator_status_flags.h>
+#include <uORB/topics/launch_detection_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/sensor_selection.h>
@@ -388,6 +389,7 @@ private:
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
 	uORB::Subscription _status_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription _launch_detection_status_sub{ORB_ID(launch_detection_status)};
 
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
@@ -511,7 +513,6 @@ private:
 
 #if defined(CONFIG_EKF2_GNSS)
 		(ParamExtInt<px4::params::EKF2_GPS_CTRL>) _param_ekf2_gps_ctrl,
-		(ParamExtFloat<px4::params::EKF2_GPS_DELAY>) _param_ekf2_gps_delay,
 
 		(ParamExtFloat<px4::params::EKF2_GPS_POS_X>) _param_ekf2_gps_pos_x,
 		(ParamExtFloat<px4::params::EKF2_GPS_POS_Y>) _param_ekf2_gps_pos_y,
@@ -621,6 +622,7 @@ private:
 		(ParamExtFloat<px4::params::EKF2_RNG_A_IGATE>) _param_ekf2_rng_a_igate,
 		(ParamExtFloat<px4::params::EKF2_RNG_QLTY_T>) _param_ekf2_rng_qlty_t,
 		(ParamExtFloat<px4::params::EKF2_RNG_K_GATE>) _param_ekf2_rng_k_gate,
+		(ParamExtFloat<px4::params::EKF2_RNG_FOG>) _param_ekf2_rng_fog,
 		(ParamExtFloat<px4::params::EKF2_RNG_POS_X>) _param_ekf2_rng_pos_x,
 		(ParamExtFloat<px4::params::EKF2_RNG_POS_Y>) _param_ekf2_rng_pos_y,
 		(ParamExtFloat<px4::params::EKF2_RNG_POS_Z>) _param_ekf2_rng_pos_z,

src/modules/ekf2/params_accel_bias.yaml

@@ -0,0 +1,72 @@
+module_name: ekf2
+parameters:
+- group: EKF2
+  definitions:
+    EKF2_ABIAS_INIT:
+      description:
+        short: 1-sigma IMU accelerometer switch-on bias
+      type: float
+      default: 0.2
+      min: 0.0
+      max: 0.5
+      unit: m/s^2
+      reboot_required: true
+      decimal: 2
+    EKF2_ACC_B_NOISE:
+      description:
+        short: Process noise for IMU accelerometer bias prediction
+      type: float
+      default: 0.003
+      min: 0.0
+      max: 0.01
+      unit: m/s^3
+      decimal: 6
+    EKF2_ABL_LIM:
+      description:
+        short: Accelerometer bias learning limit
+        long: The ekf accel bias states will be limited to within a range equivalent
+          to +- of this value.
+      type: float
+      default: 0.4
+      min: 0.0
+      max: 0.8
+      unit: m/s^2
+      decimal: 2
+    EKF2_ABL_ACCLIM:
+      description:
+        short: Maximum IMU accel magnitude that allows IMU bias learning
+        long: If the magnitude of the IMU accelerometer vector exceeds this value,
+          the EKF accel bias state estimation will be inhibited. This reduces the
+          adverse effect of high manoeuvre accelerations and IMU nonlinerity and scale
+          factor errors on the accel bias estimates.
+      type: float
+      default: 25.0
+      min: 20.0
+      max: 200.0
+      unit: m/s^2
+      decimal: 1
+    EKF2_ABL_GYRLIM:
+      description:
+        short: Maximum IMU gyro angular rate magnitude that allows IMU bias learning
+        long: If the magnitude of the IMU angular rate vector exceeds this value,
+          the EKF accel bias state estimation will be inhibited. This reduces the
+          adverse effect of rapid rotation rates and associated errors on the accel
+          bias estimates.
+      type: float
+      default: 3.0
+      min: 2.0
+      max: 20.0
+      unit: rad/s
+      decimal: 1
+    EKF2_ABL_TAU:
+      description:
+        short: Accel bias learning inhibit time constant
+        long: The vector magnitude of angular rate and acceleration used to check
+          if learning should be inhibited has a peak hold filter applied to it with
+          an exponential decay. This parameter controls the time constant of the decay.
+      type: float
+      default: 0.5
+      min: 0.1
+      max: 1.0
+      unit: s
+      decimal: 2

src/modules/ekf2/params_airspeed.yaml

@@ -0,0 +1,45 @@
+module_name: ekf2
+parameters:
+- group: EKF2
+  definitions:
+    EKF2_ARSP_THR:
+      description:
+        short: Airspeed fusion threshold
+        long: Airspeed data is fused for wind estimation if above this threshold.
+          Set to 0 to disable airspeed fusion. For reliable wind estimation both sideslip
+          (see EKF2_FUSE_BETA) and airspeed fusion should be enabled. Only applies
+          to fixed-wing vehicles (or VTOLs in fixed-wing mode).
+      type: float
+      default: 0.0
+      min: 0.0
+      unit: m/s
+      decimal: 1
+    EKF2_ASP_DELAY:
+      description:
+        short: Airspeed measurement delay relative to IMU measurements
+      type: float
+      default: 100
+      min: 0
+      max: 300
+      unit: ms
+      reboot_required: true
+      decimal: 1
+    EKF2_TAS_GATE:
+      description:
+        short: Gate size for TAS fusion
+        long: Sets the number of standard deviations used by the innovation consistency
+          test.
+      type: float
+      default: 5.0
+      min: 1.0
+      unit: SD
+      decimal: 1
+    EKF2_EAS_NOISE:
+      description:
+        short: Measurement noise for airspeed fusion
+      type: float
+      default: 1.4
+      min: 0.5
+      max: 5.0
+      unit: m/s
+      decimal: 1

src/modules/ekf2/params_aux_global_position.yaml

@@ -0,0 +1,45 @@
+module_name: ekf2
+parameters:
+- group: EKF2
+  definitions:
+    EKF2_AGP_CTRL:
+      description:
+        short: Aux global position (AGP) sensor aiding
+        long: 'Set bits in the following positions to enable: 0 : Horizontal position
+          fusion 1 : Vertical position fusion'
+      type: bitmask
+      bit:
+        0: Horizontal position
+        1: Vertical position
+      default: 0
+      min: 0
+      max: 3
+    EKF2_AGP_DELAY:
+      description:
+        short: Aux global position estimator delay relative to IMU measurements
+      type: float
+      default: 0
+      min: 0
+      max: 300
+      unit: ms
+      reboot_required: true
+      decimal: 1
+    EKF2_AGP_NOISE:
+      description:
+        short: Measurement noise for aux global position measurements
+        long:  Used to lower bound or replace the uncertainty included in the message
+      type: float
+      default: 0.9
+      min: 0.01
+      unit: m
+      decimal: 2
+    EKF2_AGP_GATE:
+      description:
+        short: Gate size for aux global position fusion
+        long: Sets the number of standard deviations used by the innovation consistency
+          test.
+      type: float
+      default: 3.0
+      min: 1.0
+      unit: SD
+      decimal: 1

src/modules/ekf2/params_aux_velocity.yaml

@@ -0,0 +1,14 @@
+module_name: ekf2
+parameters:
+- group: EKF2
+  definitions:
+    EKF2_AVEL_DELAY:
+      description:
+        short: Auxiliary Velocity Estimate delay relative to IMU measurements
+      type: float
+      default: 5
+      min: 0
+      max: 300
+      unit: ms
+      reboot_required: true
+      decimal: 1