ci: github actions: reenable some tests to increase coverage

rc.serial_port.jinja: Remove 'x' prefix in variable comparisons to resolve ShellCheck warnings
ci: github actions: match checkout actions to fix ownership problem
2026-05-27 02:40:06 +08:00 · 2024-05-18 20:20:38 +01:00 · 2024-05-18 20:15:27 +01:00 · 2024-05-18 20:15:27 +01:00 · 2024-05-18 20:15:26 +01:00 · 2024-05-18 20:15:26 +01:00
110 changed files with 719 additions and 645 deletions
@@ -1,7 +1,7 @@
 root = true

 [*]
-insert_final_newline = false
+insert_final_newline = true

 [{*.{c,cpp,cc,h,hpp},CMakeLists.txt,Kconfig}]
 indent_style = tab
@@ -28,12 +28,14 @@ jobs:
          "parameters_metadata",
        ]
    container:
-      image: px4io/px4-dev-nuttx-focal:2022-08-12
+      image: px4io/px4-dev-nuttx-jammy:2024-05-18
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: check environment
      run: |
@@ -11,11 +11,13 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-clang:2021-09-08
+    container: px4io/px4-dev-clang:2024-05-18
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{secrets.ACCESS_TOKEN}}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: make clang-tidy-quiet
      run: make clang-tidy-quiet
@@ -14,7 +14,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-armhf:2023-06-26
+    container: px4io/px4-dev-armhf:2024-05-18
    strategy:
      matrix:
        config: [
@@ -28,7 +28,7 @@ jobs:
      with:
        token: ${{secrets.ACCESS_TOKEN}}
    - name: ownership workaround
-      run: git config --system --add safe.directory '*'
+      run: git config --global --add safe.directory '*'
    - name: Prepare ccache timestamp
      id: ccache_cache_timestamp
      shell: cmake -P {0}
@@ -14,7 +14,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-aarch64:2022-08-12
+    container: px4io/px4-dev-aarch64:2024-05-18
    strategy:
      matrix:
        config: [
@@ -24,6 +24,8 @@ jobs:
    - uses: actions/checkout@v1
      with:
        token: ${{secrets.ACCESS_TOKEN}}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: Prepare ccache timestamp
      id: ccache_cache_timestamp
@@ -14,7 +14,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2022-08-12
+    container: px4io/px4-dev-nuttx-jammy:2024-05-18
    strategy:
      fail-fast: false
      matrix:
@@ -83,6 +83,8 @@ jobs:
    - uses: actions/checkout@v1
      with:
        token: ${{secrets.ACCESS_TOKEN}}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: Prepare ccache timestamp
      id: ccache_cache_timestamp
@@ -10,13 +10,15 @@ on:
 jobs:
  enumerate_targets:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
    outputs:
      matrix: ${{ steps.set-matrix.outputs.matrix }}
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{secrets.ACCESS_TOKEN}}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
    - id: set-matrix
      run: echo "::set-output name=matrix::$(./Tools/generate_board_targets_json.py)"
  build:
@@ -29,9 +31,11 @@ jobs:
    - uses: actions/checkout@v1
      with:
        token: ${{secrets.ACCESS_TOKEN}}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: ownership workaround
-      run: git config --system --add safe.directory '*'
+      run: git config --global --add safe.directory '*'

    - name: make ${{matrix.target}}
      run: make ${{matrix.target}}
@@ -5,11 +5,13 @@ on: pull_request
 jobs:
  unit_tests:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
    steps:
-    - uses: actions/checkout@v2.3.1
+    - uses: actions/checkout@v1
      with:
-        fetch-depth: 0
+        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
    - name: checkout newest version of branch
      run: |
        git fetch origin pull/${{github.event.pull_request.number}}/head:${{github.head_ref}}
@@ -5,14 +5,16 @@ on: push
 jobs:
  unit_tests:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
    env:
      GIT_COMMITTER_EMAIL: bot@px4.io
      GIT_COMMITTER_NAME: PX4BuildBot
    steps:
-    - uses: actions/checkout@v2.3.1
+    - uses: actions/checkout@v1
      with:
-        fetch-depth: 0
+        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'
    - name: main test updates change indication files
      run: make tests TESTFILTER=EKF
    - name: Check if there exists diff and save result in variable
@@ -21,12 +21,14 @@ jobs:
          "failsafe_web",
        ]
    container:
-      image: px4io/px4-dev-nuttx-focal:2022-08-12
+      image: px4io/px4-dev-nuttx-jammy:2024-05-18
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: check environment
      run: |
@@ -17,20 +17,22 @@ jobs:
        config:
          - {vehicle: "iris",          mission: "MC_mission_box",  build_type: "RelWithDebInfo"}
          - {vehicle: "rover",         mission: "rover_mission_1", build_type: "RelWithDebInfo"}
-          #- {vehicle: "plane",         mission: "FW_mission_1",    build_type: "RelWithDebInfo"}
+          - {vehicle: "plane",         mission: "FW_mission_1",    build_type: "RelWithDebInfo"}
          #- {vehicle: "plane_catapult",mission: "FW_mission_1",    build_type: "RelWithDebInfo"}
          #- {vehicle: "standard_vtol", mission: "VTOL_mission_1",  build_type: "Coverage"}
          #- {vehicle: "standard_vtol", mission: "VTOL_mission_1",  build_type: "AddressSanitizer"}
-          #- {vehicle: "tailsitter",    mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}
-          #- {vehicle: "tiltrotor",     mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}
+          - {vehicle: "tailsitter",    mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}
+          - {vehicle: "tiltrotor",     mission: "VTOL_mission_1",  build_type: "RelWithDebInfo"}

    container:
-      image: px4io/px4-dev-ros-melodic:2021-09-08
+      image: px4io/px4-dev-ros-noetic:2024-05-18
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: Prepare ccache timestamp
      id: ccache_cache_timestamp
@@ -20,12 +20,14 @@ jobs:
          #- {test_file: "mavros_posix_tests_offboard_rpyrt_ctl.test", vehicle: "iris", build_type: "RelWithDebInfo"}

    container:
-      image: px4io/px4-dev-ros-melodic:2021-09-08
+      image: px4io/px4-dev-ros-noetic:2024-05-18
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: Prepare ccache timestamp
      id: ccache_cache_timestamp
@@ -11,11 +11,13 @@ jobs:

  airframe:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-focal:2024-05-18
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: airframe metadata
      run: |
@@ -37,11 +39,13 @@ jobs:

  module:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: module documentation
      run: |
@@ -52,11 +56,13 @@ jobs:

  parameter:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: parameter metadata
      run: |
@@ -76,11 +82,13 @@ jobs:

  events:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: events metadata
      run: |
@@ -103,11 +111,13 @@ jobs:

  uorb_graph:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2022-08-12
+    container: px4io/px4-dev-nuttx-jammy:2024-05-18
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: uORB graph
      run: |
@@ -118,11 +128,13 @@ jobs:

  ROS2_msgs:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
+    container: px4io/px4-dev-base-jammy:2024-05-18
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: PX4 ROS2 msgs
      run: |
@@ -11,7 +11,7 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2022-08-12
+    container: px4io/px4-dev-nuttx-jammy:2024-05-18
    strategy:
      matrix:
        config: [
@@ -21,6 +21,8 @@ jobs:
    - uses: actions/checkout@v1
      with:
        token: ${{secrets.ACCESS_TOKEN}}
+    - name: ownership workaround
+      run: git config --global --add safe.directory '*'

    - name: make ${{matrix.config}}
      env:
@@ -21,12 +21,14 @@ jobs:
          - {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
+      image: px4io/px4-dev-simulation-focal:2024-05-18
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
    - uses: actions/checkout@v1
      with:
        token: ${{ secrets.ACCESS_TOKEN }}
+    - name: ownership workaround
+      run: git config --global --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"
@@ -6,7 +6,7 @@ function check_git_submodule {
 if [[ -f $1"/.git" || -d $1"/.git" ]]; then

 	# always update within CI environment or configuring withing VSCode CMake where you can't interact
-	if [ "$CI" == "true" ] || [ -n "${VSCODE_PID+set}" ]; then
+	if [ "$CI" == "true" ] || [ -n "${VSCODE_PID+set}" ] || [ -n "${CLION_IDE+set}" ]; then
 		git submodule --quiet sync --recursive -- $1
 		git submodule --quiet update --init --recursive --jobs=8 -- $1  || true
 		git submodule --quiet sync --recursive -- $1
@@ -5,7 +5,7 @@
 set SERIAL_DEV none
 {% for serial_device in serial_devices -%}
 if param compare "$PRT" {{ serial_device.index }}; then
-	if [ "x$PRT_{{ serial_device.tag }}_" = "x" ]; then
+	if [ "$PRT_{{ serial_device.tag }}_" = "" ]; then
 		set SERIAL_DEV {{ serial_device.device }}
 		set BAUD_PARAM SER_{{ serial_device.tag }}_BAUD
 		set PRT_{{ serial_device.tag }}_ 1
@@ -1,13 +1,13 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
-CONFIG_BOARD_NO_HELP=y
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_MAGNETOMETER_BOSCH_BMM150=y
+CONFIG_DRIVERS_MAGNETOMETER_ST_IIS2MDC=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_BOARD_UAVCAN_INTERFACES=1
@@ -8,4 +8,7 @@ icm42688p -R 0 -s start

 bmp388 -I -b 1 start

-bmm150 -I -b 1 start
+if ! iis2mdc -R 4 -I -b 1 start
+then
+	bmm150 -I -b 1 start
+fi
@@ -9,7 +9,7 @@ When running PX4 directly on the QRB5165 SoC it runs partially on the Sensor Low
 The portion running on the DSP hosts the flight critical portions of PX4 such as
 the IMU, barometer, magnetometer, GPS, ESC, and power management drivers, and the
 state estimation. The DSP acts as the real time portion of the system. Non flight
-critical applications such as Mavlink, logging, and commander are running on the
+critical applications such as Mavlink, and logging are running on the
 ARM CPU cluster (aka apps proc). The DSP and ARM CPU cluster communicate via a
 Qualcomm proprietary shared memory interface.

@@ -27,6 +27,7 @@ The full instructions are available here:
 ```
 px4$ boards/modalai/voxl2/scripts/run-docker.sh
 root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/clean.sh
+root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-deps.sh
 root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-apps.sh
 root@9373fa1401b8:/usr/local/workspace# boards/modalai/voxl2/scripts/build-slpi.sh
 root@9373fa1401b8:/usr/local/workspace# exit
@@ -69,16 +70,15 @@ pxh>
 ## Notes

 You cannot cleanly shutdown PX4 with the shutdown command on VOXL 2. You have
-to power cycle the board and restart everything.
+to power cycle the board and restart everything. Starting with SDK 1.3.0 it is possible
+to cleanly shutdown PX4 on VOXL 2.

 ## Tips

-Start with a VOXL 2 that only has the system image installed, not the SDK
-
-Run the command ```voxl-px4 -s``` on target to run the self-test
+Always use the latest SDK release

 In order to see DSP specific debug messages the mini-dm tool in the Hexagon SDK
-can be used:
+can be used (Most messages are passed to the apps proc but certain low level messages are not):
 ```
 modalai@modalai-XPS-15-9570:/local/mnt/workspace/Qualcomm/Hexagon_SDK/4.1.0.4/tools/debug/mini-dm/Ubuntu18$ sudo ./mini-dm
 [sudo] password for modalai:
@@ -20,7 +20,6 @@ adb shell chmod a+x /usr/bin/voxl-px4-hitl-start

 # Push configuration file
 adb shell mkdir -p /etc/modalai
-adb push boards/modalai/voxl2/target/voxl-px4-set-default-parameters.config /etc/modalai
 adb push boards/modalai/voxl2/target/voxl-px4-fake-imu-calibration.config /etc/modalai
 adb push boards/modalai/voxl2/target/voxl-px4-hitl-set-default-parameters.config /etc/modalai

@@ -129,12 +129,6 @@ else
    DAEMON="-d"
 fi

-if [ ! -f /data/px4/param/parameters ]; then
-   echo "[INFO] Setting default parameters for PX4 on voxl"
-   px4 $DAEMON -s /etc/modalai/voxl-px4-set-default-parameters.config
-   /bin/sync
-fi
-
 if [ $SENSOR_CAL == "FAKE" ]; then
   /bin/echo "[INFO] Setting up fake sensor calibration values"
   px4 $DAEMON -s /etc/modalai/voxl-px4-fake-imu-calibration.config
@@ -1,192 +0,0 @@
-#!/bin/sh
-# PX4 commands need the 'px4-' prefix in bash.
-# (px4-alias.sh is expected to be in the PATH)
-. px4-alias.sh
-
-param select /data/px4/param/parameters
-
-# Make sure we are running at 800Hz on IMU
-param set IMU_GYRO_RATEMAX 800
-
-# EKF2 Parameters
-param set EKF2_IMU_POS_X 0.027
-param set EKF2_IMU_POS_Y 0.009
-param set EKF2_IMU_POS_Z -0.019
-param set EKF2_EV_DELAY 5
-param set EKF2_AID_MASK 280
-param set EKF2_ABL_LIM 0.8
-param set EKF2_TAU_POS 0.25
-param set EKF2_TAU_VEL 0.25
-
-param set MC_AIRMODE 0
-
-param set MC_YAW_P 2.0
-param set MC_YAWRATE_P 0.15
-param set MC_YAWRATE_I 0.1
-param set MC_YAWRATE_D 0.0
-param set MC_YAWRATE_K 1.0
-
-param set MC_PITCH_P 5.5
-param set MC_PITCHRATE_P 0.08
-param set MC_PITCHRATE_I 0.2
-param set MC_PITCHRATE_D 0.0013
-param set MC_PITCHRATE_K 1.0
-
-param set MC_ROLL_P 5.5
-param set MC_ROLLRATE_P 0.08
-param set MC_ROLLRATE_I 0.2
-param set MC_ROLLRATE_D 0.0013
-param set MC_ROLLRATE_K 1.0
-
-param set MPC_VELD_LP 5.0
-
-# tweak MPC_THR_MIN to prevent roll/pitch losing control
-# authority under rapid downward acceleration
-param set MPC_THR_MAX 0.75
-param set MPC_THR_MIN 0.08
-param set MPC_THR_HOVER 0.42
-param set MPC_MANTHR_MIN 0.05
-
-# default position mode with a little expo, smooth mode is terrible
-param set MPC_POS_MODE 0
-param set MPC_YAW_EXPO 0.20
-param set MPC_XY_MAN_EXPO 0.20
-param set MPC_Z_MAN_EXPO 0.20
-
-# max velocities
-param set MPC_VEL_MANUAL 5.0
-param set MPC_XY_VEL_MAX 5.0
-param set MPC_XY_CRUISE 5.0
-param set MPC_Z_VEL_MAX_DN 1.5
-param set MPC_Z_VEL_MAX_UP 4.0
-param set MPC_LAND_SPEED 1.0
-
-# Horizontal position PID
-param set MPC_XY_P 0.95
-param set MPC_XY_VEL_P_ACC 3.00
-param set MPC_XY_VEL_I_ACC 0.10
-param set MPC_XY_VEL_D_ACC 0.00
-
-# Vertical position PID
-# PX4 Defaults
-param set MPC_Z_P 1.0
-param set MPC_Z_VEL_P_ACC 8.0
-param set MPC_Z_VEL_I_ACC 2.0
-param set MPC_Z_VEL_D_ACC 0.0
-
-param set MPC_TKO_RAMP_T 1.50
-param set MPC_TKO_SPEED	1.50
-
-# Set the ESC outputs to function as motors
-param set VOXL_ESC_FUNC1 101
-param set VOXL_ESC_FUNC2 103
-param set VOXL_ESC_FUNC3 104
-param set VOXL_ESC_FUNC4 102
-
-param set VOXL_ESC_SDIR1 0
-param set VOXL_ESC_SDIR2 0
-param set VOXL_ESC_SDIR3 0
-param set VOXL_ESC_SDIR4 0
-
-param set VOXL_ESC_CONFIG 1
-param set VOXL_ESC_REV 0
-param set VOXL_ESC_MODE 0
-param set VOXL_ESC_BAUD 2000000
-param set VOXL_ESC_RPM_MAX 10500
-param set VOXL_ESC_RPM_MIN 1000
-
-# Set the Voxl2 IO outputs to function as motors
-param set VOXL2_IO_FUNC1 101
-param set VOXL2_IO_FUNC2 102
-param set VOXL2_IO_FUNC3 103
-param set VOXL2_IO_FUNC4 104
-
-param set VOXL2_IO_BAUD 921600
-param set VOXL2_IO_MIN 1000
-param set VOXL2_IO_MAX 2000
-
-# Some parameters for control allocation
-param set CA_ROTOR_COUNT 4
-param set CA_R_REV 0
-param set CA_AIRFRAME 0
-param set CA_ROTOR_COUNT 4
-param set CA_ROTOR0_PX 0.15
-param set CA_ROTOR0_PY 0.15
-param set CA_ROTOR1_PX -0.15
-param set CA_ROTOR1_PY -0.15
-param set CA_ROTOR2_PX 0.15
-param set CA_ROTOR2_PY -0.15
-param set CA_ROTOR2_KM -0.05
-param set CA_ROTOR3_PX -0.15
-param set CA_ROTOR3_PY 0.15
-param set CA_ROTOR3_KM -0.05
-
-# Some parameter settings to disable / ignore certain preflight checks
-
-# No GPS driver yet so disable it
-param set SYS_HAS_GPS 0
-
-# Allow arming wihtout a magnetometer (Need magnetometer driver)
-param set SYS_HAS_MAG 0
-param set EKF2_MAG_TYPE	5
-
-# Allow arming without battery check (Need voxlpm driver)
-param set CBRK_SUPPLY_CHK 894281
-
-# Allow arming without an SD card
-param set COM_ARM_SDCARD 0
-
-# Allow arming wihtout CPU load information
-param set COM_CPU_MAX 0.0
-
-# Disable auto disarm. This is number of seconds to wait for takeoff
-# after arming. If no takeoff happens then it will disarm. A negative
-# value disables this.
-param set COM_DISARM_PRFLT -1
-
-# This parameter doesn't exist anymore. Need to see what the new method is.
-# param set MAV_BROADCAST 0
-
-# Doesn't work without setting this to Quadcopter
-param set MAV_TYPE 2
-
-# Parameters that we don't use but QGC complains about if they aren't there
-param set SYS_AUTOSTART 4001
-
-# Default RC channel mappings
-param set RC_MAP_ACRO_SW 0
-param set RC_MAP_ARM_SW 0
-param set RC_MAP_AUX1 0
-param set RC_MAP_AUX2 0
-param set RC_MAP_AUX3 0
-param set RC_MAP_AUX4 0
-param set RC_MAP_AUX5 0
-param set RC_MAP_AUX6 0
-param set RC_MAP_FAILSAFE 0
-param set RC_MAP_FLAPS 0
-param set RC_MAP_FLTMODE 6
-param set RC_MAP_GEAR_SW 0
-param set RC_MAP_KILL_SW 7
-param set RC_MAP_LOITER_SW 0
-param set RC_MAP_MAN_SW 0
-param set RC_MAP_MODE_SW 0
-param set RC_MAP_OFFB_SW 0
-param set RC_MAP_PARAM1 0
-param set RC_MAP_PARAM2 0
-param set RC_MAP_PARAM3 0
-param set RC_MAP_PITCH 2
-param set RC_MAP_POSCTL_SW 0
-param set RC_MAP_RATT_SW 0
-param set RC_MAP_RETURN_SW 0
-param set RC_MAP_ROLL 1
-param set RC_MAP_STAB_SW 0
-param set RC_MAP_THROTTLE 3
-param set RC_MAP_TRANS_SW 0
-param set RC_MAP_YAW 4
-
-param save
-
-sleep 2
-
-# Need px4-shutdown otherwise Linux system shutdown is called
-px4-shutdown
@@ -33,6 +33,7 @@ CONFIG_DRIVERS_OSD_MSP_OSD=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
+CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_RC_INPUT=y
@@ -84,6 +85,7 @@ CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
+CONFIG_SYSTEMCMDS_I2C_LAUNCHER=y
 CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_IO_BYPASS_CONTROL=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -12,9 +12,11 @@ param set-default MAV_2_RATE 100000
 param set-default MAV_2_REMOTE_PRT 14550
 param set-default MAV_2_UDP_PRT 14550

+# By disabling all 3 INA modules, we use the
+# i2c_launcher instead.
 param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
-param set-default SENS_EN_INA226 1
+param set-default SENS_EN_INA226 0

 safety_button start

@@ -17,6 +17,7 @@
 #------------------------------------------------------------------------------

 set HAVE_PM2 yes
+set INA_CONFIGURED no

 if mft query -q -k MFT -s MFT_PM2 -v 0
 then
@@ -39,6 +40,8 @@ then
 	then
 		ina226 -X -b 2 -t 2 -k start
 	fi
+
+	set INA_CONFIGURED yes
 fi

 if param compare SENS_EN_INA228 1
@@ -49,6 +52,8 @@ then
 	then
 		ina228 -X -b 2 -t 2 -k start
 	fi
+
+	set INA_CONFIGURED yes
 fi

 if param compare SENS_EN_INA238 1
@@ -59,6 +64,25 @@ then
 	then
 		ina238 -X -b 2 -t 2 -k start
 	fi
+
+	set INA_CONFIGURED yes
+fi
+
+#Start Auterion Power Module selector for Skynode boards
+if ver hwbasecmp 009 010
+then
+	pm_selector_auterion start
+else
+	if [ $INA_CONFIGURED = no ]
+	then
+		# INA226, INA228, INA238 auto-start
+		i2c_launcher start -b 1
+		if [ $HAVE_PM2 = yes ]
+		then
+			i2c_launcher start -b 2
+		fi
+	fi
+fi
 fi

 # Internal SPI bus ICM42686p (hard-mounted)
@@ -88,4 +112,5 @@ fi

 bmp388 -X -b 2 start

+unset INA_CONFIGURED
 unset HAVE_PM2
@@ -46,7 +46,10 @@ CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
 CONFIG_CDCACM_VENDORID=0x3643
 CONFIG_CDCACM_VENDORSTR="Dronecode Project, Inc."
+CONFIG_DEBUG_ERROR=y
+CONFIG_DEBUG_FEATURES=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_MEMFAULT=y
 CONFIG_DEBUG_SYMBOLS=y
 CONFIG_DEBUG_TCBINFO=y
 CONFIG_DEV_FIFO_SIZE=0
@@ -257,7 +260,6 @@ CONFIG_SCHED_INSTRUMENTATION_SWITCH=y
 CONFIG_SCHED_LPWORK=y
 CONFIG_SCHED_LPWORKPRIORITY=50
 CONFIG_SCHED_LPWORKSTACKSIZE=2032
-CONFIG_SCHED_WAITPID=y
 CONFIG_SDIO_BLOCKSETUP=y
 CONFIG_SEM_PREALLOCHOLDERS=32
 CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
@@ -276,6 +278,7 @@ CONFIG_SYSTEM_CLE=y
 CONFIG_SYSTEM_DHCPC_RENEW=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_SYSTEM_PING=y
+CONFIG_SYSTEM_SYSTEM=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 #***************************************************************************
 #
 #   Copyright (c) 2015 PX4 Development Team. All rights reserved.
@@ -35,8 +35,7 @@
 #
 # @author Andreas Antener <andreas@uaventure.com>
 #
-# The shebang of this file is currently Python2 because some
-# dependencies such as pymavlink don't play well with Python3 yet.
+
 from __future__ import division

 PKG = 'px4'
@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 #***************************************************************************
 #
 #   Copyright (c) 2015 PX4 Development Team. All rights reserved.
@@ -35,8 +35,7 @@
 #
 # @author Andreas Antener <andreas@uaventure.com>
 #
-# The shebang of this file is currently Python2 because some
-# dependencies such as pymavlink don't play well with Python3 yet.
+
 from __future__ import division

 PKG = 'px4'
@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 #***************************************************************************
 #
 #   Copyright (c) 2020 PX4 Development Team. All rights reserved.
@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 from __future__ import division

 import unittest
@@ -1,4 +1,4 @@
-#!/usr/bin/env python2
+#!/usr/bin/env python
 #***************************************************************************
 #
 #   Copyright (c) 2015-2016 PX4 Development Team. All rights reserved.
@@ -36,8 +36,6 @@
 # @author Andreas Antener <andreas@uaventure.com>
 #

-# The shebang of this file is currently Python2 because some
-# dependencies such as pymavlink don't play well with Python3 yet.
 from __future__ import division

 PKG = 'px4'
@@ -89,7 +89,7 @@ static constexpr wq_config_t ttyS9{"wq:ttyS9", 1728, -30};
 static constexpr wq_config_t ttyACM0{"wq:ttyACM0", 1728, -31};
 static constexpr wq_config_t ttyUnknown{"wq:ttyUnknown", 1728, -32};

-static constexpr wq_config_t lp_default{"wq:lp_default", 1920, -50};
+static constexpr wq_config_t lp_default{"wq:lp_default", 2000, -50};

 static constexpr wq_config_t test1{"wq:test1", 2000, 0};
 static constexpr wq_config_t test2{"wq:test2", 2000, 0};
@@ -728,7 +728,8 @@ GPS::run()
 		int32_t gps_ubx_mode = 0;
 		param_get(handle, &gps_ubx_mode);

-		if (gps_ubx_mode == 1) { // heading
+		switch (gps_ubx_mode) {
+		case 1:  // heading
 			if (_instance == Instance::Main) {
 				ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBase;

@@ -736,10 +737,13 @@ GPS::run()
 				ubx_mode = GPSDriverUBX::UBXMode::MovingBase;
 			}

-		} else if (gps_ubx_mode == 2) {
-			ubx_mode = GPSDriverUBX::UBXMode::MovingBase;
+			break;

-		} else if (gps_ubx_mode == 3) {
+		case 2:
+			ubx_mode = GPSDriverUBX::UBXMode::MovingBase;
+			break;
+
+		case 3:
 			if (_instance == Instance::Main) {
 				ubx_mode = GPSDriverUBX::UBXMode::RoverWithMovingBaseUART1;

@@ -747,11 +751,18 @@ GPS::run()
 				ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1;
 			}

-		} else if (gps_ubx_mode == 4) {
-			ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1;
+			break;

-		} else if (gps_ubx_mode == 5) { // rover with static base on Uart2
+		case 4:
+			ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1;
+			break;
+
+		case 5:  // rover with static base on Uart2
 			ubx_mode = GPSDriverUBX::UBXMode::RoverWithStaticBaseUart2;
+			break;
+
+		default:
+			break;

 		}
 	}
@@ -1561,4 +1572,4 @@ int
 gps_main(int argc, char *argv[])
 {
 	return GPS::main(argc, argv);
-}
+}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-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
@@ -152,8 +152,9 @@ PARAM_DEFINE_INT32(GPS_UBX_CFG_INTF, 0);
 *
 * The offset angle increases clockwise.
 *
- * Set this to 90 if the rover (or Unicore primary) antenna is placed on the
- * right side of the vehicle and the moving base antenna is on the left side.
+ * Set this to 90 if the rover (or Unicore primary, or Septentrio Mosaic Aux)
+ * antenna is placed on the right side of the vehicle and the moving base
+ * antenna is on the left side.
 *
 * (Note: the Unicore primary antenna is the one connected on the right as seen
 *        from the top).
@@ -172,7 +172,9 @@ void ADIS16507::RunImpl()
 			const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT);

 			if (DIAG_STAT != 0) {
-				PX4_ERR("DIAG_STAT: %#X", DIAG_STAT);
+				PX4_ERR("self test failed, resetting. DIAG_STAT: %#X", DIAG_STAT);
+				_state = STATE::RESET;
+				ScheduleDelayed(3_s);

 			} else {
 				PX4_DEBUG("self test passed");
@@ -832,7 +832,7 @@ Serial bus driver for the VectorNav VN-100, VN-200, VN-300.

 Most boards are configured to enable/start the driver on a specified UART using the SENS_VN_CFG parameter.

-Setup/usage information: https://docs.px4.io/master/en/sensor/vectornav.html
+Setup/usage information: https://docs.px4.io/main/en/sensor/vectornav.html

 ### Examples

@@ -231,6 +231,8 @@ void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param

 	AltitudePitchControl control_setpoint;

+	control_setpoint.tas_setpoint = setpoint.tas_setpoint;
+
 	control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);

 	control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param);
@@ -274,6 +276,7 @@ void TECSControl::update(const float dt, const Setpoint &setpoint, const Input &

 	AltitudePitchControl control_setpoint;

+	control_setpoint.tas_setpoint = setpoint.tas_setpoint;
 	control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);

 	if (PX4_ISFINITE(setpoint.altitude_rate_setpoint_direct)) {
@@ -320,9 +323,11 @@ float TECSControl::_calcAirspeedControlOutput(const Setpoint &setpoint, const In
 	// if airspeed measurement is not enabled then always set the rate setpoint to zero in order to avoid constant rate setpoints
 	if (flag.airspeed_enabled) {
 		// Calculate limits for the demanded rate of change of speed based on physical performance limits
-		// with a 50% margin to allow the total energy controller to correct for errors.
-		const float max_tas_rate_sp = 0.5f * limit.STE_rate_max / math::max(input.tas, FLT_EPSILON);
-		const float min_tas_rate_sp = 0.5f * limit.STE_rate_min / math::max(input.tas, FLT_EPSILON);
+		// with a 50% margin to allow the total energy controller to correct for errors. Increase it in case of fast descend
+		const float max_tas_rate_sp = (param.fast_descend * 0.5f + 0.5f) * limit.STE_rate_max / math::max(input.tas,
+					      FLT_EPSILON);
+		const float min_tas_rate_sp = (param.fast_descend * 0.5f + 0.5f) * limit.STE_rate_min / math::max(input.tas,
+					      FLT_EPSILON);
 		airspeed_rate_output = constrain((setpoint.tas_setpoint - input.tas) * param.airspeed_error_gain, min_tas_rate_sp,
 						 max_tas_rate_sp);
 	}
@@ -348,7 +353,7 @@ TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const Alt
 	// Calculate specific energy rate demands in units of (m**2/sec**3)
 	specific_energy_rates.spe_rate.setpoint = control_setpoint.altitude_rate_setpoint *
 			CONSTANTS_ONE_G; // potential energy rate of change
-	specific_energy_rates.ske_rate.setpoint = input.tas *
+	specific_energy_rates.ske_rate.setpoint = control_setpoint.tas_setpoint *
 			control_setpoint.tas_rate_setpoint; // kinetic energy rate of change

 	// Calculate specific energy rates in units of (m**2/sec**3)
@@ -394,19 +399,20 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
 	} else if (!flag.airspeed_enabled) {
 		pitch_speed_weight = 0.0f;

+	} else if (param.fast_descend > FLT_EPSILON) {
+		// pitch loop controls the airspeed to max
+		pitch_speed_weight = 1.f + param.fast_descend;
+
 	}

-	// don't allow any weight to be larger than one, as it has the same effect as reducing the control
-	// loop time constant and therefore can lead to a destabilization of that control loop
-	weight.spe_weighting = constrain(2.0f - pitch_speed_weight, 0.f, 1.f);
-	weight.ske_weighting = constrain(pitch_speed_weight, 0.f, 1.f);
+	weight.spe_weighting = constrain(2.0f - pitch_speed_weight, 0.f, 2.f);
+	weight.ske_weighting = constrain(pitch_speed_weight, 0.f, 2.f);

 	return weight;
 }

 void TECSControl::_calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rates,
-				    const Param &param,
-				    const Flag &flag)
+				    const Param &param, const Flag &flag)
 {
 	const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
 	ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)};
@@ -514,10 +520,17 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec
 	const float STE_rate_estimate_raw = specific_energy_rates.spe_rate.estimate + specific_energy_rates.ske_rate.estimate;
 	_ste_rate_estimate_filter.setParameters(dt, param.ste_rate_time_const);
 	_ste_rate_estimate_filter.update(STE_rate_estimate_raw);
-
 	ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)};
-	_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
-	float throttle_setpoint{_calcThrottleControlOutput(limit, ste_rate, param, flag)};
+	float throttle_setpoint{param.throttle_min};
+
+	if (1.f - param.fast_descend < FLT_EPSILON) {
+		// During fast descend, we control airspeed over the pitch control loop and give minimal thrust.
+		throttle_setpoint = param.throttle_min;
+
+	} else {
+		_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
+		throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag);
+	}

 	// Rate limit the throttle demand
 	if (fabsf(param.throttle_slewrate) > FLT_EPSILON) {
@@ -651,6 +664,7 @@ void TECS::initControlParams(float target_climbrate, float target_sinkrate, floa
 	_reference_param.target_sinkrate = target_sinkrate;
 	// Control
 	_control_param.tas_min = eas_to_tas * _equivalent_airspeed_min;
+	_control_param.tas_max = eas_to_tas * _equivalent_airspeed_max;
 	_control_param.pitch_max = pitch_limit_max;
 	_control_param.pitch_min = pitch_limit_min;
 	_control_param.throttle_trim = throttle_trim;
@@ -705,6 +719,11 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		initialize(altitude, hgt_rate, equivalent_airspeed, eas_to_tas);

 	} else {
+		/* Check if we want to fast descend. On fast descend, we set the throttle to min, and use the altitude control
+		loop to control the speed to the maximum airspeed. */
+		_setFastDescend(hgt_setpoint, altitude);
+		_control_param.fast_descend = _fast_descend;
+
 		// Update airspeedfilter submodule
 		const TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed = equivalent_airspeed,
 				.equivalent_airspeed_rate = speed_deriv_forward / eas_to_tas};
@@ -712,15 +731,25 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		_airspeed_filter.update(dt, airspeed_input, _airspeed_filter_param, _control_flag.airspeed_enabled);

 		// Update Reference model submodule
-		const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
-				.alt_rate = hgt_rate_sp};
+		if (1.f - _fast_descend < FLT_EPSILON) {
+			// Reset the altitude reference model, while we are in fast descend.
+			const TECSAltitudeReferenceModel::AltitudeReferenceState init_state{
+				.alt = altitude,
+				.alt_rate = hgt_rate};
+			_altitude_reference_model.initialize(init_state);

-		_altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param);
+		} else {
+			const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
+					.alt_rate = hgt_rate_sp};
+
+			_altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param);
+		}

 		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 = 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,
@@ -740,3 +769,17 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 	_update_timestamp = now;
 }

+void TECS::_setFastDescend(const float alt_setpoint, const float alt)
+{
+	if (_control_flag.airspeed_enabled && (_fast_descend_alt_err > FLT_EPSILON)
+	    && ((alt_setpoint + _fast_descend_alt_err) < alt)) {
+		_fast_descend = 1.f;
+
+	} 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);
+
+	} else {
+		_fast_descend = 0.f;
+	}
+}
@@ -202,6 +202,7 @@ public:
 		float vert_accel_limit;			///< Magnitude of the maximum vertical acceleration allowed [m/s²].
 		float equivalent_airspeed_trim;		///< Equivalent cruise airspeed for airspeed less mode [m/s].
 		float tas_min;				///< True airspeed demand lower limit [m/s].
+		float tas_max;				///< True airspeed demand upper limit [m/s].
 		float pitch_max;			///< Maximum pitch angle allowed in [rad].
 		float pitch_min;			///< Minimal pitch angle allowed in [rad].
 		float throttle_trim;		///< Normalized throttle required to fly level at calibrated airspeed setpoint [0,1]
@@ -233,6 +234,8 @@ public:

 		float load_factor_correction;				///< Gain from normal load factor increase to total energy rate demand [m²/s³].
 		float load_factor;					///< Additional normal load factor.
+
+		float fast_descend;
 	};

 	/**
@@ -363,6 +366,7 @@ private:
 	struct AltitudePitchControl {
 		float altitude_rate_setpoint;	///< Controlled altitude rate setpoint [m/s].
 		float tas_rate_setpoint;	///< Controlled true airspeed rate setpoint [m/s²].
+		float tas_setpoint; 		///< Controller true airspeed setpoint [m/s]
 	};

 	/**
@@ -393,7 +397,7 @@ private:
 	 * @brief calculate airspeed control proportional output.
 	 *
 	 * @param setpoint is the control setpoints.
-	 * @param input	is the current input measurment of the UAS.
+	 * @param input	is the current input measurement of the UAS.
 	 * @param param	is the control parameters.
 	 * @param flag	is the control flags.
 	 * @return controlled airspeed rate setpoint in [m/s²].
@@ -404,7 +408,7 @@ private:
 	 * @brief calculate altitude control proportional output.
 	 *
 	 * @param setpoint is the control setpoints.
-	 * @param input is the current input measurment of the UAS.
+	 * @param input is the current input measurement of the UAS.
 	 * @param param is the control parameters.
 	 * @return controlled altitude rate setpoint in [m/s].
 	 */
@@ -413,14 +417,14 @@ private:
 	 * @brief Calculate specific energy rates.
 	 *
 	 * @param control_setpoint is the controlles altitude and airspeed rate setpoints.
-	 * @param input is the current input measurment of the UAS.
+	 * @param input is the current input measurement of the UAS.
 	 * @return Specific energy rates in [m²/s³].
 	 */
 	SpecificEnergyRates _calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint, const Input &input) const;
 	/**
 	 * @brief Detect underspeed.
 	 *
-	 * @param input is the current input measurment of the UAS.
+	 * @param input is the current input measurement of the UAS.
 	 * @param param is the control parameters.
 	 * @param flag is the control flags.
 	 */
@@ -602,9 +606,11 @@ public:
 	void set_max_climb_rate(float climb_rate) { _control_param.max_climb_rate = climb_rate; _reference_param.max_climb_rate = climb_rate; };

 	void set_altitude_rate_ff(float altitude_rate_ff) { _control_param.altitude_setpoint_gain_ff = altitude_rate_ff; };
-	void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f);; };
+	void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f); };
+	void set_fast_descend_altitude_error(float altitude_error) { _fast_descend_alt_err = altitude_error; };

 	void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
+	void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
 	void set_equivalent_airspeed_trim(float airspeed) { _control_param.equivalent_airspeed_trim = airspeed; _airspeed_filter_param.equivalent_airspeed_trim = airspeed; }

 	void set_pitch_damping(float damping) { _control_param.pitch_damping_gain = damping; }
@@ -665,7 +671,10 @@ private:

 	hrt_abstime _update_timestamp{0};				///< last timestamp of the update function call.

-	float _equivalent_airspeed_min{3.0f};				///< equivalent airspeed demand lower limit (m/sec)
+	float _equivalent_airspeed_min{10.0f};				///< equivalent airspeed demand lower limit (m/sec)
+	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.

 	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)
@@ -697,6 +706,7 @@ private:
 		.vert_accel_limit = 0.0f,
 		.equivalent_airspeed_trim = 15.0f,
 		.tas_min = 10.0f,
+		.tas_max = 20.0f,
 		.pitch_max = 0.5f,
 		.pitch_min = -0.5f,
 		.throttle_trim = 0.0f,
@@ -716,11 +726,20 @@ private:
 		.throttle_slewrate = 0.0f,
 		.load_factor_correction = 0.0f,
 		.load_factor = 1.0f,
+		.fast_descend = 0.f
 	};

 	TECSControl::Flag _control_flag{
 		.airspeed_enabled = false,
 		.detect_underspeed_enabled = false,
 	};
+
+	/**
+	 * @brief Set fast descend value
+	 *
+	 * @param alt_setpoint is the altitude setpoint
+	 * @param alt is the current altitude
+	 */
+	void _setFastDescend(float alt_setpoint, float alt);
 };

@@ -116,111 +116,108 @@ endif()
 set(EKF_LIBS)
 set(EKF_SRCS)
 list(APPEND EKF_SRCS
-	EKF/bias_estimator.cpp
 	EKF/control.cpp
 	EKF/covariance.cpp
 	EKF/ekf.cpp
 	EKF/ekf_helper.cpp
 	EKF/estimator_interface.cpp
-	EKF/fake_height_control.cpp
-	EKF/fake_pos_control.cpp
 	EKF/height_control.cpp
-	EKF/imu_down_sampler.cpp
-	EKF/output_predictor.cpp
 	EKF/velocity_fusion.cpp
 	EKF/position_fusion.cpp
 	EKF/yaw_fusion.cpp
-	EKF/zero_innovation_heading_update.cpp

+	EKF/imu_down_sampler/imu_down_sampler.cpp
+
+	EKF/aid_sources/fake_height_control.cpp
+	EKF/aid_sources/fake_pos_control.cpp
 	EKF/aid_sources/ZeroGyroUpdate.cpp
 	EKF/aid_sources/ZeroVelocityUpdate.cpp
+	EKF/aid_sources/zero_innovation_heading_update.cpp
 )

 if(CONFIG_EKF2_AIRSPEED)
-	list(APPEND EKF_SRCS EKF/airspeed_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/aid_sources/airspeed/airspeed_fusion.cpp)
 endif()

 if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	list(APPEND EKF_SRCS EKF/aux_global_position.cpp)
+	list(APPEND EKF_SRCS EKF/aid_sources/aux_global_position/aux_global_position.cpp)
 endif()

 if(CONFIG_EKF2_AUXVEL)
-	list(APPEND EKF_SRCS EKF/auxvel_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/aid_sources/auxvel/auxvel_fusion.cpp)
 endif()

 if(CONFIG_EKF2_BAROMETER)
 	list(APPEND EKF_SRCS
-		EKF/baro_height_control.cpp
+		EKF/aid_sources/barometer/baro_height_control.cpp
 	)
 endif()

 if(CONFIG_EKF2_DRAG_FUSION)
-	list(APPEND EKF_SRCS EKF/drag_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/aid_sources/drag/drag_fusion.cpp)
 endif()

 if(CONFIG_EKF2_EXTERNAL_VISION)
 	list(APPEND EKF_SRCS
-		EKF/ev_control.cpp
-		EKF/ev_height_control.cpp
-		EKF/ev_pos_control.cpp
-		EKF/ev_vel_control.cpp
-		EKF/ev_yaw_control.cpp
+		EKF/aid_sources/external_vision/ev_control.cpp
+		EKF/aid_sources/external_vision/ev_height_control.cpp
+		EKF/aid_sources/external_vision/ev_pos_control.cpp
+		EKF/aid_sources/external_vision/ev_vel_control.cpp
+		EKF/aid_sources/external_vision/ev_yaw_control.cpp
 	)
 endif()

 if(CONFIG_EKF2_GNSS)
 	list(APPEND EKF_SRCS
-		EKF/gnss_height_control.cpp
-		EKF/gps_checks.cpp
-		EKF/gps_control.cpp
+		EKF/aid_sources/gnss/gnss_height_control.cpp
+		EKF/aid_sources/gnss/gps_checks.cpp
+		EKF/aid_sources/gnss/gps_control.cpp
 	)

+	if(CONFIG_EKF2_GNSS_YAW)
+		list(APPEND EKF_SRCS EKF/aid_sources/gnss/gps_yaw_fusion.cpp)
+	endif()
+
 	list(APPEND EKF_LIBS yaw_estimator)
 endif()

-if(CONFIG_EKF2_GNSS_YAW)
-	list(APPEND EKF_SRCS EKF/gps_yaw_fusion.cpp)
-endif()
-
 if(CONFIG_EKF2_GRAVITY_FUSION)
-	list(APPEND EKF_SRCS EKF/gravity_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/aid_sources/gravity/gravity_fusion.cpp)
 endif()

 if(CONFIG_EKF2_MAGNETOMETER)
 	list(APPEND EKF_SRCS
-		EKF/mag_3d_control.cpp
-		EKF/mag_control.cpp
-		EKF/mag_fusion.cpp
+		EKF/aid_sources/magnetometer/mag_3d_control.cpp
+		EKF/aid_sources/magnetometer/mag_control.cpp
+		EKF/aid_sources/magnetometer/mag_fusion.cpp
 	)
 endif()

 if(CONFIG_EKF2_OPTICAL_FLOW)
 	list(APPEND EKF_SRCS
-		EKF/optical_flow_control.cpp
-		EKF/optflow_fusion.cpp
+		EKF/aid_sources/optical_flow/optical_flow_control.cpp
+		EKF/aid_sources/optical_flow/optical_flow_fusion.cpp
 	)
 endif()

 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
-		EKF/range_finder_consistency_check.cpp
-		EKF/range_height_control.cpp
-		EKF/sensor_range_finder.cpp
+		EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
+		EKF/aid_sources/range_finder/range_height_control.cpp
+		EKF/aid_sources/range_finder/sensor_range_finder.cpp
 	)
 endif()

 if(CONFIG_EKF2_SIDESLIP)
-	list(APPEND EKF_SRCS EKF/sideslip_fusion.cpp)
+	list(APPEND EKF_SRCS EKF/aid_sources/sideslip/sideslip_fusion.cpp)
 endif()

 if(CONFIG_EKF2_TERRAIN)
-	list(APPEND EKF_SRCS EKF/terrain_estimator.cpp)
+	list(APPEND EKF_SRCS EKF/terrain_estimator/terrain_estimator.cpp)
 endif()

 add_subdirectory(EKF)

-
-
 px4_add_module(
 	MODULE modules__ekf2
 	MAIN ekf2
@@ -260,7 +257,10 @@ px4_add_module(
 		EKF2Utility
 		px4_work_queue
 		world_magnetic_model
+
 		${EKF_LIBS}
+		bias_estimator
+		output_predictor
 	UNITY_BUILD
 	)

@@ -31,109 +31,111 @@
 #
 ############################################################################

+add_subdirectory(bias_estimator)
+add_subdirectory(output_predictor)
+
 set(EKF_LIBS)
 set(EKF_SRCS)
 list(APPEND EKF_SRCS
-	bias_estimator.cpp
 	control.cpp
 	covariance.cpp
 	ekf.cpp
 	ekf_helper.cpp
 	estimator_interface.cpp
-	fake_height_control.cpp
-	fake_pos_control.cpp
 	height_control.cpp
-	imu_down_sampler.cpp
-	output_predictor.cpp
 	velocity_fusion.cpp
 	position_fusion.cpp
 	yaw_fusion.cpp
-	zero_innovation_heading_update.cpp

+	imu_down_sampler/imu_down_sampler.cpp
+
+	aid_sources/fake_height_control.cpp
+	aid_sources/fake_pos_control.cpp
 	aid_sources/ZeroGyroUpdate.cpp
 	aid_sources/ZeroVelocityUpdate.cpp
+	aid_sources/zero_innovation_heading_update.cpp
 )

 if(CONFIG_EKF2_AIRSPEED)
-	list(APPEND EKF_SRCS airspeed_fusion.cpp)
+	list(APPEND EKF_SRCS aid_sources/airspeed/airspeed_fusion.cpp)
 endif()

 if(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-	list(APPEND EKF_SRCS aux_global_position.cpp)
+	list(APPEND EKF_SRCS aid_sources/aux_global_position/aux_global_position.cpp)
 endif()

 if(CONFIG_EKF2_AUXVEL)
-	list(APPEND EKF_SRCS auxvel_fusion.cpp)
+	list(APPEND EKF_SRCS aid_sources/auxvel/auxvel_fusion.cpp)
 endif()

 if(CONFIG_EKF2_BAROMETER)
 	list(APPEND EKF_SRCS
-		baro_height_control.cpp
+		aid_sources/barometer/baro_height_control.cpp
 	)
 endif()

 if(CONFIG_EKF2_DRAG_FUSION)
-	list(APPEND EKF_SRCS drag_fusion.cpp)
+	list(APPEND EKF_SRCS aid_sources/drag/drag_fusion.cpp)
 endif()

 if(CONFIG_EKF2_EXTERNAL_VISION)
 	list(APPEND EKF_SRCS
-		ev_control.cpp
-		ev_height_control.cpp
-		ev_pos_control.cpp
-		ev_vel_control.cpp
-		ev_yaw_control.cpp
+		aid_sources/external_vision/ev_control.cpp
+		aid_sources/external_vision/ev_height_control.cpp
+		aid_sources/external_vision/ev_pos_control.cpp
+		aid_sources/external_vision/ev_vel_control.cpp
+		aid_sources/external_vision/ev_yaw_control.cpp
 	)
 endif()

 if(CONFIG_EKF2_GNSS)
 	list(APPEND EKF_SRCS
-		gnss_height_control.cpp
-		gps_checks.cpp
-		gps_control.cpp
+		aid_sources/gnss/gnss_height_control.cpp
+		aid_sources/gnss/gps_checks.cpp
+		aid_sources/gnss/gps_control.cpp
 	)

+	if(CONFIG_EKF2_GNSS_YAW)
+		list(APPEND EKF_SRCS aid_sources/gnss/gps_yaw_fusion.cpp)
+	endif()
+
 	add_subdirectory(yaw_estimator)
 	list(APPEND EKF_LIBS yaw_estimator)
 endif()

-if(CONFIG_EKF2_GNSS_YAW)
-	list(APPEND EKF_SRCS gps_yaw_fusion.cpp)
-endif()
-
 if(CONFIG_EKF2_GRAVITY_FUSION)
-	list(APPEND EKF_SRCS gravity_fusion.cpp)
+	list(APPEND EKF_SRCS aid_sources/gravity/gravity_fusion.cpp)
 endif()

 if(CONFIG_EKF2_MAGNETOMETER)
 	list(APPEND EKF_SRCS
-		mag_3d_control.cpp
-		mag_control.cpp
-		mag_fusion.cpp
+		aid_sources/magnetometer/mag_3d_control.cpp
+		aid_sources/magnetometer/mag_control.cpp
+		aid_sources/magnetometer/mag_fusion.cpp
 	)
 endif()

 if(CONFIG_EKF2_OPTICAL_FLOW)
 	list(APPEND EKF_SRCS
-		optical_flow_control.cpp
-		optflow_fusion.cpp
+		aid_sources/optical_flow/optical_flow_control.cpp
+		aid_sources/optical_flow/optical_flow_fusion.cpp
 	)
 endif()

 if(CONFIG_EKF2_RANGE_FINDER)
 	list(APPEND EKF_SRCS
-		range_finder_consistency_check.cpp
-		range_height_control.cpp
-		sensor_range_finder.cpp
+		aid_sources/range_finder/range_finder_consistency_check.cpp
+		aid_sources/range_finder/range_height_control.cpp
+		aid_sources/range_finder/sensor_range_finder.cpp
 	)
 endif()

 if(CONFIG_EKF2_SIDESLIP)
-	list(APPEND EKF_SRCS sideslip_fusion.cpp)
+	list(APPEND EKF_SRCS aid_sources/sideslip/sideslip_fusion.cpp)
 endif()

 if(CONFIG_EKF2_TERRAIN)
-	list(APPEND EKF_SRCS terrain_estimator.cpp)
+	list(APPEND EKF_SRCS terrain_estimator/terrain_estimator.cpp)
 endif()

 include_directories(${CMAKE_CURRENT_SOURCE_DIR})
@@ -146,7 +148,9 @@ target_include_directories(ecl_EKF PUBLIC ${EKF_GENERATED_DERIVATION_INCLUDE_PAT

 target_link_libraries(ecl_EKF
 	PRIVATE
+		bias_estimator
 		geo
+		output_predictor
 		world_magnetic_model
 		${EKF_LIBS}
 )
@@ -33,7 +33,7 @@

 #include "ekf.h"

-#include "aux_global_position.hpp"
+#include "aid_sources/aux_global_position/aux_global_position.hpp"

 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)

@@ -42,8 +42,8 @@
 //  WelfordMean for init?
 //  WelfordMean for rate

-#include "common.h"
-#include "RingBuffer.h"
+#include "../../common.h"
+#include "../../RingBuffer.h"

 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION) && defined(MODULE_NAME)

@@ -196,3 +196,39 @@ void Ekf::stopBaroHgtFusion()
 		_control_status.flags.baro_hgt = false;
 	}
 }
+
+#if defined(CONFIG_EKF2_BARO_COMPENSATION)
+float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
+{
+	if (_control_status.flags.wind && local_position_is_valid()) {
+		// calculate static pressure error = Pmeas - Ptruth
+		// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
+		// negative X and Y directions. Used to correct baro data for positional errors
+
+		// Calculate airspeed in body frame
+		const Vector3f vel_imu_rel_body_ned = _R_to_earth * (_ang_rate_delayed_raw % _params.imu_pos_body);
+		const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned;
+
+		const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f);
+
+		const Vector3f airspeed_earth = velocity_earth - wind_velocity_earth;
+
+		const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth);
+
+		const Vector3f K_pstatic_coef(
+			airspeed_body(0) >= 0.f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn,
+			airspeed_body(1) >= 0.f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn,
+			_params.static_pressure_coef_z);
+
+		const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed));
+
+		const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef));
+
+		// correct baro measurement using pressure error estimate and assuming sea level gravity
+		return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
+	}
+
+	// otherwise return the uncorrected baro measurement
+	return baro_alt_uncompensated;
+}
+#endif // CONFIG_EKF2_BARO_COMPENSATION
@@ -42,7 +42,7 @@
 #ifndef EKF_SENSOR_HPP
 #define EKF_SENSOR_HPP

-#include "common.h"
+#include <cstdint>

 namespace estimator
 {
@@ -35,9 +35,10 @@
 * @file range_finder_consistency_check.cpp
 */

-#include "range_finder_consistency_check.hpp"
+#include <aid_sources/range_finder/range_finder_consistency_check.hpp>

-void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, bool horizontal_motion, uint64_t time_us)
+void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var,
+		bool horizontal_motion, uint64_t time_us)
 {
 	if (horizontal_motion) {
 		_time_last_horizontal_motion = time_us;
@@ -55,7 +56,8 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va
 	const float vel_bottom = (dist_bottom - _dist_bottom_prev) / dt;
 	_innov = -vel_bottom - vz; // vel_bottom is +up while vz is +down

-	const float var = 2.f * dist_bottom_var / (dt * dt); // Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
+	// Variance of the time derivative of a random variable: var(dz/dt) = 2*var(z) / dt^2
+	const float var = 2.f * dist_bottom_var / (dt * dt);
 	_innov_var = var + vz_var;

 	const float normalized_innov_sq = (_innov * _innov) / _innov_var;
@@ -84,8 +86,9 @@ void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
 		}

 	} else {
-		if (_test_ratio < 1.f
-		   && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
+		if ((_test_ratio < 1.f)
+		    && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)
+		   ) {
 			_is_kinematically_consistent = true;
 		}
 	}
@@ -64,13 +64,14 @@ void Ekf::controlRangeHeightFusion()

 			if (_control_status.flags.in_air) {
 				const bool horizontal_motion = _control_status.flags.fixed_wing
-								|| (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));
+							       || (sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f));

 				const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
 				const float var = sq(_params.range_noise) + dist_dependant_var;

 				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
-				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us);
+				_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2),
+							      P(State::vel.idx + 2, State::vel.idx + 2), horizontal_motion, _time_delayed_us);
 			}

 		} else {
@@ -121,13 +122,15 @@ void Ekf::controlRangeHeightFusion()
 		}

 		// determine if we should use height aiding
-		const bool do_conditional_range_aid = (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL)) && isConditionalRangeAidSuitable();
+		const bool do_conditional_range_aid = (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
+						      && isConditionalRangeAidSuitable();
+
 		const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast<int32_t>(RngCtrl::ENABLED)) || do_conditional_range_aid)
 				&& measurement_valid
 				&& _range_sensor.isDataHealthy();

 		const bool starting_conditions_passing = continuing_conditions_passing
-				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL)
+				&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)
 				&& _range_sensor.isRegularlySendingData();

 		if (_control_status.flags.rng_hgt) {
@@ -165,13 +168,17 @@ void Ekf::controlRangeHeightFusion()

 		} else {
 			if (starting_conditions_passing) {
-				if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))) {
+				if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
+				    && (_params.rng_ctrl == static_cast<int32_t>(RngCtrl::CONDITIONAL))
+				   ) {
 					// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
 					ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
 					_height_sensor_ref = HeightSensor::RANGE;
 					bias_est.setBias(_state.pos(2) + measurement);

-				} else if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE)) && (_params.rng_ctrl != static_cast<int32_t>(RngCtrl::CONDITIONAL))) {
+				} else if ((_params.height_sensor_ref == static_cast<int32_t>(HeightSensor::RANGE))
+					   && (_params.rng_ctrl != static_cast<int32_t>(RngCtrl::CONDITIONAL))
+					  ) {
 					// Range finder is the primary height source, the ground is now the datum used
 					// to compute the local vertical position
 					ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
@@ -193,7 +200,7 @@ void Ekf::controlRangeHeightFusion()
 		}

 	} else if (_control_status.flags.rng_hgt
-		   && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL)) {
+		   && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) {
 		// No data anymore. Stop until it comes back.
 		ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
 		stopRngHgtFusion();
@@ -203,6 +210,7 @@ void Ekf::controlRangeHeightFusion()
 bool Ekf::isConditionalRangeAidSuitable()
 {
 #if defined(CONFIG_EKF2_TERRAIN)
+
 	if (_control_status.flags.in_air
 	    && _range_sensor.isHealthy()
 	    && isTerrainEstimateValid()) {
@@ -236,6 +244,7 @@ bool Ekf::isConditionalRangeAidSuitable()

 		return is_in_range && is_hagl_stable && is_below_max_speed;
 	}
+
 #endif // CONFIG_EKF2_TERRAIN

 	return false;
@@ -38,20 +38,22 @@
 *
 */

-#include "sensor_range_finder.hpp"
+#include <aid_sources/range_finder/sensor_range_finder.hpp>
+
+#include <lib/matrix/matrix/math.hpp>

 namespace estimator
 {
 namespace sensor
 {

-void SensorRangeFinder::runChecks(const uint64_t current_time_us, const Dcmf &R_to_earth)
+void SensorRangeFinder::runChecks(const uint64_t current_time_us, const matrix::Dcmf &R_to_earth)
 {
 	updateSensorToEarthRotation(R_to_earth);
 	updateValidity(current_time_us);
 }

-void SensorRangeFinder::updateSensorToEarthRotation(const Dcmf &R_to_earth)
+void SensorRangeFinder::updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth)
 {
 	// calculate 2,2 element of rotation matrix from sensor frame to earth frame
 	// this is required for use of range finder and flow data
@@ -114,7 +116,7 @@ inline bool SensorRangeFinder::isDataInRange() const

 void SensorRangeFinder::updateStuckCheck()
 {
-	if(!isStuckDetectorEnabled()){
+	if (!isStuckDetectorEnabled()) {
 		// Stuck detector disabled
 		_is_stuck = false;
 		return;
@@ -42,6 +42,7 @@
 #define EKF_SENSOR_RANGE_FINDER_HPP

 #include "Sensor.hpp"
+
 #include <matrix/math.hpp>

 namespace estimator
@@ -49,6 +50,14 @@ namespace estimator
 namespace sensor
 {

+struct rangeSample {
+	uint64_t    time_us{};  ///< timestamp of the measurement (uSec)
+	float       rng{};      ///< range (distance to ground) measurement (m)
+	int8_t      quality{};  ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
+};
+
+static constexpr uint64_t RNG_MAX_INTERVAL = 200e3;  ///< Maximum allowable time interval between range finder measurements (uSec)
+
 class SensorRangeFinder : public Sensor
 {
 public:
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_library(bias_estimator
+	bias_estimator.cpp
+	bias_estimator.hpp
+	height_bias_estimator.hpp
+	position_bias_estimator.hpp
+)
+
+add_dependencies(bias_estimator prebuild_targets)
@@ -39,7 +39,7 @@
 #define EKF_HEIGHT_BIAS_ESTIMATOR_HPP

 #include "bias_estimator.hpp"
-#include "common.h"
+#include "../common.h"

 class HeightBiasEstimator: public BiasEstimator
 {
@@ -39,7 +39,6 @@
 #define EKF_POSITION_BIAS_ESTIMATOR_HPP

 #include "bias_estimator.hpp"
-#include "common.h"

 class PositionBiasEstimator
 {
@@ -70,7 +70,6 @@ static constexpr uint64_t BARO_MAX_INTERVAL     = 200e3;  ///< Maximum allowable
 static constexpr uint64_t EV_MAX_INTERVAL       = 200e3;  ///< Maximum allowable time interval between external vision system measurements (uSec)
 static constexpr uint64_t GNSS_MAX_INTERVAL     = 500e3;  ///< Maximum allowable time interval between GNSS measurements (uSec)
 static constexpr uint64_t GNSS_YAW_MAX_INTERVAL = 1500e3; ///< Maximum allowable time interval between GNSS yaw measurements (uSec)
-static constexpr uint64_t RNG_MAX_INTERVAL      = 200e3;  ///< Maximum allowable time interval between range finder measurements (uSec)
 static constexpr uint64_t MAG_MAX_INTERVAL      = 500e3;  ///< Maximum allowable time interval between magnetic field measurements (uSec)

 // bad accelerometer detection and mitigation
@@ -197,12 +196,6 @@ struct baroSample {
 	bool        reset{false};
 };

-struct rangeSample {
-	uint64_t    time_us{};  ///< timestamp of the measurement (uSec)
-	float       rng{};      ///< range (distance to ground) measurement (m)
-	int8_t      quality{};  ///< Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
-};
-
 struct airspeedSample {
 	uint64_t    time_us{};          ///< timestamp of the measurement (uSec)
 	float       true_airspeed{};    ///< true airspeed measurement (m/sec)
@@ -282,6 +282,25 @@ void Ekf::resetQuatCov(const Vector3f &rot_var_ned)
 	P.uncorrelateCovarianceSetVariance<State::quat_nominal.dof>(State::quat_nominal.idx, rot_var_ned);
 }

+void Ekf::resetGyroBiasCov()
+{
+	// Zero the corresponding covariances and set
+	// variances to the values use for initial alignment
+	P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
+}
+
+void Ekf::resetGyroBiasZCov()
+{
+	P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias));
+}
+
+void Ekf::resetAccelBiasCov()
+{
+	// Zero the corresponding covariances and set
+	// variances to the values use for initial alignment
+	P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
+}
+
 #if defined(CONFIG_EKF2_MAGNETOMETER)
 void Ekf::resetMagCov()
 {
@@ -295,7 +314,10 @@ void Ekf::resetMagCov()
 }
 #endif // CONFIG_EKF2_MAGNETOMETER

-void Ekf::resetGyroBiasZCov()
+#if defined(CONFIG_EKF2_WIND)
+void Ekf::resetWindCov()
 {
-	P.uncorrelateCovarianceSetVariance<1>(State::gyro_bias.idx + 2, sq(_params.switch_on_gyro_bias));
+	// start with a small initial uncertainty to improve the initial estimate
+	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
 }
+#endif // CONFIG_EKF2_WIND
@@ -49,9 +49,9 @@
 # include "yaw_estimator/EKFGSF_yaw.h"
 #endif // CONFIG_EKF2_GNSS

-#include "bias_estimator.hpp"
-#include "height_bias_estimator.hpp"
-#include "position_bias_estimator.hpp"
+#include "bias_estimator/bias_estimator.hpp"
+#include "bias_estimator/height_bias_estimator.hpp"
+#include "bias_estimator/position_bias_estimator.hpp"

 #include <ekf_derivation/generated/state.h>

@@ -63,7 +63,7 @@
 #include "aid_sources/ZeroVelocityUpdate.hpp"

 #if defined(CONFIG_EKF2_AUX_GLOBAL_POSITION)
-# include "aux_global_position.hpp"
+# include "aid_sources/aux_global_position/aux_global_position.hpp"
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION

 enum class Likelihood { LOW, MEDIUM, HIGH };
@@ -265,12 +265,13 @@ public:
 	// get the 1-sigma horizontal and vertical velocity uncertainty
 	void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const;

-	// get the vehicle control limits required by the estimator to keep within sensor limitations
+	// Returns the following vehicle control limits required by the estimator to keep within sensor limitations.
+	//  vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed.
+	//  vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed.
+	//  hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
+	// hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
 	void get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const;

-	// Reset all IMU bias states and covariances to initial alignment values.
-	void resetImuBias();
-
 	void resetGyroBias();
 	void resetGyroBiasCov();

@@ -390,7 +391,7 @@ public:
 	void get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
 					float &hagl, float &beta) const;

-	// return a bitmask integer that describes which state estimates can be used for flight control
+	// return a bitmask integer that describes which state estimates are valid
 	void get_ekf_soln_status(uint16_t *status) const;

 	HeightSensor getHeightSensorRef() const { return _height_sensor_ref; }
@@ -947,10 +948,6 @@ private:
 	// and a scalar innovation value
 	void fuse(const VectorState &K, float innovation);

-#if defined(CONFIG_EKF2_BARO_COMPENSATION)
-	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
-#endif // CONFIG_EKF2_BARO_COMPENSATION
-
 	// calculate the earth rotation vector from a given latitude
 	Vector3f calcEarthRateNED(float lat_rad) const;

@@ -1079,6 +1076,11 @@ private:
 	void stopBaroHgtFusion();

 	void updateGroundEffect();
+
+# if defined(CONFIG_EKF2_BARO_COMPENSATION)
+	float compensateBaroForDynamicPressure(float baro_alt_uncompensated) const;
+# endif // CONFIG_EKF2_BARO_COMPENSATION
+
 #endif // CONFIG_EKF2_BAROMETER

 #if defined(CONFIG_EKF2_GRAVITY_FUSION)
@@ -56,43 +56,6 @@ bool Ekf::isHeightResetRequired() const
 	return (continuous_bad_accel_hgt || hgt_fusion_timeout);
 }

-#if defined(CONFIG_EKF2_BARO_COMPENSATION)
-float Ekf::compensateBaroForDynamicPressure(const float baro_alt_uncompensated) const
-{
-	if (_control_status.flags.wind && local_position_is_valid()) {
-		// calculate static pressure error = Pmeas - Ptruth
-		// model position error sensitivity as a body fixed ellipse with a different scale in the positive and
-		// negative X and Y directions. Used to correct baro data for positional errors
-
-		// Calculate airspeed in body frame
-		const Vector3f vel_imu_rel_body_ned = _R_to_earth * (_ang_rate_delayed_raw % _params.imu_pos_body);
-		const Vector3f velocity_earth = _state.vel - vel_imu_rel_body_ned;
-
-		const Vector3f wind_velocity_earth(_state.wind_vel(0), _state.wind_vel(1), 0.0f);
-
-		const Vector3f airspeed_earth = velocity_earth - wind_velocity_earth;
-
-		const Vector3f airspeed_body = _state.quat_nominal.rotateVectorInverse(airspeed_earth);
-
-		const Vector3f K_pstatic_coef(
-			airspeed_body(0) >= 0.f ? _params.static_pressure_coef_xp : _params.static_pressure_coef_xn,
-			airspeed_body(1) >= 0.f ? _params.static_pressure_coef_yp : _params.static_pressure_coef_yn,
-			_params.static_pressure_coef_z);
-
-		const Vector3f airspeed_squared = matrix::min(airspeed_body.emult(airspeed_body), sq(_params.max_correction_airspeed));
-
-		const float pstatic_err = 0.5f * _air_density * (airspeed_squared.dot(K_pstatic_coef));
-
-		// correct baro measurement using pressure error estimate and assuming sea level gravity
-		return baro_alt_uncompensated + pstatic_err / (_air_density * CONSTANTS_ONE_G);
-	}
-
-	// otherwise return the uncorrected baro measurement
-	return baro_alt_uncompensated;
-}
-#endif // CONFIG_EKF2_BARO_COMPENSATION
-
-// calculate the earth rotation vector
 Vector3f Ekf::calcEarthRateNED(float lat_rad) const
 {
 	return Vector3f(CONSTANTS_EARTH_SPIN_RATE * cosf(lat_rad),
@@ -235,7 +198,6 @@ void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const
 	*ekf_epv = sqrtf(P(State::pos.idx + 2, State::pos.idx + 2));
 }

-// get the 1-sigma horizontal and vertical velocity uncertainty
 void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 {
 	float hvel_err = sqrtf(P.trace<2>(State::vel.idx));
@@ -276,13 +238,6 @@ void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) const
 	*ekf_evv = sqrtf(P(State::vel.idx + 2, State::vel.idx + 2));
 }

-/*
-Returns the following vehicle control limits required by the estimator to keep within sensor limitations.
-vxy_max : Maximum ground relative horizontal speed (meters/sec). NaN when limiting is not needed.
-vz_max : Maximum ground relative vertical speed (meters/sec). NaN when limiting is not needed.
-hagl_min : Minimum height above ground (meters). NaN when limiting is not needed.
-hagl_max : Maximum height above ground (meters). NaN when limiting is not needed.
-*/
 void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, float *hagl_max) const
 {
 	// Do not require limiting by default
@@ -330,12 +285,6 @@ void Ekf::get_ekf_ctrl_limits(float *vxy_max, float *vz_max, float *hagl_min, fl
 #endif // CONFIG_EKF2_RANGE_FINDER
 }

-void Ekf::resetImuBias()
-{
-	resetGyroBias();
-	resetAccelBias();
-}
-
 void Ekf::resetGyroBias()
 {
 	// Zero the gyro bias states
@@ -344,13 +293,6 @@ void Ekf::resetGyroBias()
 	resetGyroBiasCov();
 }

-void Ekf::resetGyroBiasCov()
-{
-	// Zero the corresponding covariances and set
-	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<State::gyro_bias.dof>(State::gyro_bias.idx, sq(_params.switch_on_gyro_bias));
-}
-
 void Ekf::resetAccelBias()
 {
 	// Zero the accel bias states
@@ -359,18 +301,6 @@ void Ekf::resetAccelBias()
 	resetAccelBiasCov();
 }

-void Ekf::resetAccelBiasCov()
-{
-	// Zero the corresponding covariances and set
-	// variances to the values use for initial alignment
-	P.uncorrelateCovarianceSetVariance<State::accel_bias.dof>(State::accel_bias.idx, sq(_params.switch_on_accel_bias));
-}
-
-// get EKF innovation consistency check status information comprising of:
-// status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
-// Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.
-// A value > 1 indicates that the sensor measurement has exceeded the maximum acceptable level and has been rejected by the EKF
-// Where a measurement type is a vector quantity, eg magnetometer, GPS position, etc, the maximum value is returned.
 void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, float &pos, float &hgt, float &tas,
 				     float &hagl, float &beta) const
 {
@@ -498,7 +428,6 @@ void Ekf::get_innovation_test_status(uint16_t &status, float &mag, float &vel, f
 #endif // CONFIG_EKF2_SIDESLIP
 }

-// return a bitmask integer that describes which state estimates are valid
 void Ekf::get_ekf_soln_status(uint16_t *status) const
 {
 	ekf_solution_status_u soln_status{};
@@ -694,53 +623,6 @@ void Ekf::updateGroundEffect()
 }
 #endif // CONFIG_EKF2_BAROMETER

-void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
-{
-	// save a copy of the quaternion state for later use in calculating the amount of reset change
-	const Quatf quat_before_reset = _state.quat_nominal;
-
-	// update the yaw angle variance
-	if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
-		P.uncorrelateCovarianceSetVariance<1>(2, yaw_variance);
-	}
-
-	// update transformation matrix from body to world frame using the current estimate
-	// update the rotation matrix using the new yaw value
-	_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
-
-	// calculate the amount that the quaternion has changed by
-	const Quatf quat_after_reset(_R_to_earth);
-	const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized());
-
-	// update quaternion states
-	_state.quat_nominal = quat_after_reset;
-
-	// add the reset amount to the output observer buffered data
-	_output_predictor.resetQuaternion(q_error);
-
-#if defined(CONFIG_EKF2_EXTERNAL_VISION)
-	// update EV attitude error filter
-	if (_ev_q_error_initialized) {
-		const Quatf ev_q_error_updated = (q_error * _ev_q_error_filt.getState()).normalized();
-		_ev_q_error_filt.reset(ev_q_error_updated);
-	}
-#endif // CONFIG_EKF2_EXTERNAL_VISION
-
-	// record the state change
-	if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) {
-		_state_reset_status.quat_change = q_error;
-
-	} else {
-		// there's already a reset this update, accumulate total delta
-		_state_reset_status.quat_change = q_error * _state_reset_status.quat_change;
-		_state_reset_status.quat_change.normalize();
-	}
-
-	_state_reset_status.reset_count.quat++;
-
-	_time_last_heading_fuse = _time_delayed_us;
-}
-
 #if defined(CONFIG_EKF2_WIND)
 void Ekf::resetWind()
 {
@@ -764,11 +646,6 @@ void Ekf::resetWindToZero()
 	resetWindCov();
 }

-void Ekf::resetWindCov()
-{
-	// start with a small initial uncertainty to improve the initial estimate
-	P.uncorrelateCovarianceSetVariance<State::wind_vel.dof>(State::wind_vel.idx, sq(_params.initial_wind_uncertainty));
-}
 #endif // CONFIG_EKF2_WIND

 void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed)
@@ -266,7 +266,7 @@ void EstimatorInterface::setAirspeedData(const airspeedSample &airspeed_sample)
 #endif // CONFIG_EKF2_AIRSPEED

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-void EstimatorInterface::setRangeData(const rangeSample &range_sample)
+void EstimatorInterface::setRangeData(const sensor::rangeSample &range_sample)
 {
 	if (!_initialised) {
 		return;
@@ -274,7 +274,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)

 	// Allocate the required buffer size if not previously done
 	if (_range_buffer == nullptr) {
-		_range_buffer = new RingBuffer<rangeSample>(_obs_buffer_length);
+		_range_buffer = new RingBuffer<sensor::rangeSample>(_obs_buffer_length);

 		if (_range_buffer == nullptr || !_range_buffer->valid()) {
 			delete _range_buffer;
@@ -291,7 +291,7 @@ void EstimatorInterface::setRangeData(const rangeSample &range_sample)
 	// limit data rate to prevent data being lost
 	if (time_us >= static_cast<int64_t>(_range_buffer->get_newest().time_us + _min_obs_interval_us)) {

-		rangeSample range_sample_new{range_sample};
+		sensor::rangeSample range_sample_new{range_sample};
 		range_sample_new.time_us = time_us;

 		_range_buffer->push(range_sample_new);
@@ -63,12 +63,12 @@

 #include "common.h"
 #include "RingBuffer.h"
-#include "imu_down_sampler.hpp"
-#include "output_predictor.h"
+#include "imu_down_sampler/imu_down_sampler.hpp"
+#include "output_predictor/output_predictor.h"

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-# include "range_finder_consistency_check.hpp"
-# include "sensor_range_finder.hpp"
+# include "aid_sources/range_finder/range_finder_consistency_check.hpp"
+# include "aid_sources/range_finder/sensor_range_finder.hpp"
 #endif // CONFIG_EKF2_RANGE_FINDER

 #include <lib/atmosphere/atmosphere.h>
@@ -107,7 +107,7 @@ public:
 #endif // CONFIG_EKF2_AIRSPEED

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	void setRangeData(const rangeSample &range_sample);
+	void setRangeData(const estimator::sensor::rangeSample &range_sample);

 	// set sensor limitations reported by the rangefinder
 	void set_rangefinder_limits(float min_distance, float max_distance)
@@ -115,7 +115,7 @@ public:
 		_range_sensor.setLimits(min_distance, max_distance);
 	}

-	const rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
+	const estimator::sensor::rangeSample &get_rng_sample_delayed() { return *(_range_sensor.getSampleAddress()); }
 #endif // CONFIG_EKF2_RANGE_FINDER

 #if defined(CONFIG_EKF2_OPTICAL_FLOW)
@@ -356,7 +356,7 @@ protected:
 #endif // CONFIG_EKF2_EXTERNAL_VISION

 #if defined(CONFIG_EKF2_RANGE_FINDER)
-	RingBuffer<rangeSample> *_range_buffer{nullptr};
+	RingBuffer<sensor::rangeSample> *_range_buffer{nullptr};
 	uint64_t _time_last_range_buffer_push{0};

 	sensor::SensorRangeFinder _range_sensor{};
@@ -1,4 +1,4 @@
-#include "imu_down_sampler.hpp"
+#include "imu_down_sampler/imu_down_sampler.hpp"

 #include <lib/mathlib/mathlib.h>

@@ -41,7 +41,7 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>

-#include "common.h"
+#include "../common.h"

 using namespace estimator;

@@ -0,0 +1,39 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_library(output_predictor
+	output_predictor.cpp
+	output_predictor.h
+)
+
+add_dependencies(output_predictor prebuild_targets)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4. All rights reserved.
+ *   Copyright (c) 2022-2024 PX4. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4. All rights reserved.
+ *   Copyright (c) 2022-2024 PX4. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -36,8 +36,7 @@

 #include <matrix/math.hpp>

-#include "common.h"
-#include "RingBuffer.h"
+#include "../RingBuffer.h"

 #include <lib/geo/geo.h>

@@ -37,7 +37,12 @@ import symforce
 symforce.set_epsilon_to_symbol()

 import symforce.symbolic as sf
-from utils.derivation_utils import *
+
+# generate_px4_function from derivation_utils in EKF/ekf_derivation/utils
+import os, sys
+derivation_utils_dir = os.path.dirname(os.path.abspath(__file__)) + "/../../python/ekf_derivation/utils"
+sys.path.append(derivation_utils_dir)
+import derivation_utils

 def predict_opt_flow(
        terrain_vpos: sf.Scalar,
@@ -49,7 +54,7 @@ def predict_opt_flow(
    R_to_earth = sf.Rot3(sf.Quaternion(xyz=q_att[1:4], w=q_att[0])).to_rotation_matrix()
    flow_pred = sf.V2()
    dist = - (pos_z - terrain_vpos)
-    dist = add_epsilon_sign(dist, dist, epsilon)
+    dist = derivation_utils.add_epsilon_sign(dist, dist, epsilon)
    flow_pred[0] = -v[1] / dist * R_to_earth[2, 2]
    flow_pred[1] = v[0] / dist * R_to_earth[2, 2]
    return flow_pred
@@ -90,5 +95,5 @@ def terr_est_compute_flow_y_innov_var_and_h(
    return (innov_var, Hy[0, 0])

 print("Derive terrain estimator equations...")
-generate_px4_function(terr_est_compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
-generate_px4_function(terr_est_compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
+derivation_utils.generate_px4_function(terr_est_compute_flow_xy_innov_var_and_hx, output_names=["innov_var", "H"])
+derivation_utils.generate_px4_function(terr_est_compute_flow_y_innov_var_and_h, output_names=["innov_var", "H"])
@@ -38,8 +38,8 @@
 */

 #include "ekf.h"
-#include "python/ekf_derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h"
-#include "python/ekf_derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h"
+#include "terrain_estimator/derivation/generated/terr_est_compute_flow_xy_innov_var_and_hx.h"
+#include "terrain_estimator/derivation/generated/terr_est_compute_flow_y_innov_var_and_h.h"

 #include <mathlib/mathlib.h>

@@ -136,3 +136,50 @@ void Ekf::computeYawInnovVarAndH(float variance, float &innovation_variance, Vec
 {
 	sym::ComputeYawInnovVarAndH(_state.vector(), P, variance, &innovation_variance, &H_YAW);
 }
+
+void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
+{
+	// save a copy of the quaternion state for later use in calculating the amount of reset change
+	const Quatf quat_before_reset = _state.quat_nominal;
+
+	// update the yaw angle variance
+	if (PX4_ISFINITE(yaw_variance) && (yaw_variance > FLT_EPSILON)) {
+		P.uncorrelateCovarianceSetVariance<1>(2, yaw_variance);
+	}
+
+	// update transformation matrix from body to world frame using the current estimate
+	// update the rotation matrix using the new yaw value
+	_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
+
+	// calculate the amount that the quaternion has changed by
+	const Quatf quat_after_reset(_R_to_earth);
+	const Quatf q_error((quat_after_reset * quat_before_reset.inversed()).normalized());
+
+	// update quaternion states
+	_state.quat_nominal = quat_after_reset;
+
+	// add the reset amount to the output observer buffered data
+	_output_predictor.resetQuaternion(q_error);
+
+#if defined(CONFIG_EKF2_EXTERNAL_VISION)
+	// update EV attitude error filter
+	if (_ev_q_error_initialized) {
+		const Quatf ev_q_error_updated = (q_error * _ev_q_error_filt.getState()).normalized();
+		_ev_q_error_filt.reset(ev_q_error_updated);
+	}
+#endif // CONFIG_EKF2_EXTERNAL_VISION
+
+	// record the state change
+	if (_state_reset_status.reset_count.quat == _state_reset_count_prev.quat) {
+		_state_reset_status.quat_change = q_error;
+
+	} else {
+		// there's already a reset this update, accumulate total delta
+		_state_reset_status.quat_change = q_error * _state_reset_status.quat_change;
+		_state_reset_status.quat_change.normalize();
+	}
+
+	_state_reset_status.reset_count.quat++;
+
+	_time_last_heading_fuse = _time_delayed_us;
+}
@@ -2368,7 +2368,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)

 			int8_t quality = static_cast<float>(optical_flow.quality) / static_cast<float>(UINT8_MAX) * 100.f;

-			rangeSample range_sample {
+			estimator::sensor::rangeSample range_sample {
 				.time_us = optical_flow.timestamp_sample,
 				.rng = optical_flow.distance_m,
 				.quality = quality,
@@ -2507,7 +2507,7 @@ void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 	if (_distance_sensor_selected >= 0 && _distance_sensor_subs[_distance_sensor_selected].update(&distance_sensor)) {
 		// EKF range sample
 		if (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING) {
-			rangeSample range_sample {
+			estimator::sensor::rangeSample range_sample {
 				.time_us = distance_sensor.timestamp,
 				.rng = distance_sensor.current_distance,
 				.quality = distance_sensor.signal_quality,
@@ -55,7 +55,7 @@ public:
 	void setLimits(float min_distance_m, float max_distance_m);

 private:
-	rangeSample _range_sample{};
+	estimator::sensor::rangeSample _range_sample{};
 	float _min_distance{0.2f};
 	float _max_distance{20.0f};

@@ -34,7 +34,7 @@
 #include <gtest/gtest.h>
 #include <math.h>
 #include "EKF/ekf.h"
-#include "EKF/imu_down_sampler.hpp"
+#include "EKF/imu_down_sampler/imu_down_sampler.hpp"

 class EkfImuSamplingTest : public ::testing::TestWithParam<std::tuple<float, float, Vector3f, Vector3f>>
 {
@@ -34,10 +34,10 @@
 #include <gtest/gtest.h>
 #include <math.h>
 #include "EKF/common.h"
-#include "EKF/sensor_range_finder.hpp"
+#include "EKF/aid_sources/range_finder/sensor_range_finder.hpp"
 #include <matrix/math.hpp>

-using estimator::rangeSample;
+using estimator::sensor::rangeSample;
 using matrix::Dcmf;
 using matrix::Eulerf;
 using namespace estimator::sensor;
@@ -129,6 +129,7 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
 	_tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed());
 	_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed());
+	_tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed());
 	_tecs.set_throttle_damp(_param_fw_t_thr_damping.get());
 	_tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get());
 	_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
@@ -137,6 +138,7 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
 	_tecs.set_pitch_damping(_param_fw_t_ptch_damp.get());
 	_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
+	_tecs.set_fast_descend_altitude_error(_param_fw_t_fast_alt_err.get());
 	_tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get());
 	_tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get());
 	_tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get());
@@ -964,6 +964,7 @@ private:

 		(ParamFloat<px4::params::FW_T_HRATE_FF>) _param_fw_t_hrate_ff,
 		(ParamFloat<px4::params::FW_T_ALT_TC>) _param_fw_t_h_error_tc,
+		(ParamFloat<px4::params::FW_T_F_ALT_ERR>) _param_fw_t_fast_alt_err,
 		(ParamFloat<px4::params::FW_T_THR_INTEG>) _param_fw_t_thr_integ,
 		(ParamFloat<px4::params::FW_T_I_GAIN_PIT>) _param_fw_t_I_gain_pit,
 		(ParamFloat<px4::params::FW_T_PTCH_DAMP>) _param_fw_t_ptch_damp,
@@ -633,6 +633,15 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.1f);
 */
 PARAM_DEFINE_FLOAT(FW_T_ALT_TC, 5.0f);

+/**
+ * Minimum altitude error needed to descend with max airspeed. A negative value disables fast descend.
+ *
+ * @min -1.0
+ * @decimal 0
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_F_ALT_ERR, -1.0f);
+
 /**
 * Height rate feed forward
 *
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Nuno Marques	ba71d87319	ci: github actions: reenable some tests to increase coverage	2024-05-18 20:20:38 +01:00
Nuno Marques	03050f23a8	rc.serial_port.jinja: Remove 'x' prefix in variable comparisons to resolve ShellCheck warnings	2024-05-18 20:15:27 +01:00
Nuno Marques	e2aa9605d2	ci: github actions: match checkout actions to fix ownership problem	2024-05-18 20:15:27 +01:00
Nuno Marques	f8205995be	integration tests: mavros: remove python2 shebang	2024-05-18 20:15:26 +01:00
Nuno Marques	0f4154005a	ci: github: add ownership workaround	2024-05-18 20:15:26 +01:00
Nuno Marques	2eeb358d3c	ci: github actions: update container images - Updated container images to newer versions with Ubuntu Jammy as per 2024-05-18 - Migrated ROS container from Melodic to Noetic, since Melodic is EOL	2024-05-18 20:15:26 +01:00
Thomas Frans	9fd1c54570	style(editorconfig): update newline setting The setting wasn't consistent with the one used in the Visual Studio Code settings, which caused different newline formatting depending on whether the user uses Visual Studio Code or another editor that uses EditorConfig.	2024-05-15 11:40:11 -04:00
dirksavage88	ee2a8c9bda	increase lp default stack to 2000 Signed-off-by: dirksavage88 <dirksavage88@gmail.com>	2024-05-15 11:08:48 -04:00
Hamish Willee	2e7a99ac41	VectorNav.cpp - fix docs link to usage guide	2024-05-15 11:07:50 -04:00
Konrad	17916b7fdc	uxcre_dds_client: use topic name as defined in the dds_topics.yaml to register stream	2024-05-15 11:07:01 -04:00
Eric Katzfey	293389abf3	Minor updates to the VOXL 2 board README file	2024-05-14 12:25:22 -04:00
Eric Katzfey	839f5bbb12	Removed obsolete voxl 2 board default parameter setting	2024-05-14 11:08:43 -04:00
Peter van der Perk	253208fdd4	fmu-v6xrt: Add I2C driver launcher	2024-05-08 11:34:13 -04:00
Peter van der Perk	5789803665	fmu-v6xrt: Enable debug features for more verbose hardfault output	2024-05-08 06:14:24 -04:00
Julian Oes	b1b9c8fd99	gps: add note to param This notes the reference yaw angle for the Septentrio Mosaic-H. It's unfortunately a bit tricky in that Unicore has the main antenna in front by default while Septentrio decided to put the aux antenna in front. Signed-off-by: Julian Oes <julian@oes.ch>	2024-05-07 21:15:44 -04:00
Benjamin Philipp Ketterer	5d025e6d3d	increased uxrce-dds stack size to prevent overflow	2024-05-07 21:13:58 -04:00
alexklimaj	b9a696d025	boards: ark septentrio gps add iis2mdc	2024-05-07 21:12:15 -04:00
RomanBapst	ca9cb2214f	quadchute: fixed sign for handling altitude resets Signed-off-by: RomanBapst <bapstroman@gmail.com>	2024-05-07 16:01:09 +02:00
muramura	b5467d88f7	gps: Change the IF statement to a SWITCH statement	2024-05-07 15:58:15 +02:00
Konrad	6984e6da7f	TECS:use tas_setpoint instead of measured tas for specific kinetic energy calculation	2024-05-07 14:20:07 +02:00
Konrad	f56f4c7033	TECS: enable specific energy weights to have a value up to 2	2024-05-07 14:20:07 +02:00
Konrad	f8a20e1964	TECS: increase airspeed control limit for fast descend	2024-05-07 14:20:07 +02:00
Konrad	6a789b54c6	TECS: allow for fast descend up to maximum airspeed. Use pitch control loop to control max airspeed while giving minimal throttle.	2024-05-07 14:20:07 +02:00
Beat Küng	e17faece3d	mavlink_ftp: do not store reply on kErrNoSessionsAvailable This would interfere with an existing ongoing session	2024-05-07 07:26:12 +02:00
Beat Küng	f002b08e6a	mavlink_ftp: ensure there's enough space for the 2. path in _workRename Prevents accessing invalid memory when reading ptr + oldpath_sz + 1 and oldpath_sz fills out the whole or N-1 bytes of the payload.	2024-05-07 07:26:12 +02:00
Beat Küng	f16115d8be	mavlink_ftp: handle relative paths correctly by ensuring there's a '/' in between when concatenating the path with _root_dir.	2024-05-07 07:26:12 +02:00
Julian Oes	f04d17d160	Tools: skip submodule check in CLion Same as what's required for VSCode. Signed-off-by: Julian Oes <julian@oes.ch>	2024-05-05 11:33:24 +12:00
Beat Küng	9e6dcb1f60	fix mavlink: cmd_logging_{start,stop}_acknowledgement flags were not reset Regression from https://github.com/PX4/PX4-Autopilot/pull/23043 Also avoids a race condition by making sure the command ack is handled before sending out the mavlink message (in case an external component reacts immediately to the mavlink message).	2024-05-03 06:59:55 +02:00
Alex Klimaj	36ea872e72	drivers: adis16507 reschedule reset after failed self test	2024-05-02 17:52:26 -04:00
Daniel Agar	224d6f2fa7	ekf2: ekf_helper.cpp remove duplicate method comments (comment on declaration only, not definition)	2024-05-02 17:40:58 -04:00
Daniel Agar	c1fc893cca	ekf2: move gyro/accel/wind covariance reset helpers to covariance.cpp	2024-05-02 17:40:58 -04:00
Daniel Agar	63c2ea33c1	ekf2: move Ekf::resetQuatStateYaw() to yaw_fusion.cpp	2024-05-02 17:40:58 -04:00
Daniel Agar	1ca4056b6a	ekf2: delete unused Ekf::resetImuBias()	2024-05-02 17:40:58 -04:00
Daniel Agar	6b3b66619b	ekf2: move baro dynamic pressure compensation to aid_sources/barometer	2024-05-02 17:40:58 -04:00
Daniel Agar	4f0eb72fc9	ekf2: move IMU down sampler to imu_down_sampler/	2024-05-02 17:40:58 -04:00
Daniel Agar	58637d3825	ekf2: move terrain estimator and derivation to terrain_estimator/	2024-05-02 17:40:58 -04:00
Daniel Agar	58de8cbb77	ekf2: move fake_height, fake_pos, zero_innovation_heading to aid_sources/	2024-05-02 17:40:58 -04:00
Daniel Agar	49c782bad9	ekf2: move bias estimators to bias_estimtor/	2024-05-02 17:40:58 -04:00
Daniel Agar	e262fde4dc	ekf2: move aux global position fusion to aid_sources/aux_global_position	2024-05-02 17:40:58 -04:00
Daniel Agar	b8d46e60a5	ekf2: move mag fusion to aid_sources/magnetometer	2024-05-02 17:40:58 -04:00
Daniel Agar	3f6c3e0649	ekf2: move output predictor to output_predictor/	2024-05-02 17:40:58 -04:00
Daniel Agar	24fdd696cb	ekf2: move range finder files to aid_sources/range_finder	2024-05-02 17:40:58 -04:00
Daniel Agar	3dbd3f8a1a	ekf2: move airspeed fusion file to aid_sources/airspeed	2024-05-02 17:40:58 -04:00
Daniel Agar	789b2b3d8a	ekf2: move sideslip fusion file to aid_sources/sideslip	2024-05-02 17:40:58 -04:00
Daniel Agar	eb8ee74066	ekf2: move baro height file to aid_sources/barometer	2024-05-02 17:40:58 -04:00
Daniel Agar	de178b1435	ekf2: move gravity fusion file to aid_sources/gravity	2024-05-02 17:40:58 -04:00
Daniel Agar	78f2ccbb60	ekf2: move optical flow files to aid_sources/optical_flow	2024-05-02 17:40:58 -04:00
Daniel Agar	fcf94e7670	ekf2: move GNSS files to aid_sources/gnss	2024-05-02 17:40:58 -04:00
Daniel Agar	31ae5b77fe	ekf2: move drag_fusion file to aid_sources/drag	2024-05-02 17:40:58 -04:00
Daniel Agar	c3fb0b1090	ekf2: move auxvel file to aid_sources/auxvel	2024-05-02 17:40:58 -04:00
Daniel Agar	b5d1e87368	ekf2: move EV files to aid_sources/external_vision	2024-05-02 17:40:58 -04:00
Peter van der Perk	f382e585e8	sd_bench: Add U option for forcing byte aligned Co-authored-by: David Sidrane <david.sidrane@nscdg.com>	2024-05-02 12:33:25 -04:00
Daniel Agar	c64104e9f1	sensors/vehicle_angular_velocity: silence gyro selection fallback warning (PX4_WARN -> PX4_DEBUG) - this warning was to catch any potential errors in sensor selection relative to what's actually available, we don't need to complain about initial selection before the EKF selector is available	2024-05-02 11:53:31 -04:00