Add ifdefs for airspeed gz msgs

Revert "Revert "Update GZBridge to be able to use gazebo airspeed. Add quadtailsitter. (#23455 )""
2026-06-13 03:50:04 +08:00 · 2024-08-21 09:44:14 +02:00 · 2024-08-21 09:36:30 +02:00
197 changed files with 3482 additions and 6177 deletions
@@ -77,7 +77,6 @@ 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,8 +1,3 @@
-# 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:
@@ -19,8 +14,7 @@ on:
 jobs:
  group_targets:
    name: Scan for Board Targets
-    # runs-on: ubuntu-latest
-    runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"]
+    runs-on: ubuntu-latest
    outputs:
      matrix: ${{ steps.set-matrix.outputs.matrix }}
      timestamp: ${{ steps.set-timestamp.outputs.timestamp }}
@@ -40,8 +34,7 @@ jobs:

  setup:
    name: ${{ matrix.group }}
-    # runs-on: ubuntu-latest
-    runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}"]
+    runs-on: ubuntu-latest
    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@v4
+      uses: actions/upload-artifact@v2-preview
      with:
        name: coredump
        path: px4.core
@@ -103,21 +103,21 @@ jobs:

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

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

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

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

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

    - name: Store ROS log
      if: failure()
-      uses: actions/upload-artifact@v4
+      uses: actions/upload-artifact@v2
      with:
        name: ros_log
        path: ~/.ros/**/rostest-*.log
@@ -1,8 +1,3 @@
-# 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:
@@ -15,139 +10,126 @@ on:

 jobs:
  build:
-    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
+    runs-on: ubuntu-latest
    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@v4
-        with:
-          fetch-depth: 0
+    - uses: actions/checkout@v1
+      with:
+        token: ${{ secrets.ACCESS_TOKEN }}

-      - name: Git Ownership Workaround
-        run: git config --system --add safe.directory '*'
+    - 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"

-      - id: set-timestamp
-        name: Set timestamp for cache
-        run: echo "::set-output name=timestamp::$(date +"%Y%m%d%H%M%S")"
+    - 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

-      - 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: 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 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: Core dump settings
+      run: |
+          ulimit -c unlimited
+          echo "`pwd`/%e.core" > /proc/sys/kernel/core_pattern

-      - name: Build PX4
-        env:
-          PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-        run: make px4_sitl_default
+    - 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: Cache Post-Run [px4_sitl_default]
-        run: ccache -s
+    - 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: Build SITL Gazebo
-        env:
-          PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-        run: make px4_sitl_default sitl_gazebo-classic
+    - name: Upload px4 binary
+      if: failure()
+      uses: actions/upload-artifact@v2-preview
+      with:
+        name: binary
+        path: build/px4_sitl_default/bin/px4

-      - 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
+    # 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
@@ -286,11 +286,6 @@ 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,20 +113,12 @@ include(px4_parse_function_args)
 include(px4_git)

 execute_process(
-	COMMAND git describe --exclude ext/* --tags --match "v[0-9]*"
+	COMMAND git describe --exclude ext/* --always --tags
 	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,7 +340,6 @@ 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 \
@@ -14,19 +14,15 @@ 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_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_SPEED 7
+param set-default RD_HEADING_P 5
+param set-default RD_HEADING_I 0.1
 param set-default RD_MAX_YAW_RATE 180
-param set-default RD_MISS_SPD_DEF 5
+param set-default RD_MISS_SPD_DEF 7
 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_YAW_P 5
-param set-default RD_YAW_I 0.1
+param set-default RD_HEADING_P 5
+param set-default RD_HEADING_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,92 @@
+#!/bin/sh
+#
+# @name Quadrotor + Tailsitter
+#
+# @type VTOL Quad Tailsitter
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter}
+
+param set-default SIM_GZ_EN 1
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 0
+
+param set-default MAV_TYPE 20
+
+param set-default CA_AIRFRAME 4
+
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.23
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.23
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.23
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.23
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default CA_SV_CS_COUNT 0
+
+param set-default SIM_GZ_EC_FUNC1 101
+param set-default SIM_GZ_EC_MIN1 10
+param set-default SIM_GZ_EC_MAX1 1500
+param set-default SIM_GZ_EC_FUNC2 102
+param set-default SIM_GZ_EC_MIN2 10
+param set-default SIM_GZ_EC_MAX2 1500
+param set-default SIM_GZ_EC_FUNC3 103
+param set-default SIM_GZ_EC_MIN3 10
+param set-default SIM_GZ_EC_MAX3 1500
+param set-default SIM_GZ_EC_FUNC4 104
+param set-default SIM_GZ_EC_MIN4 10
+param set-default SIM_GZ_EC_MAX4 1500
+
+param set-default FD_FAIL_R 70
+
+param set-default FW_P_TC 0.6
+
+param set-default FW_PR_I 0.3
+param set-default FW_PR_P 0.5
+param set-default FW_PSP_OFF 2
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_I 0.1
+param set-default FW_RR_P 0.2
+param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P
+param set-default FW_YR_I 0
+param set-default FW_THR_TRIM 0.35
+param set-default FW_THR_MAX 0.8
+param set-default FW_THR_MIN 0.05
+param set-default FW_T_CLMB_MAX 6
+param set-default FW_T_HRATE_FF 0.5
+param set-default FW_T_SINK_MAX 3
+param set-default FW_T_SINK_MIN 1.6
+param set-default FW_AIRSPD_STALL 10
+param set-default FW_AIRSPD_MIN 14
+param set-default FW_AIRSPD_TRIM 18
+param set-default FW_AIRSPD_MAX 22
+
+param set-default MC_AIRMODE 2
+param set-default MAN_ARM_GESTURE 0 # required for yaw airmode
+param set-default MC_ROLL_P 3
+param set-default MC_PITCH_P 3
+param set-default MC_ROLLRATE_P 0.3
+param set-default MC_PITCHRATE_P 0.3
+
+param set-default VT_ARSP_TRANS 15
+param set-default VT_B_TRANS_DUR 5
+param set-default VT_FW_DIFTHR_EN 7
+param set-default VT_FW_DIFTHR_S_Y 1
+param set-default VT_F_TRANS_DUR 1.5
+param set-default VT_TYPE 0
+
+param set-default WV_EN 0
@@ -1,10 +0,0 @@
-#!/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,7 +85,7 @@ px4_add_romfs_files(
 	4011_gz_lawnmower
 	4012_gz_rover_ackermann
 	4013_gz_x500_lidar
-	4014_gz_x500_mono_cam_down
+	4014_gz_quadtailsitter

 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
@@ -43,10 +43,6 @@ 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
@@ -66,8 +62,6 @@ 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
@@ -81,5 +75,3 @@ param set SIH_L_ROLL 0.145

 # sih as tailsitter
 param set SIH_VEHICLE_TYPE 2
-
-param set-default VT_ARSP_TRANS 6
@@ -1,99 +0,0 @@
-#!/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,27 +20,3 @@ 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,7 +48,6 @@ 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,13 +105,6 @@ 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 (Harmonic) will be installed"
+		echo "Gazebo (Garden) 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-harmonic"
+		gazebo_packages="gz-garden"
 	elif [[ "${UBUNTU_RELEASE}" == "21.3" ]]; then
 		echo "Gazebo (Garden) will be installed"
 		echo "Earlier versions will be removed"
@@ -85,6 +85,8 @@ 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,6 +39,7 @@ 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
@@ -69,6 +70,7 @@ 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
@@ -77,6 +79,7 @@ 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
@@ -88,6 +91,7 @@ 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
@@ -95,3 +99,4 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_EXAMPLES_FAKE_GPS=y
@@ -3,8 +3,6 @@ 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,6 +1,4 @@
 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,6 +29,7 @@ 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=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_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_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
 CONFIG_DRIVERS_OSD_MSP_OSD=y
-CONFIG_DRIVERS_POWER_MONITOR_INA226=n
+CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 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=n
+CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
 CONFIG_MODULES_AIRSPEED_SELECTOR=y
 CONFIG_MODULES_BATTERY_STATUS=y
@@ -377,6 +377,7 @@
 *(.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_STICK_GESTURE = 0
+uint8 SOURCE_RC_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 in degrees Celcius, 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. 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,7 +73,6 @@ set(msg_files
 	DebugVect.msg
 	DifferentialPressure.msg
 	DistanceSensor.msg
-	DistanceSensorModeChangeRequest.msg
 	Ekf2Timestamps.msg
 	EscReport.msg
 	EscStatus.msg
@@ -153,10 +152,6 @@ set(msg_files
 	ObstacleDistance.msg
 	OffboardControlMode.msg
 	OnboardComputerStatus.msg
-	OpenDroneIdArmStatus.msg
-	OpenDroneIdOperatorId.msg
-	OpenDroneIdSelfId.msg
-	OpenDroneIdSystem.msg
 	OrbitStatus.msg
 	OrbTest.msg
 	OrbTestLarge.msg
@@ -187,7 +182,6 @@ set(msg_files
 	RoverAckermannGuidanceStatus.msg
 	RoverAckermannStatus.msg
 	RoverDifferentialGuidanceStatus.msg
-	RoverDifferentialSetpoint.msg
 	RoverDifferentialStatus.msg
 	Rpm.msg
 	RtlStatus.msg
@@ -40,8 +40,3 @@ 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
@@ -1,5 +0,0 @@
-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,8 +45,6 @@ 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
@@ -59,6 +57,7 @@ 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)

@@ -1,3 +0,0 @@
-uint64 timestamp
-uint8 status
-char[50] error
@@ -1,4 +0,0 @@
-uint64 timestamp
-uint8[20] id_or_mac
-uint8 operator_id_type
-char[20] operator_id
@@ -1,4 +0,0 @@
-uint64 timestamp
-uint8[20] id_or_mac
-uint8 description_type
-char[23] description
@@ -1,13 +0,0 @@
-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,8 +30,7 @@ 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   # horizontal acceptance_radius (meters)
-float32 alt_acceptance_radius # vertical acceptance radius, only used for fixed wing guidance, NAN = let guidance choose (meters)
+float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation

 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,7 +1,10 @@
 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
@@ -1,9 +0,0 @@
-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,13 +1,8 @@
 uint64 timestamp # time since system start (microseconds)

-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
+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

 # 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 being reported by the wind / airflow sensor
+float32 direction		# the direction bein report by the wind / airflow sensor
 uint8 status			# Status code from the sensor
@@ -27,4 +27,3 @@ 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,9 +4,6 @@ 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_STICK_GESTURE = 1
+uint8 ARM_DISARM_REASON_RC_STICK = 1
 uint8 ARM_DISARM_REASON_RC_SWITCH = 2
 uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3
 uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4
@@ -357,7 +357,6 @@ 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
@@ -373,7 +372,6 @@ 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 40
+set LOGGER_BUF 64
@@ -55,7 +55,6 @@
 #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;

@@ -144,11 +143,8 @@ 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) :
@@ -415,17 +411,6 @@ 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)) {
@@ -453,7 +438,7 @@ int LightwareLaser::updateRestriction()
 		updateParams();
 	}

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

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

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

@@ -513,8 +498,6 @@ 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");

@@ -523,14 +506,6 @@ 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 Enabled in VTOL MC mode, listen to request from system in FW mode
+ * @value 2 Disabled during VTOL fast forward flight
 *
 * @min 0
 * @max 2
@@ -88,7 +88,6 @@ 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());
 }

@@ -307,7 +306,6 @@ 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");
@@ -317,7 +315,6 @@ 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);
@@ -384,7 +381,6 @@ 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,7 +291,6 @@ 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))
@@ -340,7 +339,6 @@ 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,7 +80,6 @@ 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());
 }

@@ -225,11 +224,9 @@ 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
@@ -253,7 +250,6 @@ 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);
@@ -313,7 +309,6 @@ 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,7 +73,6 @@ 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,7 +14,6 @@ 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,8 +147,6 @@ px4_add_module(
 		arming_status.hpp
 		beep.cpp
 		beep.hpp
-		remoteid.cpp
-		remoteid.hpp
 		rgbled.cpp
 		rgbled.hpp
 		safety_state.cpp
@@ -26,10 +26,6 @@ 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
@@ -1,356 +0,0 @@
-/****************************************************************************
- *
- *   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);
-}
@@ -1,107 +0,0 @@
-/****************************************************************************
- *
- *   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 excessive bus traffic
+	// Max update rate to avoid exessive bus traffic
 	static constexpr unsigned MAX_RATE_HZ = 20;

 	void periodic_update(const uavcan::TimerEvent &);
@@ -96,9 +96,6 @@ 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
@@ -561,16 +558,6 @@ 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,10 +68,6 @@

 #include "logmessage.hpp"

-#if defined(CONFIG_UAVCAN_REMOTEID_CONTROLLER)
-#include "remoteid.hpp"
-#endif
-
 #if defined(CONFIG_UAVCAN_RGB_CONTROLLER)
 #include "rgbled.hpp"
 #endif
@@ -273,9 +269,6 @@ 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,11 +106,6 @@ 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
@@ -154,7 +149,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 = _temperature_c;
+	battery_status.temperature = NAN;
 	battery_status.cell_count = _params.n_cells;
 	battery_status.connected = _connected;
 	battery_status.source = _source;
@@ -91,7 +91,6 @@ 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
@@ -169,7 +168,6 @@ 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,8 +39,7 @@ 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);
-	set_mode(distance_sensor_s::MODE_UNKNOWN);
+	set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); // Default to type LASER
 }

 PX4Rangefinder::~PX4Rangefinder()
@@ -60,8 +60,6 @@ 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": "stick_gesture",
-                            "description": "Stick gesture"
+                            "name": "rc_stick",
+                            "description": "RC"
                        },
                        "2": {
                            "name": "rc_switch",
-                            "description": "RC switch"
+                            "description": "RC (switch)"
                        },
                        "3": {
                            "name": "command_internal",
@@ -116,39 +116,6 @@ 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)
@@ -255,19 +222,29 @@ public:
 		return self;
 	}

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

-	Matrix<Type, P, Q> operator*(const Type &scalar) const
+	Matrix<Type, P, Q> operator*(const Type &other) const
 	{
-		return Matrix<Type, P, Q> {*this} * scalar;
+		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;
 	}

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

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

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

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

 };

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

@@ -262,79 +262,6 @@ 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,13 +80,4 @@ 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, const uint8_t instance_start) :
+			   SchedulingPolicy scheduling_policy, bool support_esc_calibration, bool ramp_up) :
 	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(instance_start);
+	initParamHandles();

 	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(const uint8_t instance_start)
+void MixingOutput::initParamHandles()
 {
 	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 + instance_start);
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FUNC", i + 1);
 		_param_handles[i].function = param_find(param_name);
-		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "DIS", i + instance_start);
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "DIS", i + 1);
 		_param_handles[i].disarmed = param_find(param_name);
-		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MIN", i + instance_start);
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MIN", i + 1);
 		_param_handles[i].min = param_find(param_name);
-		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MAX", i + instance_start);
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "MAX", i + 1);
 		_param_handles[i].max = param_find(param_name);
-		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FAIL", i + instance_start);
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", _param_prefix, "FAIL", i + 1);
 		_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, const uint8_t instance_start = 1);
+		     bool support_esc_calibration, bool ramp_up = true);

 	~MixingOutput();

@@ -221,7 +221,7 @@ private:

 	void cleanupFunctions();

-	void initParamHandles(const uint8_t instance_start);
+	void initParamHandles();

 	void limitAndUpdateOutputs(float outputs[MAX_ACTUATORS], bool has_updates);

@@ -43,4 +43,3 @@ 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_TWOPI_F); // "velocity" is heading. 2Pi limit is needed for correct angle wrapping
+	_velocity_smoothing.setMaxVel(M_PI_F); // smoothed "velocity" is heading [-pi, pi]
 }

 void HeadingSmoothing::reset(const float heading, const float heading_rate)
@@ -1,147 +0,0 @@
-/****************************************************************************
- *
- *   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,16 +78,18 @@ 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();
-	_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;
+	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

-	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,8 +103,6 @@ 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:
 	/**
@@ -124,7 +122,5 @@ protected:
 		float lookahead_min{1.f};
 	} _params{};
 private:
-	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
+	float _lookahead_distance{0.f};
 };
@@ -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,12 +525,16 @@ 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
-		throttle_setpoint = param.throttle_min;
+		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;
+		}

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

 	// Rate limit the throttle demand
@@ -673,11 +677,6 @@ 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)
 {
@@ -700,9 +699,6 @@ 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,
@@ -757,7 +753,8 @@ 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 = calcTrueAirspeedSetpoint(eas_to_tas, EAS_setpoint);
+		control_setpoint.tas_setpoint = _control_param.tas_max * _fast_descend + (1 - _fast_descend) * eas_to_tas *
+						EAS_setpoint;

 		const TECSControl::Input control_input{ .altitude = altitude,
 							.altitude_rate = hgt_rate,
@@ -768,13 +765,11 @@ 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;
 }
@@ -783,22 +778,13 @@ 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)) {
-		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);
+		_fast_descend = 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,8 +50,6 @@
 #include <motion_planning/VelocitySmoothing.hpp>
 #include <motion_planning/ManualVelocitySmoothingZ.hpp>

-using namespace time_literals;
-
 class TECSAirspeedFilter
 {
 public:
@@ -205,8 +203,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 above trim allowed in [rad].
-		float pitch_min;			///< Minimal pitch angle below trim allowed in [rad].
+		float pitch_max;			///< Maximum pitch angle allowed in [rad].
+		float pitch_min;			///< Minimal pitch angle 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.
@@ -322,7 +320,7 @@ public:
 	/**
 	 * @brief Get the pitch setpoint.
 	 *
-	 * @return The commanded pitch angle above trim in [rad].
+	 * @return THe commanded pitch angle in [rad].
 	 */
 	float getPitchSetpoint() const {return _pitch_setpoint;};
 	/**
@@ -480,7 +478,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 above trim [rad].
+	 * @return pitch setpoint angle [rad].
 	 */
 	float _calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param,
 				      const Flag &flag) const;
@@ -539,7 +537,7 @@ private:

 	// Output
 	DebugOutput _debug_output;				///< Debug output.
-	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint above trim [rad].
+	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint [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]
 };
@@ -549,13 +547,11 @@ 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;
@@ -662,17 +658,6 @@ 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
 	 *
@@ -690,11 +675,9 @@ 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,8 +53,7 @@ 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 --git_tag '${PX4_GIT_TAG}'
+	COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py ${px4_git_ver_header} --validate
 	DEPENDS
 		${CMAKE_CURRENT_SOURCE_DIR}/px_update_git_header.py
 		${git_dir_path}/HEAD
@@ -10,7 +10,6 @@ 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',
@@ -37,11 +36,8 @@ header = """


 # PX4
-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(),
+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:
@@ -61,7 +57,17 @@ 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 not m:
+    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:
        print("")
        print("Error: the git tag '{:}' does not match the expected format.".format(git_tag_test))
        print("")
@@ -97,9 +103,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 main branch (used for metadata)
+    # either a release or master branch (used for metadata)
    if not tag_or_branch.startswith('release-'):
-        tag_or_branch = 'main'
+        tag_or_branch = 'master'

 header += f"""
 #define PX4_GIT_VERSION_STR "{git_version}"
@@ -109,7 +115,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 main branch
+#define PX4_GIT_TAG_OR_BRANCH_NAME "{tag_or_branch}" // special variable: git tag, release or master 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::stick_gesture: return "Stick gesture";
+	case arm_disarm_reason_t::rc_stick: return "RC";

-	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::stick_gesture
+		} else if (calling_reason == arm_disarm_reason_t::rc_stick
 			   || 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::stick_gesture)
+		const bool commanded_by_rc = (calling_reason == arm_disarm_reason_t::rc_stick)
 					     || (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::stick_gesture) {
+			if (calling_reason != arm_disarm_reason_t::rc_stick) {
 				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,23 +698,11 @@ Commander::Commander() :
 	// default for vtol is rotary wing
 	_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;

-	param_t param_mav_comp_id = param_find("MAV_COMP_ID");
-	param_t param_mav_sys_id = param_find("MAV_SYS_ID");
+	_param_mav_comp_id = param_find("MAV_COMP_ID");
+	_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();
 }

@@ -887,14 +875,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			}

 			if (desired_nav_state != vehicle_status_s::NAVIGATION_STATE_MAX) {
-
-				// 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)) {
+				if (_user_mode_intention.change(desired_nav_state, getSourceFromCommand(cmd))) {
 					main_ret = TRANSITION_CHANGED;

 				} else {
@@ -1069,13 +1050,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		break;

 	case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
-			// 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)) {
+			if (_user_mode_intention.change(vehicle_status_s::NAVIGATION_STATE_AUTO_LAND, getSourceFromCommand(cmd))) {
 				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");
@@ -1512,7 +1487,6 @@ 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;

@@ -1629,7 +1603,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_STICK_GESTURE
+	    && (action_request.source == action_request_s::SOURCE_RC_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)) {
@@ -1637,7 +1611,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 	}

 	switch (action_request.source) {
-	case action_request_s::SOURCE_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::stick_gesture; break;
+	case action_request_s::SOURCE_RC_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::rc_stick; break;

 	case action_request_s::SOURCE_RC_SWITCH: arm_disarm_reason = arm_disarm_reason_t::rc_switch; break;

@@ -1708,11 +1682,22 @@ 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,6 +324,8 @@ 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,8 +48,7 @@ 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)
-					|| (dist_sens.mode == distance_sensor_s::MODE_DISABLED));
+			valid = _distance_sensor_sub[instance].copy(&dist_sens) && hrt_elapsed_time(&dist_sens.timestamp) < 1_s;
 			reporter.setIsPresent(health_component_t::distance_sensor);
 		}

@@ -273,27 +273,12 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 		}

 		if (!context.isArmed() && ekf_gps_check_fail) {
-			NavModes required_groups_gps;
-			events::Log log_level;
+			NavModes required_groups_gps = required_groups;
+			events::Log log_level = events::Log::Error;

-			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:
+			if (_param_com_arm_wo_gps.get()) {
 				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
@@ -453,20 +438,11 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
 			}

 			if (message && reporter.mavlink_log_pub()) {
-				switch (static_cast<GnssArmingCheck>(_param_com_arm_wo_gps.get())) {
-				default:
-
-				/* FALLTHROUGH */
-				case GnssArmingCheck::DenyArming:
+				if (!_param_com_arm_wo_gps.get()) {
 					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;
+				} else {
+					mavlink_log_critical(reporter.mavlink_log_pub(), message, "");
 				}
 			}
 		}
@@ -59,12 +59,6 @@ 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);
@@ -114,7 +108,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,
-					(ParamInt<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
+					(ParamBool<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,14 +225,11 @@ PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f);
 PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f);

 /**
- * GPS preflight check
- *
- * Measures taken when a check defined by EKF2_GPS_CHECK is failing.
+ * Allow arming without GPS
 *
 * @group Commander
- * @value 0 Deny arming
- * @value 1 Warning only
- * @value 2 Disabled
+ * @value 0 Require GPS lock to arm
+ * @value 1 Allow arming without GPS
 */
 PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);

@@ -288,7 +285,7 @@ PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
 * @unit s
 * @min 0.0
 * @max 25.0
- * @decimal 1
+ * @decimal 3
 */
 PARAM_DEFINE_FLOAT(COM_FAIL_ACT_T, 5.f);

@@ -338,14 +335,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 Stabilized
- * @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 Manual
+ * @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);
@@ -453,12 +450,16 @@ PARAM_DEFINE_FLOAT(COM_RC_STICK_OV, 30.0f);
 PARAM_DEFINE_INT32(COM_ARM_MIS_REQ, 0);

 /**
- * Position mode navigation loss response
+ * Position control navigation loss response.
 *
- * This sets the flight mode that will be used if navigation accuracy is no longer adequate for position control in manual Position mode.
+ * This sets the flight mode that will be used if navigation accuracy is no longer adequate for position control.
 *
- * @value 0 Altitude mode
- * @value 1 Land mode (descend)
+ * 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
 *
 * @group Commander
 */
@@ -60,7 +60,6 @@ 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:
@@ -114,7 +113,6 @@ FailsafeBase::ActionOptions Failsafe::fromGfActParam(int param_value)

 	case geofence_violation_action::Land_mode:
 		options.action = Action::Land;
-		options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
 		break;

 	default:
@@ -292,7 +290,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::Stabilized:
+	case offboard_loss_failsafe_mode::Manual:
 		action = Action::FallbackStab;
 		user_intended_mode = vehicle_status_s::NAVIGATION_STATE_STAB;
 		break;
@@ -357,7 +355,6 @@ 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,
-		Stabilized = 2,
+		Manual = 2,
 		Return_mode = 3,
 		Land_mode = 4,
 		Hold_mode = 5,
@@ -111,7 +111,6 @@ 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
@@ -136,27 +135,24 @@ 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_MODULE_PARAMS params_barometer.yaml)
+	list(APPEND EKF_SRCS
+		EKF/aid_sources/barometer/baro_height_control.cpp
+	)
 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)
@@ -167,7 +163,6 @@ 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)
@@ -182,13 +177,10 @@ 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)
@@ -196,7 +188,6 @@ 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)
@@ -204,7 +195,6 @@ 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)
@@ -214,22 +204,18 @@ 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)
@@ -262,12 +248,9 @@ 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,8 +113,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed

 				} else {
 					// Try to initialize using measurement
-					if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
-							sample.epv)) {
+					if (ekf.setEkfGlobalOrigin(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,20 +75,55 @@ void Ekf::controlFakePosFusion()
 				      Vector2f(getStateVariance<State::pos>()) + obs_var, // innovation variance
 				      innov_gate);                                        // innovation gate

-		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;
+		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;

-		_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);
+		if (_control_status.flags.fake_pos) {
+			if (enable_conditions_passing) {

-	} 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;
+				// 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();
 	}
 }

@@ -103,41 +138,10 @@ void Ekf::resetFakePosFusion()
 	_aid_src_fake_pos.time_last_fuse = _time_delayed_us;
 }

-bool Ekf::runFakePosStateMachine(const bool enable_conditions_passing, bool status_flag,
-				 estimator_aid_source2d_s &aid_src)
+void Ekf::stopFakePosFusion()
 {
-	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();
-		}
+	if (_control_status.flags.fake_pos) {
+		ECL_INFO("stop fake position fusion");
+		_control_status.flags.fake_pos = false;
 	}
-
-	return status_flag;
 }
@@ -41,6 +41,10 @@

 #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
@@ -59,27 +63,46 @@ 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()) {
-			setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc);
+			_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);
+			}
 		}

 		// 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)) {
-			setAltOriginFromCurrentPos(gps.alt, gps.vacc);
+			_gps_alt_ref = gps.alt + _state.pos(2);
 		}

+		_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");
+	}

-		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
+	if ((isTimedOut(_wmm_gps_time_last_checked, 1e6)) || (_wmm_gps_time_last_set == 0)) {
+		// a rough 2D fix is sufficient to lookup declination
 		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,8 +39,6 @@
 #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)
@@ -68,9 +66,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 			_control_status.flags.mag_fault = false;

 			_state.mag_B.zero();
-			resetMagBiasCov();
-
-			stopMagFusion();
+			resetMagCov();

 			_mag_lpf.reset(mag_sample.mag);
 			_mag_counter = 1;
@@ -80,43 +76,15 @@ 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)
-		    && (_wmm_earth_field_gauss.isAllFinite() && _wmm_earth_field_gauss.longerThan(0.f))
+		    && (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps))
 		   ) {
-			mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, _wmm_earth_field_gauss);
+			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);

 			_control_status.flags.synthetic_mag_z = true;

@@ -172,6 +140,8 @@ 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;


@@ -205,14 +175,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_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;
+		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;

 		if (_control_status.flags.mag) {

 			if (continuing_conditions_passing && _control_status.flags.yaw_align) {

-				if (checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_not_moving)) {
+				if (mag_sample.reset || checkHaglYawResetReq() || (wmm_updated && no_ne_aiding_or_pre_takeoff)) {
 					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;
@@ -233,35 +203,14 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 					}

 					if (_control_status.flags.mag_dec) {
-
-						// 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);
-						}
+						fuseDeclination(0.5f);
 					}
 				}

 				const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.reset_timeout_max);

 				if (is_fusion_failing) {
-					if (no_ne_aiding_or_not_moving) {
+					if (no_ne_aiding_or_pre_takeoff) {
 						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;
@@ -322,9 +271,6 @@ 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;
@@ -383,36 +329,29 @@ 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;

-	static constexpr float kMagEarthMinGauss = 0.01f; // minimum difference in mag earth field strength for reset (Gauss)
+	// reset covariances to default
+	resetMagCov();

 	// if world magnetic model (inclination, declination, strength) available then use it to reset mag states
-	if (_wmm_earth_field_gauss.longerThan(0.f) && _wmm_earth_field_gauss.isAllFinite()) {
+	if (PX4_ISFINITE(_mag_inclination_gps) && PX4_ISFINITE(_mag_declination_gps) && PX4_ISFINITE(_mag_strength_gps)) {
+
 		// 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_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
+		// mag_B: reset
 		if (!reset_heading && _control_status.flags.yaw_align) {
-			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)
+			// mag_B: reset using WMM
+			const Dcmf R_to_body = quatToInverseRotMat(_state.quat_nominal);
+			_state.mag_B = mag - (R_to_body * mag_earth_pred);

 		} 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);
 		}
@@ -420,32 +359,34 @@ 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 heading
+		// Use the magnetometer measurement to reset the field states
 		if (reset_heading) {
 			resetMagHeading(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();
-		}
+		// mag_I: use the last magnetometer measurements to reset the field states
+		_state.mag_I = _R_to_earth * mag;
 	}

-	if ((_state.mag_I - mag_I_before_reset).longerThan(0.f)) {
+	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 {
 		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 ((_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));
+		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)
+				);
+		}
 	}

 	// record the start time for the magnetic field alignment
@@ -510,8 +451,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(_wmm_field_strength_gauss)) {
-			if (!isMeasuredMatchingExpected(_mag_strength, _wmm_field_strength_gauss, _params.mag_check_strength_tolerance_gs)) {
+		if (PX4_ISFINITE(_mag_strength_gps)) {
+			if (!isMeasuredMatchingExpected(_mag_strength, _mag_strength_gps, _params.mag_check_strength_tolerance_gs)) {
 				_control_status.flags.mag_field_disturbed = true;
 				is_check_failing = true;
 			}
@@ -534,9 +475,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(_wmm_inclination_rad)) {
+		if (PX4_ISFINITE(_mag_inclination_gps)) {
 			const float inc_tol_rad = radians(_params.mag_check_inclination_tolerance_deg);
-			const float inc_error_rad = wrap_pi(_mag_inclination - _wmm_inclination_rad);
+			const float inc_error_rad = wrap_pi(_mag_inclination - _mag_inclination_gps);

 			if (fabsf(inc_error_rad) > inc_tol_rad) {
 				_control_status.flags.mag_field_disturbed = true;
@@ -608,60 +549,13 @@ 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)
-		   && 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;
+	} 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;
 		}
 	}

-	return false;
+	// otherwise use the parameter value
+	return math::radians(_params.mag_declination_deg);
 }
@@ -99,8 +99,7 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 				resetQuatCov(_params.mag_heading_noise);
 			}

-			resetMagEarthCov();
-			resetMagBiasCov();
+			resetMagCov();

 			return false;
 		}
@@ -151,44 +150,51 @@ bool Ekf::fuseMag(const Vector3f &mag, const float R_MAG, VectorState &H, estima
 	return false;
 }

-bool Ekf::fuseDeclination(float decl_measurement_rad, float R, bool update_all_states)
+bool Ekf::fuseDeclination(float decl_sigma)
 {
-	VectorState H;
-	float decl_pred;
-	float innovation_variance;
+	float decl_measurement = NAN;

-	sym::ComputeMagDeclinationPredInnovVarAndH(_state.vector(), P, R, FLT_EPSILON,
-			&decl_pred, &innovation_variance, &H);
+	if ((_params.mag_declination_source & GeoDeclinationMask::USE_GEO_DECL)
+	    && PX4_ISFINITE(_mag_declination_gps)
+	   ) {
+		decl_measurement = _mag_declination_gps;

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

-	// Calculate the Kalman gains
-	VectorState Kfusion = P * H / innovation_variance;
+	if (PX4_ISFINITE(decl_measurement)) {

-	if (!update_all_states) {
-		// zero non-mag Kalman gains if not updating all states
+		// observation variance (rad**2)
+		const float R_DECL = sq(decl_sigma);

-		// 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);
+		VectorState H;
+		float decl_pred;
+		float innovation_variance;

-		// 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;
+		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;
 	}

-	const bool is_fused = measurementUpdate(Kfusion, H, R, innovation);
-
-	_fault_status.flags.bad_mag_decl = !is_fused;
-
-	return is_fused;
+	return false;
 }

 float Ekf::calculate_synthetic_mag_z_measurement(const Vector3f &mag_meas, const Vector3f &mag_earth_predicted)
@@ -122,16 +122,8 @@ 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
@@ -152,7 +144,6 @@ 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

@@ -169,7 +160,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(flow_sample);
+						resetFlowFusion();

 						if (_control_status.flags.opt_flow_terrain && !isTerrainEstimateValid()) {
 							resetTerrainToFlow();
@@ -203,7 +194,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				} else {
 					if (isTerrainEstimateValid() || (_height_sensor_ref == HeightSensor::RANGE)) {
 						ECL_INFO("starting optical flow, resetting");
-						resetFlowFusion(flow_sample);
+						resetFlowFusion();
 						_control_status.flags.opt_flow = true;

 					} else if (_control_status.flags.opt_flow_terrain) {
@@ -222,13 +213,11 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 	}
 }

-void Ekf::resetFlowFusion(const flowSample &flow_sample)
+void Ekf::resetFlowFusion()
 {
 	ECL_INFO("reset velocity to flow");
 	_information_events.flags.reset_vel_to_flow = true;
-
-	const float flow_vel_var = sq(predictFlowRange()) * calcOptFlowMeasVar(flow_sample);
-	resetHorizontalVelocityTo(getFilteredFlowVelNE(), flow_vel_var);
+	resetHorizontalVelocityTo(_flow_vel_ne, calcOptFlowMeasVar(_flow_sample_delayed));

 	// reset position, estimate is relative to initial position in this mode, so we start with zero error
 	if (!_control_status.flags.in_air) {
@@ -282,8 +271,6 @@ 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,7 +54,6 @@ 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,9 +84,8 @@ void SensorRangeFinder::updateValidity(uint64_t current_time_us)

 			if (isTiltOk() && isDataInRange()) {
 				updateStuckCheck();
-				updateFogCheck(getDistBottom(), _sample.time_us);

-				if (!_is_stuck && !_is_blocked) {
+				if (!_is_stuck) {
 					_is_sample_valid = true;
 					_time_last_valid_us = _sample.time_us;
 				}
@@ -147,23 +146,5 @@ 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
Jaeyoung Lim	022756732e	Add ifdefs for airspeed gz msgs	2024-08-21 09:44:14 +02:00
Jaeyoung Lim	00e783766a	Revert "Revert "Update GZBridge to be able to use gazebo airspeed. Add quadtailsitter. (#23455 )""	2024-08-21 09:36:30 +02:00