added full support for standard vtol in SIH

Signed-off-by: RomanBapst <bapstroman@gmail.com>
added full support for external autostart scripts
2026-06-11 00:40:05 +08:00 · 2024-11-12 11:16:44 +03:00 · 2024-11-12 11:16:22 +03:00 · 2024-11-12 11:16:22 +03:00 · 2024-09-18 11:17:37 +02:00 · 2024-09-17 10:55:00 -07:00
196 changed files with 6186 additions and 3396 deletions
@@ -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",
@@ -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) }}
@@ -88,7 +88,7 @@ jobs:
      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
+      uses: actions/upload-artifact@v4
      with:
        name: coredump
        path: px4.core
@@ -103,21 +103,21 @@ jobs:

    - name: Upload px4 binary
      if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
      with:
        name: binary
        path: build/px4_sitl_default/bin/px4

    - name: Store PX4 log
      if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
      with:
        name: px4_log
        path: ~/.ros/log/*/*.ulg

    - name: Store ROS log
      if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
      with:
        name: ros_log
        path: ~/.ros/**/rostest-*.log
@@ -83,7 +83,7 @@ jobs:
      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
+      uses: actions/upload-artifact@v4
      with:
        name: coredump
        path: px4.core
@@ -98,21 +98,21 @@ jobs:

    - name: Upload px4 binary
      if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
      with:
        name: binary
        path: build/px4_sitl_default/bin/px4

    - name: Store PX4 log
      if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
      with:
        name: px4_log
        path: ~/.ros/log/*/*.ulg

    - name: Store ROS log
      if: failure()
-      uses: actions/upload-artifact@v2
+      uses: actions/upload-artifact@v4
      with:
        name: ros_log
        path: ~/.ros/**/rostest-*.log
@@ -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
@@ -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
@@ -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})

@@ -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 \
@@ -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

@@ -14,15 +14,19 @@ 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_MAX_THR_YAW_R 5
+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_MAX_SPEED 5
+param set-default RD_MAX_THR_SPD 7
+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

@@ -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
@@ -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
@@ -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
@@ -43,6 +43,10 @@ param set-default CA_SV_CS1_TRQ_P 0.3
 param set-default CA_SV_CS1_TRQ_Y -0.3
 param set-default CA_SV_CS1_TYPE 6

+param set-default FW_AIRSPD_MAX 12
+param set-default FW_AIRSPD_MIN 7
+param set-default FW_AIRSPD_TRIM 10
+
 param set-default HIL_ACT_FUNC1 101
 param set-default HIL_ACT_FUNC2 102
 param set-default HIL_ACT_FUNC5 202
@@ -62,6 +66,8 @@ param set-default CBRK_SUPPLY_CHK 894281
 # - without safety switch
 param set-default CBRK_IO_SAFETY 22027

+param set-default SENS_DPRES_OFF 0.001
+
 param set SIH_T_MAX 2.0
 param set SIH_Q_MAX 0.0165
 param set SIH_MASS 0.2
@@ -75,3 +81,5 @@ param set SIH_L_ROLL 0.145

 # sih as tailsitter
 param set SIH_VEHICLE_TYPE 2
+
+param set-default VT_ARSP_TRANS 6
@@ -0,0 +1,99 @@
+#!/bin/sh
+#
+# @name SIH Tailsitter Duo
+#
+# @type Simulation
+# @class VTOL
+#
+# @output Motor1 motor right
+# @output Motor2 motor left
+# @output Servo1 elevon right
+# @output Servo2 elevon left
+#
+# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
+#
+# @board px4_fmu-v2 exclude
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set UAVCAN_ENABLE 0
+param set-default VT_B_TRANS_DUR 5
+param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_TYPE 2
+param set-default MPC_MAN_Y_MAX 60
+param set-default MC_PITCH_P 5
+
+param set-default CA_AIRFRAME 2
+param set-default CA_ROTOR_COUNT 5
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR0_PX 0.2
+param set-default CA_ROTOR0_PY 0.2
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR1_PX -0.2
+param set-default CA_ROTOR1_PY -0.2
+param set-default CA_ROTOR2_PX 0.2
+param set-default CA_ROTOR2_PY -0.2
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.2
+param set-default CA_ROTOR3_PY 0.2
+param set-default CA_ROTOR3_KM -0.05
+
+
+param set-default CA_ROTOR4_PX -0.3
+param set-default CA_ROTOR4_KM 0.05
+param set-default CA_ROTOR4_AX 1
+param set-default CA_ROTOR4_AZ 0
+
+param set-default CA_SV_CS_COUNT 3
+param set-default CA_SV_CS0_TRQ_R 0.5
+param set-default CA_SV_CS0_TYPE 2
+param set-default CA_SV_CS1_TRQ_P 1
+param set-default CA_SV_CS1_TYPE 3
+param set-default CA_SV_CS2_TRQ_Y 1
+
+param set HIL_ACT_REV 32
+
+param set-default FW_AIRSPD_MAX 12
+param set-default FW_AIRSPD_MIN 7
+param set-default FW_AIRSPD_TRIM 10
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC3 103
+param set-default HIL_ACT_FUNC4 104
+param set-default HIL_ACT_FUNC5 201
+param set-default HIL_ACT_FUNC6 202
+param set-default HIL_ACT_FUNC7 203
+param set-default HIL_ACT_FUNC8 105
+
+param set-default MAV_TYPE 22
+
+
+
+# set SYS_HITL to 2 to start the SIH and avoid sensors startup
+param set-default SYS_HITL 2
+
+# disable some checks to allow to fly:
+# - without real battery
+param set-default CBRK_SUPPLY_CHK 894281
+# - without safety switch
+param set-default CBRK_IO_SAFETY 22027
+
+param set-default SENS_DPRES_OFF 0.001
+
+param set SIH_T_MAX 2.0
+param set SIH_Q_MAX 0.0165
+param set SIH_MASS 0.2
+# IXX and IZZ are inverted from the thesis as the body frame is pitched by 90 deg
+param set SIH_IXX 0.00354
+param set SIH_IYY 0.000625
+param set SIH_IZZ 0.00300
+param set SIH_IXZ 0
+param set SIH_KDV 0.2
+param set SIH_L_ROLL 0.145
+
+# sih as tailsitter
+param set SIH_VEHICLE_TYPE 3
+
+param set-default VT_ARSP_TRANS 6
@@ -20,3 +20,27 @@ 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 10
+param set-default RD_MAX_THR_YAW_R 4
+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_MAX_SPEED 1.8
+param set-default RD_MAX_THR_SPD 2
+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
@@ -48,6 +48,7 @@ if(CONFIG_MODULES_SIMULATION_PWM_OUT_SIM)
 		1100_rc_quad_x_sih.hil
 		1101_rc_plane_sih.hil
 		1102_tailsitter_duo_sih.hil
+		1103_standard_vtol_sih.hil
 	)
 endif()

@@ -105,6 +105,13 @@ then
 		fi
 	fi

+  if [ -e /fs/microsd/new ]
+  then
+    echo "Updating external autostart files"
+    rm -r $SDCARD_EXT_PATH
+    mv /fs/microsd/new $SDCARD_EXT_PATH
+  fi
+
 	# Check for an update of the ext_autostart folder, and replace the old one with it
 	if [ -e /fs/microsd/ext_autostart_new ]
 	then
@@ -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"
@@ -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
@@ -39,7 +39,6 @@ CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
-CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
@@ -70,7 +69,6 @@ CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
-CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
@@ -79,7 +77,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 +88,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 +95,3 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
-CONFIG_EXAMPLES_FAKE_GPS=y
@@ -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
@@ -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
@@ -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
@@ -23,27 +23,27 @@ CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_HEATER=y
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16470=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y
-CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
-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_DRIVERS_IMU_INVENSENSE_ICM20602=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42670P=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=n
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=n
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=n
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_OSD_MSP_OSD=y
-CONFIG_DRIVERS_POWER_MONITOR_INA226=y
+CONFIG_DRIVERS_POWER_MONITOR_INA226=n
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
-CONFIG_DRIVERS_UAVCAN=y
+CONFIG_DRIVERS_UAVCAN=n
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
@@ -377,7 +377,6 @@
 *(.text._ZN19StickAccelerationXYC1EP12ModuleParams)
 *(.text.imxrt_epsubmit)
 *(.text._ZN15PositionControl6updateEf)
-*(.text._ZN3Ekf29checkVerticalAccelerationBiasERKN9estimator9imuSampleE)
 *(.text._ZN23MavlinkStreamScaledIMU24sendEv)
 *(.text._ZN5PX4IO10io_reg_getEhhPtj)
 *(.text.imxrt_dma_send)
@@ -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
@@ -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
@@ -73,6 +73,7 @@ set(msg_files
 	DebugVect.msg
 	DifferentialPressure.msg
 	DistanceSensor.msg
+	DistanceSensorModeChangeRequest.msg
 	Ekf2Timestamps.msg
 	EscReport.msg
 	EscStatus.msg
@@ -152,6 +153,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 +187,7 @@ set(msg_files
 	RoverAckermannGuidanceStatus.msg
 	RoverAckermannStatus.msg
 	RoverDifferentialGuidanceStatus.msg
+	RoverDifferentialSetpoint.msg
 	RoverDifferentialStatus.msg
 	Rpm.msg
 	RtlStatus.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
@@ -0,0 +1,5 @@
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 request_on_off 			# request to disable/enable the distance sensor
+uint8 REQUEST_OFF = 0
+uint8 REQUEST_ON  = 1
@@ -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)

@@ -0,0 +1,3 @@
+uint64 timestamp
+uint8 status
+char[50] error
@@ -0,0 +1,4 @@
+uint64 timestamp
+uint8[20] id_or_mac
+uint8 operator_id_type
+char[20] operator_id
@@ -0,0 +1,4 @@
+uint64 timestamp
+uint8[20] id_or_mac
+uint8 description_type
+char[23] description
@@ -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
@@ -30,7 +30,8 @@ bool loiter_direction_counter_clockwise # loiter direction is clockwise by defau
 float32 loiter_orientation 	# Orientation of the major axis with respect to true north in rad [-pi,pi)
 uint8 	loiter_pattern		# loitern pattern to follow

-float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation
+float32 acceptance_radius   # horizontal acceptance_radius (meters)
+float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed wing guidance, NAN = let guidance choose (meters)

 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
@@ -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
@@ -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
@@ -1,8 +1,13 @@
 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_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 actual_speed	         # [m/s] Actual forward speed of the rover
+float32 actual_yaw               # [rad] Actual yaw of the rover
+float32 actual_yaw_rate          # [rad/s] Actual yaw rate of the rover
+float32 desired_yaw_rate         # [rad/s] Yaw rate setpoint for the closed loop yaw rate controller
+float32 forward_speed_normalized # [-1, 1] Normalized forward speed setpoint
+float32 speed_diff_normalized    # [-1, 1] Normalized speed difference setpoint between the left and right motor
+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
@@ -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
@@ -27,3 +27,4 @@ float32 pitch_sp_rad			# Current pitch setpoint [rad]
 float32 throttle_trim			# estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions

 float32 underspeed_ratio		# 0: no underspeed, 1: maximal underspeed. Controller takes measures to avoid stall proportional to ratio if >0.
+float32 fast_descend_ratio 		#  value indicating if fast descend mode is enabled with ramp up and ramp down [0-1]
@@ -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)

@@ -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
@@ -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
 };

 /************************************************************************************
@@ -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
@@ -55,6 +55,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/distance_sensor_mode_change_request.h>

 using namespace time_literals;

@@ -143,8 +144,11 @@ private:
 	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	typeof(px4::msg::VehicleStatus::vehicle_type) _vehicle_type{px4::msg::VehicleStatus::VEHICLE_TYPE_UNKNOWN};
+	uORB::Subscription _dist_sense_mode_change_sub{ORB_ID(distance_sensor_mode_change_request)};
+	typeof(px4::msg::DistanceSensorModeChangeRequest::request_on_off) _req_mode{px4::msg::DistanceSensorModeChangeRequest::REQUEST_OFF};
 	bool _restriction{false};
 	bool _auto_restriction{false};
+	bool _prev_restriction{false};
 };

 LightwareLaser::LightwareLaser(const I2CSPIDriverConfig &config) :
@@ -411,6 +415,17 @@ void LightwareLaser::start()

 int LightwareLaser::updateRestriction()
 {
+	if (_dist_sense_mode_change_sub.updated()) {
+		distance_sensor_mode_change_request_s dist_sense_mode_change;
+
+		if (_dist_sense_mode_change_sub.copy(&dist_sense_mode_change)) {
+			_req_mode = dist_sense_mode_change.request_on_off;
+
+		} else {
+			_req_mode = distance_sensor_mode_change_request_s::REQUEST_OFF;
+		}
+	}
+
 	px4::msg::VehicleStatus vehicle_status;

 	if (_vehicle_status_sub.update(&vehicle_status)) {
@@ -438,7 +453,7 @@ int LightwareLaser::updateRestriction()
 		updateParams();
 	}

-	bool _prev_restriction{_restriction};
+	_prev_restriction = _restriction;

 	switch (_param_sf1xx_mode.get()) {
 	case 0: // Sensor disabled
@@ -451,7 +466,7 @@ int LightwareLaser::updateRestriction()
 		break;

 	case 2:
-		_restriction = _auto_restriction;
+		_restriction = _auto_restriction && _req_mode != distance_sensor_mode_change_request_s::REQUEST_ON;
 		break;
 	}

@@ -498,6 +513,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 +523,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);
@@ -54,7 +54,7 @@ PARAM_DEFINE_INT32(SENS_EN_SF1XX, 0);
 *
 * @value 0 Disabled
 * @value 1 Enabled
- * @value 2 Disabled during VTOL fast forward flight
+ * @value 2 Enabled in VTOL MC mode, listen to request from system in FW mode
 *
 * @min 0
 * @max 2
@@ -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) {
@@ -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};
@@ -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) {
@@ -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 */
@@ -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
@@ -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
@@ -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
@@ -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);
+}
@@ -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;
+};
@@ -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 &);
@@ -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();

@@ -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
@@ -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;
@@ -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]
@@ -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()
@@ -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(); };
@@ -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",
@@ -116,6 +116,39 @@ public:
 		return self;
 	}

+	template<size_t MM, size_t NN>
+	Matrix<Type, P, Q> operator-(const SliceT<const Matrix<Type, MM, NN>, Type, P, Q, MM, NN> &other)
+	{
+		return Matrix<Type, P, Q> {*this} - other;
+	}
+
+
+	Matrix<Type, P, Q> operator-(const Matrix<Type, P, Q> &other)
+	{
+		return Matrix<Type, P, Q> {*this} - other;
+	}
+
+	Matrix<Type, P, Q> operator-(const Type &other)
+	{
+		return Matrix<Type, P, Q> {*this} - other;
+	}
+
+	template<size_t MM, size_t NN>
+	Matrix<Type, P, Q> operator+(const SliceT<const Matrix<Type, MM, NN>, Type, P, Q, MM, NN> &other)
+	{
+		return Matrix<Type, P, Q> {*this} + other;
+	}
+
+	Matrix<Type, P, Q> operator+(const Matrix<Type, P, Q> &other)
+	{
+		return Matrix<Type, P, Q> {*this} + other;
+	}
+
+	Matrix<Type, P, Q> operator+(const Type &other)
+	{
+		return Matrix<Type, P, Q> {*this} + other;
+	}
+
 	// allow assigning vectors to a slice that are in the axis
 	template <size_t DUMMY = 1> // make this a template function since it only exists for some instantiations
 	SliceT<MatrixT, Type, 1, Q, M, N> &operator=(const Vector<Type, Q> &other)
@@ -222,29 +255,19 @@ public:
 		return self;
 	}

-	SliceT<MatrixT, Type, P, Q, M, N> &operator/=(const Type &other)
+	SliceT<MatrixT, Type, P, Q, M, N> &operator/=(const Type &scalar)
 	{
-		return operator*=(Type(1) / other);
+		return operator*=(Type(1) / scalar);
 	}

-	Matrix<Type, P, Q> operator*(const Type &other) const
+	Matrix<Type, P, Q> operator*(const Type &scalar) const
 	{
-		const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
-		Matrix<Type, P, Q> res;
-
-		for (size_t i = 0; i < P; i++) {
-			for (size_t j = 0; j < Q; j++) {
-				res(i, j) = self(i, j) * other;
-			}
-		}
-
-		return res;
+		return Matrix<Type, P, Q> {*this} * scalar;
 	}

-	Matrix<Type, P, Q> operator/(const Type &other) const
+	Matrix<Type, P, Q> operator/(const Type &scalar) const
 	{
-		const SliceT<MatrixT, Type, P, Q, M, N> &self = *this;
-		return self * (Type(1) / other);
+		return (*this) * (1 / scalar);
 	}

 	template<size_t R, size_t S>
@@ -317,6 +317,7 @@ public:
 	}
 };

+using SquareMatrix2f = SquareMatrix<float, 2>;
 using SquareMatrix3f = SquareMatrix<float, 3>;
 using SquareMatrix3d = SquareMatrix<double, 3>;

@@ -102,7 +102,6 @@ public:

 };

-
 using Vector2f = Vector2<float>;
 using Vector2d = Vector2<double>;

@@ -262,6 +262,79 @@ TEST(MatrixSliceTest, Slice)
 	float O_check_data_12 [4] = {2.5, 3, 4, 5};
 	EXPECT_EQ(res_12, (SquareMatrix<float, 2>(O_check_data_12)));
 }
+TEST(MatrixSliceTest, SliceAdditions)
+{
+	float data[9] = {0, 2, 3,
+			 4, 5, 6,
+			 7, 8, 10
+			};
+	SquareMatrix3f A{data};
+
+	float operand_data [4] = {2, 1,
+				  -3, -1
+				 };
+	const SquareMatrix2f operand(operand_data);
+
+	// 2x2 Slice + 2x2 Matrix
+	SquareMatrix2f res_1 = A.slice<2, 2>(1, 0) + operand;
+	float res_1_check_data[4] = {6, 6,
+				     4, 7
+				    };
+	EXPECT_EQ(res_1, (SquareMatrix2f(res_1_check_data)));
+
+	// 2x1 Slice + 2x1 Slice
+	Vector2f res_2 = A.slice<2, 1>(1, 1) + operand.slice<2, 1>(0, 0);
+	EXPECT_EQ(res_2, Vector2f(7, 5));
+
+	// 3x3 Slice + Scalar
+	SquareMatrix3f res_3 = A.slice<3, 3>(0, 0) + (-1);
+	float res_3_check_data[9] = {-1, 1, 2,
+				     3, 4, 5,
+				     6, 7, 9
+				    };
+	EXPECT_EQ(res_3, (SquareMatrix3f(res_3_check_data)));
+
+	// 3x1 Slice + 3 Vector
+	Vector3f res_4 = A.col(1) + Vector3f(1, -2, 3);
+	EXPECT_EQ(res_4, Vector3f(3, 3, 11));
+
+}
+TEST(MatrixSliceTest, SliceSubtractions)
+{
+	float data[9] = {0, 2, 3,
+			 4, 5, 6,
+			 7, 8, 10
+			};
+	SquareMatrix3f A{data};
+
+	float operand_data[4] = {2, 1,
+				 -3, -1
+				};
+	const SquareMatrix2f operand(operand_data);
+
+	// 2x2 Slice - 2x2 Matrix
+	SquareMatrix2f res_1 = A.slice<2, 2>(1, 0) - operand;
+	float res_1_check_data[4] = {2, 4,
+				     10, 9
+				    };
+	EXPECT_EQ(res_1, (SquareMatrix2f(res_1_check_data)));
+
+	// 2x1 Slice - 2x1 Slice
+	Vector2f res_2 = A.slice<2, 1>(1, 1) - operand.slice<2, 1>(0, 0);
+	EXPECT_EQ(res_2, Vector2f(3, 11));
+
+	// 3x3 Slice - Scalar
+	SquareMatrix3f res_3 = A.slice<3, 3>(0, 0) - (-1);
+	float res_3_check_data[9] = {1, 3, 4,
+				     5, 6, 7,
+				     8, 9, 11
+				    };
+	EXPECT_EQ(res_3, (SquareMatrix3f(res_3_check_data)));
+
+	// 3x1 Slice - 3 Vector
+	Vector3f res_4 = A.col(1) - Vector3f(1, -2, 3);
+	EXPECT_EQ(res_4, Vector3f(1, 7, 5));
+}

 TEST(MatrixSliceTest, XYAssignmentTest)
 {
@@ -80,4 +80,13 @@ TEST(MatrixVector3Test, Vector3)
 	Vector3f m2(3.1f, 4.1f, 5.1f);
 	EXPECT_EQ(m2, m1 + 2.1f);
 	EXPECT_EQ(m2 - 2.1f, m1);
+
+	// Test Addition and Subtraction of Slices
+	Vector3f v1(3, 13, 0);
+	Vector3f v2(42, 6, 256);
+
+	EXPECT_EQ(v1.xy() - v2.xy(), Vector2f(-39, 7));
+	EXPECT_EQ(v1.xy() + v2.xy(), Vector2f(45, 19));
+	EXPECT_EQ(v1.xy() + 2.f, Vector2f(5, 15));
+	EXPECT_EQ(v1.xy() - 2.f, Vector2f(1, 11));
 }
@@ -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);
 	}

@@ -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);

@@ -43,3 +43,4 @@ target_include_directories(motion_planning PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

 px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS motion_planning)
 px4_add_unit_gtest(SRC PositionSmoothingTest.cpp LINKLIBS motion_planning)
+px4_add_unit_gtest(SRC HeadingSmoothingTest.cpp LINKLIBS motion_planning)
@@ -35,7 +35,7 @@

 HeadingSmoothing::HeadingSmoothing()
 {
-	_velocity_smoothing.setMaxVel(M_PI_F); // smoothed "velocity" is heading [-pi, pi]
+	_velocity_smoothing.setMaxVel(M_TWOPI_F); // "velocity" is heading. 2Pi limit is needed for correct angle wrapping
 }

 void HeadingSmoothing::reset(const float heading, const float heading_rate)
@@ -0,0 +1,147 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Test code for the Heading Smoothing library
+ * Run exclusively this test with the command "make tests TESTFILTER=HeadingSmoothing"
+ */
+
+#include "mathlib/math/Limits.hpp"
+#include <gtest/gtest.h>
+
+#include <motion_planning/HeadingSmoothing.hpp>
+
+using namespace matrix;
+
+class HeadingSmoothingTest : public ::testing::Test
+{
+public:
+	HeadingSmoothingTest()
+	{
+		_smoothing.setMaxHeadingRate(15.f);
+		_smoothing.setMaxHeadingAccel(1.f);
+	}
+
+	HeadingSmoothing _smoothing;
+	float _dt{.1f};
+};
+
+TEST_F(HeadingSmoothingTest, convergence)
+{
+	const float heading_current = math::radians(0.f);
+	const float heading_target = math::radians(5.f);
+	_smoothing.reset(heading_current, 0.f);
+
+	const int nb_steps = ceilf(1.f / _dt);
+
+	for (int i = 0; i < nb_steps; i++) {
+		_smoothing.update(heading_target, _dt);
+	}
+
+	const float heading = _smoothing.getSmoothedHeading();
+	const float heading_rate = _smoothing.getSmoothedHeadingRate();
+	EXPECT_EQ(heading, heading_target) << "heading (deg): " << math::degrees(heading);
+	EXPECT_EQ(heading_rate, 0.f);
+}
+
+TEST_F(HeadingSmoothingTest, zero_crossing)
+{
+	const float heading_current = math::radians(-95.f);
+	const float heading_target = math::radians(5.f);
+	_smoothing.reset(heading_current, 0.f);
+
+	const int nb_steps = ceilf(4.f / _dt);
+
+	for (int i = 0; i < nb_steps; i++) {
+		_smoothing.update(heading_target, _dt);
+	}
+
+	const float heading = _smoothing.getSmoothedHeading();
+	const float heading_rate = _smoothing.getSmoothedHeadingRate();
+	EXPECT_EQ(heading, heading_target) << "heading (deg): " << math::degrees(heading);
+	EXPECT_EQ(heading_rate, 0.f);
+}
+
+TEST_F(HeadingSmoothingTest, wrap_pi)
+{
+	const float heading_current = math::radians(-170.f);
+	const float heading_target = math::radians(170.f);
+	_smoothing.reset(heading_current, 0.f);
+
+	const int nb_steps = ceilf(2.f / _dt);
+
+	for (int i = 0; i < nb_steps; i++) {
+		_smoothing.update(heading_target, _dt);
+	}
+
+	const float heading = _smoothing.getSmoothedHeading();
+	const float heading_rate = _smoothing.getSmoothedHeadingRate();
+	EXPECT_EQ(heading, heading_target) << "heading (deg): " << math::degrees(heading);
+	EXPECT_EQ(heading_rate, 0.f);
+}
+
+TEST_F(HeadingSmoothingTest, positive_rate)
+{
+	const float max_heading_rate = .15f;
+	_smoothing.setMaxHeadingRate(max_heading_rate);
+	const float heading_current = math::radians(-170.f);
+	const float heading_target = math::radians(-20.f);
+	_smoothing.reset(heading_current, 0.f);
+
+	const int nb_steps = ceilf(2.f / _dt);
+
+	for (int i = 0; i < nb_steps; i++) {
+		_smoothing.update(heading_target, _dt);
+	}
+
+	const float heading_rate = _smoothing.getSmoothedHeadingRate();
+	EXPECT_EQ(heading_rate, max_heading_rate);
+}
+
+TEST_F(HeadingSmoothingTest, negative_rate)
+{
+	const float max_heading_rate = 0.15;
+	_smoothing.setMaxHeadingRate(max_heading_rate);
+	const float heading_current = math::radians(-20.f);
+	const float heading_target = math::radians(-140.f);
+	_smoothing.reset(heading_current, 0.f);
+
+	const int nb_steps = ceilf(2.f / _dt);
+
+	for (int i = 0; i < nb_steps; i++) {
+		_smoothing.update(heading_target, _dt);
+	}
+
+	const float heading_rate = _smoothing.getSmoothedHeadingRate();
+	EXPECT_EQ(heading_rate, -max_heading_rate);
+}
@@ -78,18 +78,16 @@ float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2

 	const Vector2f prev_wp_to_curr_pos = curr_pos_ned - prev_wp_ned;
 	const Vector2f prev_wp_to_curr_wp_u = prev_wp_to_curr_wp.unit_or_zero();
-	const Vector2f distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) *
-			prev_wp_to_curr_wp_u; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp
-	const Vector2f curr_pos_to_path = distance_on_line_segment -
-					  prev_wp_to_curr_pos; // Shortest vector from the current position to the path
+	_distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * prev_wp_to_curr_wp_u;
+	_curr_pos_to_path = _distance_on_line_segment - prev_wp_to_curr_pos;

-	if (curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point
-		return atan2f(curr_pos_to_path(1), curr_pos_to_path(0));
+	if (_curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point
+		return atan2f(_curr_pos_to_path(1), _curr_pos_to_path(0));
 	}

-	const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(curr_pos_to_path.norm(),
-					  2.f)); // Length of the vector from the endpoint of distance_on_line_segment to the intersection point
-	const Vector2f prev_wp_to_intersection_point = distance_on_line_segment + line_extension *
+	const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(_curr_pos_to_path.norm(),
+					  2.f)); // Length of the vector from the endpoint of _distance_on_line_segment to the intersection point
+	const Vector2f prev_wp_to_intersection_point = _distance_on_line_segment + line_extension *
 			prev_wp_to_curr_wp_u;
 	const Vector2f curr_pos_to_intersection_point = prev_wp_to_intersection_point - prev_wp_to_curr_pos;
 	return atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0));
@@ -103,6 +103,8 @@ public:
 				 float vehicle_speed);

 	float getLookaheadDistance() {return _lookahead_distance;};
+	float getCrosstrackError() {return _curr_pos_to_path.norm();};
+	float getDistanceOnLineSegment() {return _distance_on_line_segment.norm();};

 protected:
 	/**
@@ -122,5 +124,7 @@ protected:
 		float lookahead_min{1.f};
 	} _params{};
 private:
-	float _lookahead_distance{0.f};
+	float _lookahead_distance{0.f}; // Radius of the circle around the vehicle
+	Vector2f _distance_on_line_segment{}; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp
+	Vector2f _curr_pos_to_path{}; // Shortest vector from the current position to the path
 };
@@ -44,11 +44,11 @@

 #include "matrix/Matrix.hpp"
 #include "matrix/Vector2.hpp"
+#include <mathlib/math/Functions.hpp>

 using math::constrain;
 using math::max;
 using math::min;
-using namespace time_literals;

 static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);}

@@ -525,16 +525,12 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec

 	if (1.f - param.fast_descend < FLT_EPSILON) {
 		// During fast descend, we control airspeed over the pitch control loop. Give minimal thrust as soon as we are descending
-		if (specific_energy_rates.spe_rate.estimate > 0) { // We have a positive altitude rate and are stil climbing
-			throttle_setpoint = param.throttle_trim; // Do not cut off throttle yet
-
-		} else {
-			throttle_setpoint = param.throttle_min;
-		}
+		throttle_setpoint = param.throttle_min;

 	} else {
 		_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
-		throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag);
+		throttle_setpoint = (1.f - param.fast_descend) * _calcThrottleControlOutput(limit, ste_rate, param,
+				    flag) + param.fast_descend * param.throttle_min;
 	}

 	// Rate limit the throttle demand
@@ -677,6 +673,11 @@ void TECS::initControlParams(float target_climbrate, float target_sinkrate, floa
 	_control_param.throttle_min = throttle_min;
 }

+float TECS::calcTrueAirspeedSetpoint(float eas_to_tas, float eas_setpoint)
+{
+	return lerp(eas_to_tas * eas_setpoint, _control_param.tas_max, _fast_descend);
+}
+
 void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed,
 		      float eas_to_tas)
 {
@@ -699,6 +700,9 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo
 						.tas_rate = 0.0f};

 	_control.initialize(control_setpoint, control_input, _control_param, _control_flag);
+
+	_fast_descend = 0.f;
+	_enabled_fast_descend_timestamp = 0U;
 }

 void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
@@ -753,8 +757,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		TECSControl::Setpoint control_setpoint;
 		control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference();
 		control_setpoint.altitude_rate_setpoint_direct = _altitude_reference_model.getHeightRateSetpointDirect();
-		control_setpoint.tas_setpoint = _control_param.tas_max * _fast_descend + (1 - _fast_descend) * eas_to_tas *
-						EAS_setpoint;
+		control_setpoint.tas_setpoint = calcTrueAirspeedSetpoint(eas_to_tas, EAS_setpoint);

 		const TECSControl::Input control_input{ .altitude = altitude,
 							.altitude_rate = hgt_rate,
@@ -765,11 +768,13 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 	}

 	_debug_status.control = _control.getDebugOutput();
+	_debug_status.true_airspeed_sp = calcTrueAirspeedSetpoint(eas_to_tas, EAS_setpoint);
 	_debug_status.true_airspeed_filtered = eas_to_tas * _airspeed_filter.getState().speed;
 	_debug_status.true_airspeed_derivative = eas_to_tas * _airspeed_filter.getState().speed_rate;
 	_debug_status.altitude_reference = _altitude_reference_model.getAltitudeReference().alt;
 	_debug_status.height_rate_reference = _altitude_reference_model.getAltitudeReference().alt_rate;
 	_debug_status.height_rate_direct = _altitude_reference_model.getHeightRateSetpointDirect();
+	_debug_status.fast_descend = _fast_descend;

 	_update_timestamp = now;
 }
@@ -778,13 +783,22 @@ void TECS::_setFastDescend(const float alt_setpoint, const float alt)
 {
 	if (_control_flag.airspeed_enabled && (_fast_descend_alt_err > FLT_EPSILON)
 	    && ((alt_setpoint + _fast_descend_alt_err) < alt)) {
-		_fast_descend = 1.f;
+		auto now = hrt_absolute_time();
+
+		if (_enabled_fast_descend_timestamp == 0U) {
+			_enabled_fast_descend_timestamp = now;
+		}
+
+		_fast_descend = constrain(max(_fast_descend, static_cast<float>(now - _enabled_fast_descend_timestamp) /
+					      static_cast<float>(FAST_DESCEND_RAMP_UP_TIME)), 0.f, 1.f);

 	} else if ((_fast_descend > FLT_EPSILON) && (_fast_descend_alt_err > FLT_EPSILON)) {
 		// Were in fast descend, scale it down. up until 5m above target altitude
 		_fast_descend = constrain((alt - alt_setpoint - 5.f) / _fast_descend_alt_err, 0.f, 1.f);
+		_enabled_fast_descend_timestamp = 0U;

 	} else {
 		_fast_descend = 0.f;
+		_enabled_fast_descend_timestamp = 0U;
 	}
 }
@@ -50,6 +50,8 @@
 #include <motion_planning/VelocitySmoothing.hpp>
 #include <motion_planning/ManualVelocitySmoothingZ.hpp>

+using namespace time_literals;
+
 class TECSAirspeedFilter
 {
 public:
@@ -203,8 +205,8 @@ public:
 		float equivalent_airspeed_trim;		///< Equivalent cruise airspeed for airspeed less mode [m/s].
 		float tas_min;				///< True airspeed demand lower limit [m/s].
 		float tas_max;				///< True airspeed demand upper limit [m/s].
-		float pitch_max;			///< Maximum pitch angle allowed in [rad].
-		float pitch_min;			///< Minimal pitch angle allowed in [rad].
+		float pitch_max;			///< Maximum pitch angle above trim allowed in [rad].
+		float pitch_min;			///< Minimal pitch angle below trim allowed in [rad].
 		float throttle_trim;		///< Normalized throttle required to fly level at calibrated airspeed setpoint [0,1]
 		float throttle_max;			///< Normalized throttle upper limit.
 		float throttle_min;			///< Normalized throttle lower limit.
@@ -320,7 +322,7 @@ public:
 	/**
 	 * @brief Get the pitch setpoint.
 	 *
-	 * @return THe commanded pitch angle in [rad].
+	 * @return The commanded pitch angle above trim in [rad].
 	 */
 	float getPitchSetpoint() const {return _pitch_setpoint;};
 	/**
@@ -478,7 +480,7 @@ private:
 	 * @param seb_rate is the specific energy balance rate in [m²/s³].
 	 * @param param is the control parameters.
 	 * @param flag is the control flags.
-	 * @return pitch setpoint angle [rad].
+	 * @return pitch setpoint angle above trim [rad].
 	 */
 	float _calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param,
 				      const Flag &flag) const;
@@ -537,7 +539,7 @@ private:

 	// Output
 	DebugOutput _debug_output;				///< Debug output.
-	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint [rad].
+	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint above trim [rad].
 	float _throttle_setpoint{0.0f};				///< Controlled throttle setpoint [0,1].
 	float _ratio_undersped{0.0f};				///< A continuous representation of how "undersped" the TAS is [0,1]
 };
@@ -547,11 +549,13 @@ class TECS
 public:
 	struct DebugOutput {
 		TECSControl::DebugOutput control;
+		float true_airspeed_sp;
 		float true_airspeed_filtered;
 		float true_airspeed_derivative;
 		float altitude_reference;
 		float height_rate_reference;
 		float height_rate_direct;
+		float fast_descend;
 	};
 public:
 	TECS() = default;
@@ -658,6 +662,17 @@ private:
 	void initControlParams(float target_climbrate, float target_sinkrate, float eas_to_tas, float pitch_limit_max,
 			       float pitch_limit_min, float throttle_min, float throttle_setpoint_max, float throttle_trim);

+	/**
+	 * @brief calculate true airspeed setpoint
+	 *
+	 * Calculate true airspeed setpoint based on input and fast descend ratio
+	 *
+	 * @param eas_to_tas is the eas to tas conversion factor
+	 * @param eas_setpoint is the desired equivalent airspeed setpoint [m/s]
+	 * @return true airspeed setpoint[m/s]
+	 */
+	float calcTrueAirspeedSetpoint(float eas_to_tas, float eas_setpoint);
+
 	/**
 	 * @brief Initialize the control loop
 	 *
@@ -675,9 +690,11 @@ private:
 	float _equivalent_airspeed_max{20.0f};				///< equivalent airspeed demand upper limit (m/sec)
 	float _fast_descend_alt_err{-1.f};	 				///< Altitude difference between current altitude to altitude setpoint needed to descend with higher airspeed [m].
 	float _fast_descend{0.f};					///< Value for fast descend in [0,1]. continuous value used to flatten the high speed value out when close to target altitude.
+	hrt_abstime _enabled_fast_descend_timestamp{0U};		///< timestamp at activation of fast descend mode

 	static constexpr float DT_MIN = 0.001f;				///< minimum allowed value of _dt (sec)
 	static constexpr float DT_MAX = 1.0f;				///< max value of _dt allowed before a filter state reset is performed (sec)
+	static constexpr hrt_abstime FAST_DESCEND_RAMP_UP_TIME = 2_s; 	///< Ramp up time until fast descend is fully engaged

 	DebugOutput _debug_status{};

@@ -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
@@ -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
 """


@@ -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);
@@ -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};

@@ -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);
 		}

@@ -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;
 				}
 			}
 		}
@@ -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,
@@ -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
 */
@@ -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:
@@ -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,
@@ -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
@@ -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;
@@ -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;
 }
@@ -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;
 	}
 }

@@ -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;
 }
@@ -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)
@@ -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;
 	}
 }

@@ -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);

@@ -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
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
RomanBapst	4d930bdec2	added full support for standard vtol in SIH Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-11-12 11:16:44 +03:00
RomanBapst	f417ae73ef	added full support for external autostart scripts Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-11-12 11:16:22 +03:00
RomanBapst	31433b6402	enable sih for v6x Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-11-12 11:16:22 +03:00
Claudio Chies	5d2e7c8748	Misc: Matrix: Added Addition and Subtraction to Slices (#23679 ) * Added Addition and Subtraction to Slices * MatrixSliceTest: refactor Addition/Substraction checks * Slice: replace operations returning a Matrix with calling the existing Matrix function --------- Co-authored-by: Matthias Grob <maetugr@gmail.com>	2024-09-18 11:17:37 +02:00
Marco Hauswirth	4a99a51fb1	update upload-artifact v2->v4	2024-09-17 10:55:00 -07:00
chfriedrich98	8aece9bff2	differential: fix CI issue	2024-09-17 09:34:51 -07:00
chfriedrich98	2fd4150b38	differential: Add stabilized and position mode (#23669 ) * differential: add position and stabilized mode * differential: add hardcoded stick input deadzones	2024-09-16 12:09:51 +02:00
chfriedrich98	81747f35bb	rover: add descend navigation state to land detection	2024-09-16 09:36:38 +02:00
Silvan Fuhrer	1c9c5e51c2	boards: cuav x7pro: remove build of ROVER and Q_ATTITUDE_ESTIMATOR to save flash Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-09-16 09:11:23 +02:00
Konrad	82a7d0410c	DistanceSensorModeChangeRequest: renaming of variable	2024-09-16 09:11:23 +02:00
Konrad	aab2390e51	navigator: publish distance sensor mode change request when in RTL landing phase or during mission landing	2024-09-16 09:11:23 +02:00
Konrad	1755b8304e	RTL direct: Make sure the _rtl_state captures the current status and not the next one	2024-09-16 09:11:23 +02:00
Konrad	e6f07bde2a	lightware_laser: add option to listen to system to enable/disable distance sensor	2024-09-16 09:11:23 +02:00
Silvan Fuhrer	9ca0630376	airframes: SIH_tailsitter: add SENS_DPRES_OFF to bypass airspeed cal Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-09-13 15:53:52 +03:00
RomanBapst	11440cfb45	added some default parameteter that allow the transition to complete Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-09-13 15:53:52 +03:00
RomanBapst	878c8bfcce	SIH: fix airspeed for tailsitter Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-09-13 15:53:52 +03:00
Konrad	4713a6737e	TECS: ramp up fast descend over 2_s to ramp down the throttle command	2024-09-13 14:04:39 +02:00
Konrad	585c92796d	mission_base: do not make terrain avoidance check when mission is not run anymore	2024-09-13 13:59:24 +02:00
Claudio Chies	4ba4b340cc	Reduce the orbit jerk by using a slew rate	2024-09-13 10:28:42 +02:00
Marco Hauswirth	22c2878cf8	send acknowledgement after receiving vehicle wind cmd	2024-09-12 09:34:08 +02:00
chfriedrich98	741ea6b707	differential: add individual parameters for speed and yaw rate feedforward	2024-09-11 13:57:27 +02:00
chfriedrich98	5d8a107925	differential: fix closed loop control removed thresholds for closed loop setpoints and added minimum thresholds for yaw rate and speed measurements instead to avoid moving due to measurement noise	2024-09-11 13:57:27 +02:00
Roman Bapst	c94c1ce4d2	Navigator: Support straight line mission landings (#23576 ) * introduced altitude acceptance radius in position setpoint for fixed wing guidance - allows navigator to explicitly set the altitude acceptance radius - needed for staright line landing support * added ignore_alt_acceptance to position setpoint message to allow guidance logic to ignore altitude error on waypoint - can be useful to prevent loitering at a waypoint within a mission landing sequence * only set altitude acceptance radius to infinity for a waypoint inside a mission landing for fixed wing vehicles * navigator: return altitude acceptance radius from triplet if it's valid * FixedWingPositionControl: check if alt acceptance radius provided in position setpoint is larger 0 --------- Signed-off-by: RomanBapst <bapstroman@gmail.com> Co-authored-by: Alvaro Fernandez <alvaro@auterion.com>	2024-09-10 17:44:24 +02:00
Mathieu Bresciani	03aec2e188	HeadingSmoothing: fix angle wrapping and add unit tests Co-authored-by: Matthias Grob <maetugr@gmail.com>	2024-09-10 15:06:19 +02:00
jmackay2	a5729da4e9	Simplify gz bridge CMakeLists and add GZ Ionic (#23657 ) Co-authored-by: jmackay2 <jmackay2@gmail.com>	2024-09-10 11:53:00 +02:00
bresch	15e9c65a8f	dist-sensor: reduce enum names	2024-09-09 15:40:40 +02:00
bresch	8bca467c15	dist-sensor: use enum instead of integer	2024-09-09 15:40:40 +02:00
chfriedrich98	bb0210ecd7	rover: add rtl as a landed condition for rovers (#23646 ) The RTL sequence from the navigator requires the vehicle to land. This is now handled for rovers by setting its state to landed if it is within the acceptance radius of the home position when in return mode.	2024-09-06 17:21:49 +02:00
Hubert	67ee4817ae	Makefile add micoair h743 bootloader	2024-09-06 11:14:30 -04:00
Hubert	232f699a7f	cmake-variants.yaml add micoair h743	2024-09-06 11:14:30 -04:00
Hubert	c2bd3900be	Jenkins: compile add micoair h743	2024-09-06 11:14:30 -04:00
Marco Hauswirth	44967bdaab	ekf2: uncorrelate position covariance after velocity reset (#23644 )	2024-09-06 08:51:15 -04:00
RomanBapst	1337fca4d0	vtol backtransition: removed downscaling of fw controls during the backtransition Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-09-06 13:55:48 +03:00
Alexander Lerach	3d36c8519d	drivers/power_monitor: Implement temperature sensor support for INA228 / INA238	2024-09-05 23:09:01 -04:00
Matthias Grob	f98eb067be	logger params: clarify AUX1 logging trigger	2024-09-05 18:06:29 +02:00
Matthias Grob	e4d25df58a	Consistently use "stick gesture" for "rc stick gesture"	2024-09-05 18:06:29 +02:00
Silvan Fuhrer	8eaf93468e	Commander: feedback string for arming/disarming: make clear when from gesture Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-09-05 18:06:29 +02:00
Silvan Fuhrer	d967cdbb48	Manual control: rename SOURCE_RC_STICK_GESTURE to SOURCE_MANUAL_CONTROL_GESTURE Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-09-05 18:06:29 +02:00
Silvan Fuhrer	556a302a09	Logger: replace RC keyword by 'manual control' Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-09-05 18:06:29 +02:00
Silvan Fuhrer	f7c35291ee	Rover Differential: remove RC keyword from params Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-09-05 18:06:29 +02:00
Silvan Fuhrer	81cf6a736d	Commander: add VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE to list of externaly handled commands (#23642 ) Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-09-05 11:20:39 +02:00
Silvan Fuhrer	6fa6360aef	Commander: always allow to switch to LAND mode (#23580 ) 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. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-09-03 18:10:37 +02:00
Konrad	80d4fad624	DistanceSensorCheck: do not raise a distance sensor failure if the SFXX_MODE is set to 2 and we are in a VTOL FX flight phase	2024-09-03 15:53:09 +02:00
chfriedrich98	a9cdb36d7c	differential: reset integrators when disarmed (#23637 )	2024-09-03 09:31:39 +02:00
Julian Oes	8f6ce4edbf	mavlink/lib: move open_drone_id helpers to mavlink I could not extract the open_drone_id helpers to a separate lib because it would require the mavlink headers while the mavlink library would also depend on it, so it ended up being a circular dependency. Instead, I'm now just using the headers from within the mavlink module as well as from the uavcan driver.	2024-09-02 16:20:10 +12:00
Julian Oes	b7c5ba1752	boards: make flash space for remote ID over DroneCAN	2024-09-02 16:20:10 +12:00
Julian Oes	cd63cfed3a	remoteid: implement System as sent from GCS This will send the System message if it is already being sent by a ground station. Otherwise, it will assemble the message itself using the takeoff/home location.	2024-09-02 16:20:10 +12:00
Julian Oes	7d1d398984	remoteid: add SelfID message	2024-09-02 16:20:10 +12:00
Julian Oes	04ea4f9b3a	uavcan: add OpenDroneID ArmStatus, operator ID In order to have operator ID be sent by QGC, we need to forward ArmStatus from the remote ID module (here on DroneCAN) to MAVLink.	2024-09-02 16:20:10 +12:00
Julian Oes	d999258171	uavcan: implement OpenDroneID System	2024-09-02 16:20:10 +12:00
Julian Oes	de00c23e19	uavcan: implement OpenDroneID Location	2024-09-02 16:20:10 +12:00
Julian Oes	cf19764d75	uavcan: implement OpenDroneID BasicID Signed-off-by: Julian Oes <julian@oes.ch>	2024-09-02 16:20:10 +12:00
Julian Oes	87a63e75be	mavlink: extract OpenDroneID function to lib This extracts the function mapping from MAV_TYPE to MAV_ODID_UA_TYPE to the library, so that it can be re-used later by the remote ID implementation over DroneCAN. Signed-off-by: Julian Oes <julian@oes.ch>	2024-09-02 16:20:10 +12:00
Julian Oes	4c63e9e4f9	libuavcan: update DroneCAN submodule Signed-off-by: Julian Oes <julian@oes.ch>	2024-09-02 16:20:10 +12:00
Marco Hauswirth	7dcea6b2e4	EKF2: range measurement rejection in rain/fog (#23579 )	2024-08-30 17:25:56 +02:00
Benjamin Perseghetti	787fe9590d	Fix typo where 22.04 still says Gz (Garden) (#23632 ) Signed-off-by: Benjamin Perseghetti <bperseghetti@rudislabs.com>	2024-08-29 20:42:56 +02:00
Daniel Agar	5b0014cb06	ekf2: remove legacy accel z bias checks (#23341 ) Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>	2024-08-29 11:51:27 -04:00
chfriedrich98	f8188f0981	differential: update module (#23629 ) Improve the slow down effect and add support for speed change in mission mode. Seperate code related to turning setpoints into motor commands into its own folder and refactor code.	2024-08-29 15:27:08 +02:00
Silvan Fuhrer	c86d44f831	Commander: remove 2 decimals from COM_FAIL_ACT_T Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-08-29 10:56:04 +02:00
Silvan Fuhrer	6b3e3aa363	Commander: improve param description of COM_POSCTL_NAVL and rename Manual-->Stabilized Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-08-29 10:56:04 +02:00
Mathieu Bresciani	2cda0efd84	Commander: extend COM_ARM_WO_GPS to disable warning (#23628 )	2024-08-28 17:33:58 +02:00
Jacob Dahl	0f1507c24e	[gz] X500 mono_cam_down and aruco world (#23450 ) * x500 mono cam down and aruco world * remove duplicate line	2024-08-28 17:23:39 +02:00
bresch	bab256bfe6	ekf2: handle external altitude resets	2024-08-28 11:02:26 +02:00
bresch	cd2170deea	ekf2-origin: backcompute based on lpos validity	2024-08-28 11:02:26 +02:00
bresch	130fefb1e7	ekf2: initialize origin from corrent position when possible	2024-08-28 11:02:26 +02:00
bresch	af752536b9	ekf2: extract setting origin from current lat/lon/alt This is not only needed when GNSS is available but also for other global sources of position (e.g.: aux global pos and manual pos updates)	2024-08-28 11:02:26 +02:00
bresch	9169a7c5fc	ekf2: split horizontal and vertical origin reset Allow partial resets (only lat/lon or only altitude)	2024-08-28 11:02:26 +02:00
Daniel Agar	f3d58cdf10	ekf2: resetFlowFusion() pass flowSample by const ref	2024-08-27 10:38:17 -04:00
Daniel Agar	6c24413888	ekf2: filter flow vel (used for flow velocity reset) - individual flow samples can be quite erratic	2024-08-27 10:38:17 -04:00
dagar	5ff4eea870	[AUTO COMMIT] update change indication	2024-08-27 16:16:55 +02:00
Daniel Agar	ac48b8b51d	ekf2: mag declination fusion always if there is no aiding	2024-08-27 16:16:55 +02:00
Daniel Agar	2a9e205442	ekf2: fuseDeclination respect mag update_all_states - when both mag_hdg/mag_3d are inactive we should be able to continue updating mag without any possible impact on other states	2024-08-27 16:16:55 +02:00
Daniel Agar	9d57a3c02f	ekf2: split resetMagCov() and skip mag reset if negligible change	2024-08-27 16:16:55 +02:00
Daniel Agar	bbcf741e9e	ekf2: make mag control responsible for WMM - this further untangles mag control (which requires the WMM) from GPS	2024-08-27 16:16:55 +02:00
sbtjagu	be4d0d351c	ackermann: add speed waypoint support and fix delay detection (#23572 )	2024-08-27 13:35:48 +02:00
jmackay2	5fff1ad6d1	Fix spelling of airflow sensor msg comments	2024-08-27 09:23:43 +02:00
Jukka Laitinen	f67eb6989d	mavlink: Fix ESC_STATUS sending for batches > 1 The indexing was wrong for esc_status sending for ESCs 4-> Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>	2024-08-26 15:06:55 -04:00
LucaS	ca47f6f016	lib/mixer_module: added a constant instance start so that when instance start is changed in actuator yaml files they parameters are able to be used (#23616 ) Co-authored-by: Luca Scheuer <luca.scheuer@iq-control.com>	2024-08-26 14:51:09 -04:00
Ramon Roche	16c77be7c0	tests: loosen radius of vtol rtl landing pos check	2024-08-26 14:05:17 -04:00
Daniel Agar	a75db1286d	logger: automatically limit buffer size to largest available free chunk (NuttX only)	2024-08-26 13:24:39 -04:00
Silvan Fuhrer	8bfd3b0f62	platforms/nuttx/init/stm32f7: rc.board_arch_defaults reduce LOGGER_BUF to 40 To get some breathing space on setups with memory-intensive components (e.g. UAVCAN). Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-08-26 13:13:11 -04:00
bresch	9183c479a5	ekf2: correctly compute vel variance from flow variance Co-authored-by: Marco Hauswirth <marco.hauswirth@auterion.com>	2024-08-26 11:28:36 -04:00
Claudio Chies	1a4e8a7341	FLOW: PARAM: GCS Parameter readability	2024-08-26 16:09:21 +02:00
SuddenDeath	510d3cfb39	gz: Fix endless wait for gazebo on different worlds (#23613 ) Co-authored-by: your-sudden-death <noreply@pm.me>	2024-08-24 17:15:41 +02:00
Daniel Agar	ebbb880e92	ekf2: always use corrected accel/gyro for filtered metrics	2024-08-23 17:35:59 -04:00
Daniel Agar	56560726d3	ekf2: sensor simulator fix GPS replay scaling	2024-08-23 14:35:05 -04:00
Daniel Agar	d7b165991f	cmake: relax git tag requirements - default to v0.0.0 if tag isn't available - src/lib/px_update_git_header.py use same PX4_GIT_TAG as cmake - update lingering master branch references to main	2024-08-23 12:05:34 -04:00
Ramon Roche	54f7b58007	Commander: lock down mav sys and comp id - keeps them as local params at init - only allow to set at init	2024-08-23 11:19:25 -04:00
bresch	1a0f97ebbd	ekf2-fake pos: add valid fake position fusion This is similar to fake pos but is only used when the ekf has an external information telling it that the vehicle is not changing position. This information can then be used to keep a valid local position even when the vehicle isn't exactly at rest.	2024-08-23 11:17:21 +02:00
bresch	64b0586dad	ekf2: return validity based on dead-reckoning time only	2024-08-23 11:17:21 +02:00
David Sidrane	cf941b18df	Nuttx with stm32h7: STM32H7X5XX selects hardware files backport	2024-08-23 05:12:28 -04:00
jfbblue0922	13c413622b	Nuttx with stm32h7: STM32H7X5XX selects hardware files backport	2024-08-23 04:48:20 -04:00
Jaeyoung Lim	b1dfe1d731	Update gz version to harmonic	2024-08-22 21:37:00 -04:00
Ramon Roche	00c3017334	ci: add note regarding RunsOn	2024-08-22 12:06:50 -04:00
Ramon Roche	89f29e91de	ci: slow down sitl test realtime	2024-08-22 12:06:50 -04:00
Ramon Roche	7f33dcfcfb	ci: upgrade sitl mavsdk tests workflow	2024-08-22 12:06:50 -04:00
Jaeyoung Lim	d617bf4129	simulation/gz_bridge: Fix build issues with unused variable	2024-08-22 11:48:46 -04:00
Daniel Agar	7250ee1b32	ekf2: organize gyro_bias/accel_bias param yaml	2024-08-22 10:56:16 -04:00
Daniel Agar	ebbd2c1825	ekf2: organize aid source parameters	2024-08-22 10:56:16 -04:00
Claudio Chies	ee022a70c1	Navigator: Land: Improve it for VTOL by taking breaking distance into account (#23566 ) * vtol adjust landing setpoint * improve comment Co-authored-by: Silvan Fuhrer <silvan@auterion.com> --------- Co-authored-by: Silvan Fuhrer <silvan@auterion.com>	2024-08-22 14:10:36 +02:00
Silvan Fuhrer	e0bb56b6a7	Commander: Failsafe: set clear condition for action Land like for RTL (#23569 ) For many failsafes, it is possible to select RTL and Land as actions. In this commit I synchronize the clear condition for these two action options, to always only clear on Disarm or manual mode change. Reasoning is that for the user RTL and Land is a similar action and I would thus expect them to be as similar as possible. And I in general would rather not clear a failsafe state instead of too often clearing it. Example: GF failsafe with action Land --> even if the drone is marginally within the GF again, I want it to proceed with the Landing unless I manually intervene. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-08-22 14:03:24 +02:00
Silvan Fuhrer	6ef82ada6e	Navigator: make sure VTOL transitions in Descend mode are alays triggered (#23578 ) It previously didn't catch switches to Descend from a manual mode, as both modes have navigation_mode_new=nullptr. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-08-22 14:02:32 +02:00
Konrad	20b6f343a3	mission_base: make sure all mission_items during landing phase have yaw set to NaN	2024-08-22 12:58:44 +02:00
ZeroOne-Aero	02ed1162ed	Update pab_manifest.c (#23594 ) * Update pab_manifest.c I have rebased on main and squash my commits into 1. * Update pab_manifest.c I have updated pab_manifest.c: // BASE ID 0x150 ZeroOne Pixhawk Baseboard Alaised to ID 0 {HW_BASE_ID(0x150), base_configuration_0, arraySize(base_configuration_0)}, // ZeroOne Pixhawk Baseboard ver 0x150	2024-08-22 04:02:02 -04:00
jmackay2	b33b0398dd	Fix param typo in quadtailsitter airframe (#23588 )	2024-08-22 10:30:10 +10:00
Jaeyoung Lim	ae16556107	simulation/gz_bridge: follow model in gz GUI (#22808 )	2024-08-21 11:41:47 -04:00
Ramon Roche	b2f663648e	ci: github actions runs-on Dronecode AWS Infra * ci: try runs-on Dronecode Infra * ci: comment on how to disable RunsOn * Update .github/workflows/build_all_targets.yml	2024-08-21 10:56:37 -04:00