RpmControl: class clean up

ControlAllocator: introduce helicopter rotor rpm controller
RPMCapture: fix dt should not have a minimum which would limit to 6000rpm
2026-06-06 06:40:04 +08:00 · 2024-12-11 18:55:57 +01:00 · 2024-12-11 16:57:43 +01:00 · 2024-12-10 19:53:57 +01:00 · 2024-12-10 19:52:04 +01:00 · 2024-12-10 19:50:35 +01:00
147 changed files with 3306 additions and 1502 deletions
@@ -25,28 +25,27 @@ jobs:
          "NO_NINJA_BUILD=1 px4_fmu-v5_default",
          "NO_NINJA_BUILD=1 px4_sitl_default",
          "px4_sitl_allyes",
-          "airframe_metadata",
          "module_documentation",
-          "parameters_metadata",
        ]
-    container:
-      image: px4io/px4-dev-nuttx-focal:2022-08-12
-      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0

-    - name: check environment
-      run: |
-          export
-          ulimit -a
-    - name: ${{matrix.check}}
-      run: make ${{matrix.check}}
-    - name: upload coverage
-      if: contains(matrix.check, 'coverage')
-      uses: codecov/codecov-action@v1
-      with:
-        token: ${{ secrets.CODECOV_TOKEN }}
-        flags: unittests
-        file: coverage/lcov.info
+      - name: Building [${{ matrix.check }}]
+        uses: addnab/docker-run-action@v3
+        with:
+          image: px4io/px4-dev-nuttx-focal:2022-08-12
+          options: -v ${{ github.workspace }}:/workspace
+          run: |
+            cd /workspace
+            git config --global --add safe.directory /workspace
+            make ${{ matrix.check }}
+
+      - name: Uploading Coverage to Codecov.io
+        if: contains(matrix.check, 'coverage')
+        uses: codecov/codecov-action@v1
+        with:
+          token: ${{ secrets.CODECOV_TOKEN }}
+          flags: unittests
+          file: coverage/lcov.info
@@ -11,13 +11,17 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-clang:2021-09-08
    steps:
-    - uses: actions/checkout@v1
+    - uses: actions/checkout@v4
      with:
-        token: ${{secrets.ACCESS_TOKEN}}
+        fetch-depth: 0

-    - uses: corrupt952/actions-retry-command@v1.0.7
+    - name: Testing (clang-tidy-quiet)
+      uses: addnab/docker-run-action@v3
      with:
-        command: make clang-tidy-quiet
-        max_attempts: 3
+        image: px4io/px4-dev-clang:2021-09-08
+        options: -v ${{ github.workspace }}:/workspace
+        run: |
+          cd /workspace
+          git config --global --add safe.directory /workspace
+          make clang-tidy-quiet
@@ -19,13 +19,11 @@ jobs:
          ]
    steps:
    - name: install Python 3.10
-      uses: actions/setup-python@v4
+      uses: actions/setup-python@v5
      with:
        python-version: "3.10"

-    - uses: actions/checkout@v1
-      with:
-        token: ${{secrets.ACCESS_TOKEN}}
+    - uses: actions/checkout@v4

    - name: setup
      run: ./Tools/setup/macos.sh; ./Tools/setup/macos.sh
@@ -37,7 +35,7 @@ jobs:
        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
        message("::set-output name=timestamp::${current_date}")
    - name: ccache cache files
-      uses: actions/cache@v2
+      uses: actions/cache@v4
      with:
        path: ~/.ccache
        key: macos_${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
@@ -25,8 +25,14 @@ jobs:
          submodules: false
          fetch-depth: 0

+      - name: Set PX4 Tag
+        id: px4-tag
+        run: |
+          echo "tag=$(git describe --tags --match 'v[0-9]*')" >> $GITHUB_OUTPUT
+
      - name: Login to Docker Hub
        uses: docker/login-action@v3
+        if: github.event_name != 'pull_request'
        with:
          username: ${{ secrets.DOCKERHUB_USERNAME }}
          password: ${{ secrets.DOCKERHUB_TOKEN }}
@@ -44,16 +50,15 @@ jobs:
        with:
          images: |
            ghcr.io/PX4/px4-dev
-            px4io/px4-dev
+            ${{ (github.event_name != 'pull_request') && 'px4io/px4-dev' || '' }}
          tags: |
-            type=schedule
            type=semver,pattern={{version}}
            type=semver,pattern={{major}}.{{minor}}
            type=semver,pattern={{major}}
-            type=ref,event=branch,suffix=-{{date 'YYYYMMDD'}},priority=600
+            type=ref,event=branch,value=${{ steps.px4-tag.outputs.tag }},priority=700
+            type=ref,event=branch,suffix=-{{date 'YYYY-MM-DD'}},priority=600
            type=ref,event=branch,suffix=,priority=500
            type=ref,event=pr
-            type=sha

      - name: Set up Docker Buildx
        uses: docker/setup-buildx-action@v3
@@ -1,21 +1,28 @@
 name: EKF Change Indicator

-on: pull_request
+on:
+  pull_request:
+    branches:
+    - '*'

 jobs:
  unit_tests:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
    steps:
-    - uses: actions/checkout@v2.3.1
+    - uses: actions/checkout@v4
      with:
        fetch-depth: 0
-    - name: checkout newest version of branch
-      run: |
-        git fetch origin pull/${{github.event.pull_request.number}}/head:${{github.head_ref}}
-        git checkout ${GITHUB_HEAD_REF}
+
    - name: main test
-      run: make tests TESTFILTER=EKF
+      uses: addnab/docker-run-action@v3
+      with:
+        image: px4io/px4-dev-base-focal:2021-09-08
+        options: -v ${{ github.workspace }}:/workspace
+        run: |
+          cd /workspace
+          git config --global --add safe.directory /workspace
+          make tests TESTFILTER=EKF
+
    - name: Check if there is a functional change
      run: git diff --exit-code
      working-directory: src/modules/ekf2/test/change_indication
@@ -5,25 +5,40 @@ on: push
 jobs:
  unit_tests:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
    env:
      GIT_COMMITTER_EMAIL: bot@px4.io
      GIT_COMMITTER_NAME: PX4BuildBot
    steps:
-    - uses: actions/checkout@v2.3.1
+    - uses: actions/checkout@v4
      with:
        fetch-depth: 0
-    - name: main test updates change indication files
-      run: make tests TESTFILTER=EKF
+
+    - name: main test
+      uses: addnab/docker-run-action@v3
+      with:
+        image: px4io/px4-dev-base-focal:2021-09-08
+        options: -v ${{ github.workspace }}:/workspace
+        run: |
+          cd /workspace
+          git config --global --add safe.directory /workspace
+          make tests TESTFILTER=EKF
+
    - name: Check if there exists diff and save result in variable
-      run: echo "CHANGE_INDICATED=$(git diff --exit-code --output=/dev/null || echo $?)" >> $GITHUB_ENV
+      id: diff-check
+      run: echo "CHANGE_INDICATED=$(git diff --exit-code --output=/dev/null || echo $?)" >> $GITHUB_OUTPUT
      working-directory: src/modules/ekf2/test/change_indication
+
    - name: auto-commit any changes to change indication
      uses: stefanzweifel/git-auto-commit-action@v4
      with:
-        commit_message: '[AUTO COMMIT] update change indication'
+        file_pattern: 'src/modules/ekf2/test/change_indication/*.csv'
        commit_user_name: ${GIT_COMMITTER_NAME}
        commit_user_email: ${GIT_COMMITTER_EMAIL}
-    - if: ${{env.CHANGE_INDICATED}}
-      name: if there is a functional change, fail check
+        commit_message: |
+          '[AUTO COMMIT] update change indication'
+
+          See .github/workflopws/ekf_update_change_indicator.yml for more details
+
+    - name: if there is a functional change, fail check
+      if: ${{ steps.diff-check.outputs.CHANGE_INDICATED }}
      run: exit 1
@@ -24,21 +24,23 @@ jobs:
      image: px4io/px4-dev-nuttx-focal:2022-08-12
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
+      - name: Install Node v20.18.0
+        uses: actions/setup-node@v4
+        with:
+          node-version: 20.18.0

-    - name: check environment
-      run: |
-          export
-          ulimit -a
-    - name: install emscripten
-      run: |
-        git clone https://github.com/emscripten-core/emsdk.git _emscripten_sdk
-        cd _emscripten_sdk
-        ./emsdk install latest
-        ./emsdk activate latest
-    - name: ${{matrix.check}}
-      run: |
-        . ./_emscripten_sdk/emsdk_env.sh
-        make ${{matrix.check}}
+      - uses: actions/checkout@v4
+        with:
+          fetch-depth: 0
+
+      - name: Install empscripten
+        run: |
+          git clone https://github.com/emscripten-core/emsdk.git _emscripten_sdk
+          cd _emscripten_sdk
+          ./emsdk install latest
+          ./emsdk activate latest
+
+      - name: Testing [${{ matrix.check }}]
+        run: |
+          . ./_emscripten_sdk/emsdk_env.sh
+          make ${{ matrix.check }}
@@ -8,6 +8,10 @@ on:
    branches:
      - '*'

+env:
+  MIN_FLASH_POS_DIFF_FOR_COMMENT: 50
+  MIN_FLASH_NEG_DIFF_FOR_COMMENT: -50
+
 jobs:
  analyze_flash:
    name: Analyzing ${{ matrix.target }}
@@ -18,8 +22,10 @@ jobs:
      matrix:
        target: [px4_fmu-v5x, px4_fmu-v6x]
    outputs:
-      px4_fmu-v5x: ${{ steps.gen-output.outputs.output_px4_fmu-v5x }}
-      px4_fmu-v6x: ${{ steps.gen-output.outputs.output_px4_fmu-v6x }}
+      px4_fmu-v5x-bloaty-output: ${{ steps.gen-output.outputs.px4_fmu-v5x-bloaty-output }}
+      px4_fmu-v5x-bloaty-summary-map: ${{ steps.gen-output.outputs.px4_fmu-v5x-bloaty-summary-map }}
+      px4_fmu-v6x-bloaty-output: ${{ steps.gen-output.outputs.px4_fmu-v6x-bloaty-output }}
+      px4_fmu-v6x-bloaty-summary-map: ${{ steps.gen-output.outputs.px4_fmu-v6x-bloaty-summary-map }}
    steps:
      - uses: actions/checkout@v4
        with:
@@ -30,19 +36,20 @@ jobs:
        run: git config --system --add safe.directory '*'

      - name: Build Target
-        run: make ${{ matrix.target }}
+        run: make ${{ matrix.target }}_flash-analysis

      - name: Store the ELF with the change
-        run: cp ./build/${{ matrix.target }}_default/${{ matrix.target }}_default.elf ./with-change.elf
+        run: cp ./build/**/*.elf ./with-change.elf

      - name: Clean previous build
        run: |
          make clean
          make distclean

-      - name: If it's a PR checkout the base commit
+      - name: If it's a PR checkout the base branch
        if: ${{ github.event.pull_request }}
-        run: git checkout ${{ github.event.pull_request.base.sha }}
+        # As checkout creates a merge commit (merging the base branch into the PR branch), the base branch is the base for a diff of the PR changes.
+        run: git checkout ${{ github.event.pull_request.base.ref }}

      - name: If it's a push checkout the previous commit
        if: github.event_name == 'push'
@@ -52,30 +59,39 @@ jobs:
        run: make submodulesupdate

      - name: Build
-        run: make ${{ matrix.target }}
+        run: make ${{ matrix.target }}_flash-analysis

      - name: Store the ELF before the change
-        run: cp ./build/${{ matrix.target }}_default/${{ matrix.target }}_default.elf ./before-change.elf
+        run: cp ./build/**/*.elf ./before-change.elf

      - name: bloaty-action
-        uses: carlosperate/bloaty-action@v1.1.0
+        uses: PX4/bloaty-action@v1.0.0
        id: bloaty-step
        with:
-          bloaty-args: -d sections,compileunits -n 0 ./with-change.elf -- ./before-change.elf
+          bloaty-file-args: ./with-change.elf -- ./before-change.elf
+          bloaty-additional-args: -d sections,compileunits -s vm -n 20
          output-to-summary: true

      - name: Generate output
        id: gen-output
        run: |
          EOF=$(dd if=/dev/urandom bs=15 count=1 status=none | base64)
-          echo "output_${{ matrix.target }}<<$EOF" >> $GITHUB_OUTPUT
+          echo "${{ matrix.target }}-bloaty-output<<$EOF" >> $GITHUB_OUTPUT
          echo "${{ steps.bloaty-step.outputs.bloaty-output-encoded }}" >> $GITHUB_OUTPUT
          echo "$EOF" >> $GITHUB_OUTPUT
+          echo "${{ matrix.target }}-bloaty-summary-map<<$EOF" >> $GITHUB_OUTPUT
+          echo '${{ steps.bloaty-step.outputs.bloaty-summary-map }}' >> $GITHUB_OUTPUT
+          echo "$EOF" >> $GITHUB_OUTPUT

  post_pr_comment:
    name: Publish Results
    runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
    needs: [analyze_flash]
+    env:
+      V5X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-absolute) }}
+      V5X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-percentage) }}
+      V6X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-absolute) }}
+      V6X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-percentage) }}
    if: ${{ github.event.pull_request }}
    steps:
      - name: Find Comment
@@ -92,25 +108,32 @@ jobs:
          echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT

      - name: Create or update comment
+        # This can't be moved to the job-level conditions, as GH actions don't allow a job-level if condition to access the env.
+        if: |
+          steps.fc.outputs.comment-id != '' ||
+          env.V5X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) ||
+          env.V5X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT) ||
+          env.V6X-SUMMARY-MAP-ABS >= fromJSON(env.MIN_FLASH_POS_DIFF_FOR_COMMENT) ||
+          env.V6X-SUMMARY-MAP-ABS <= fromJSON(env.MIN_FLASH_NEG_DIFF_FOR_COMMENT)
        uses: peter-evans/create-or-update-comment@v4
        with:
          comment-id: ${{ steps.fc.outputs.comment-id }}
          issue-number: ${{ github.event.pull_request.number }}
          body: |
-            ## FLASH Analysis
+            ## 🔎 FLASH Analysis
            <details>
-              <summary>px4_fmu-v5x</summary>
+              <summary>px4_fmu-v5x [Total VM Diff: ${{ env.V5X-SUMMARY-MAP-ABS }} byte (${{ env.V5X-SUMMARY-MAP-PERC}} %)]</summary>

              ```
-              ${{ needs.analyze_flash.outputs.px4_fmu-v5x }}
+              ${{ needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-output }}
              ```
            </details>

            <details>
-              <summary>px4_fmu-v6x</summary>
+              <summary>px4_fmu-v6x [Total VM Diff: ${{ env.V6X-SUMMARY-MAP-ABS }} byte (${{ env.V6X-SUMMARY-MAP-PERC }} %)]</summary>

              ```
-              ${{ needs.analyze_flash.outputs.px4_fmu-v6x }}
+              ${{ needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-output }}
              ```
            </details>

@@ -11,131 +11,26 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    env:
-      ACTIONS_ALLOW_USE_UNSECURE_NODE_VERSION: true
    strategy:
      fail-fast: false
      matrix:
        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_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: "iris",          mission: "MC_mission_box"}
+          - {vehicle: "rover",         mission: "rover_mission_1"}

-    container:
-      image: px4io/px4-dev-ros-melodic:2021-09-08
-      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
-    - uses: actions/checkout@v1
+    - uses: actions/checkout@v4
      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
+        fetch-depth: 0

-    - name: Prepare ccache timestamp
-      id: ccache_cache_timestamp
-      shell: cmake -P {0}
-      run: |
-        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
-        message("::set-output name=timestamp::${current_date}")
-    - name: ccache cache files
-      uses: actions/cache@v2
+    - name: Build SITL and Run Tests
+      uses: addnab/docker-run-action@v3
      with:
-        path: ~/.ccache
-        key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
-        restore-keys: sitl_tests-${{matrix.config.build_type}}-ccache-
-    - name: setup ccache
-      run: |
-          mkdir -p ~/.ccache
-          echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf
-          echo "compression = true" >> ~/.ccache/ccache.conf
-          echo "compression_level = 6" >> ~/.ccache/ccache.conf
-          echo "max_size = 100M" >> ~/.ccache/ccache.conf
-          echo "hash_dir = false" >> ~/.ccache/ccache.conf
-          ccache -s
-          ccache -z
-
-    - name: check environment
-      run: |
-          export
-          ulimit -a
-    - name: Build PX4 and sitl_gazebo-classic
-      env:
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: |
-        ccache -z
-        make px4_sitl_default
-        make px4_sitl_default sitl_gazebo-classic
-        ccache -s
-
-    - name: Core dump settings
-      run: |
-          ulimit -c unlimited
-          echo "`pwd`/%e.core" > /proc/sys/kernel/core_pattern
-
-    - name: Run SITL tests
-      env:
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: |
-        export
-        ./test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=${{matrix.config.mission}} vehicle:=${{matrix.config.vehicle}}
-      timeout-minutes: 45
-
-    - name: Look at core files
-      if: failure()
-      run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit"
-    - name: Upload px4 coredump
-      if: failure()
-      uses: actions/upload-artifact@v3
-      with:
-        name: coredump
-        path: px4.core
-
-    - name: ecl EKF analysis
-      if: always()
-      run: ./Tools/ecl_ekf/process_logdata_ekf.py ~/.ros/log/*/*.ulg || true
-
-    - name: Upload logs to flight review
-      if: always()
-      run: ./Tools/upload_log.py -q --description "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID}" --feedback "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID} ${GITHUB_REPOSITORY} ${GITHUB_REF}" --source CI ~/.ros/log/*/*.ulg
-
-    - name: Upload px4 binary
-      if: failure()
-      uses: actions/upload-artifact@v3
-      with:
-        name: binary
-        path: build/px4_sitl_default/bin/px4
-
-    - name: Store PX4 log
-      if: failure()
-      uses: actions/upload-artifact@v3
-      with:
-        name: px4_log
-        path: ~/.ros/log/*/*.ulg
-
-    - name: Store ROS log
-      if: failure()
-      uses: actions/upload-artifact@v3
-      with:
-        name: ros_log
-        path: ~/.ros/**/rostest-*.log
-
-    # Report test coverage
-    - name: Upload coverage
-      if: contains(matrix.config.build_type, 'Coverage')
-      run: |
-          git config --global credential.helper "" # disable the keychain credential helper
-          git config --global --add credential.helper store # enable the local store credential helper
-          echo "https://x-access-token:${{ secrets.ACCESS_TOKEN }}@github.com" >> ~/.git-credentials # add credential
-          git config --global url."https://github.com/".insteadof git@github.com: # credentials add credential
-          mkdir -p coverage
-          lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info
-    - name: Upload coverage information to Codecov
-      if: contains(matrix.config.build_type, 'Coverage')
-      uses: codecov/codecov-action@v1
-      with:
-        token: ${{ secrets.CODECOV_TOKEN }}
-        flags: mavros_mission
-        file: coverage/lcov.info
+        image: px4io/px4-dev-ros-melodic:2021-09-08
+        options: -v ${{ github.workspace }}:/workspace
+        run: |
+          cd /workspace
+          git config --global --add safe.directory /workspace
+          make px4_sitl_default
+          make px4_sitl_default sitl_gazebo-classic
+          ./test/rostest_px4_run.sh mavros_posix_test_mission.test mission:=${{matrix.config.mission}} vehicle:=${{matrix.config.vehicle}}
@@ -17,120 +17,21 @@ jobs:
      fail-fast: false
      matrix:
        config:
-          - {test_file: "mavros_posix_tests_offboard_posctl.test",    vehicle: "iris", build_type: "RelWithDebInfo"}
-          #- {test_file: "mavros_posix_tests_offboard_attctl.test",    vehicle: "iris", build_type: "RelWithDebInfo"}
-          #- {test_file: "mavros_posix_tests_offboard_rpyrt_ctl.test", vehicle: "iris", build_type: "RelWithDebInfo"}
+          - {test_file: "mavros_posix_tests_offboard_posctl.test",    vehicle: "iris"}

-    container:
-      image: px4io/px4-dev-ros-melodic:2021-09-08
-      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
-    - uses: actions/checkout@v1
+    - uses: actions/checkout@v4
      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
+        fetch-depth: 0

-    - name: Prepare ccache timestamp
-      id: ccache_cache_timestamp
-      shell: cmake -P {0}
-      run: |
-        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
-        message("::set-output name=timestamp::${current_date}")
-    - name: ccache cache files
-      uses: actions/cache@v2
+    - name: Build PX4 and Run Tests
+      uses: addnab/docker-run-action@v3
      with:
-        path: ~/.ccache
-        key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
-        restore-keys: sitl_tests-${{matrix.config.build_type}}-ccache-
-    - name: setup ccache
-      run: |
-          mkdir -p ~/.ccache
-          echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf
-          echo "compression = true" >> ~/.ccache/ccache.conf
-          echo "compression_level = 6" >> ~/.ccache/ccache.conf
-          echo "max_size = 100M" >> ~/.ccache/ccache.conf
-          echo "hash_dir = false" >> ~/.ccache/ccache.conf
-          ccache -s
-          ccache -z
-
-    - name: check environment
-      run: |
-          export
-          ulimit -a
-    - name: Build PX4 and sitl_gazebo-classic
-      env:
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: |
-        ccache -z
-        make px4_sitl_default
-        make px4_sitl_default sitl_gazebo-classic
-        ccache -s
-
-    - name: Core dump settings
-      run: |
-          ulimit -c unlimited
-          echo "`pwd`/%e.core" > /proc/sys/kernel/core_pattern
-
-    - name: Run SITL tests
-      env:
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: |
-        export
-        ./test/rostest_px4_run.sh ${{matrix.config.test_file}} vehicle:=${{matrix.config.vehicle}}
-      timeout-minutes: 45
-
-    - name: Look at core files
-      if: failure()
-      run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit"
-    - name: Upload px4 coredump
-      if: failure()
-      uses: actions/upload-artifact@v3
-      with:
-        name: coredump
-        path: px4.core
-
-    - name: ecl EKF analysis
-      if: always()
-      run: ./Tools/ecl_ekf/process_logdata_ekf.py ~/.ros/log/*/*.ulg || true
-
-    - name: Upload logs to flight review
-      if: always()
-      run: ./Tools/upload_log.py -q --description "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID}" --feedback "${GITHUB_WORKFLOW} ${GITHUB_RUN_ID} ${GITHUB_REPOSITORY} ${GITHUB_REF}" --source CI ~/.ros/log/*/*.ulg
-
-    - name: Upload px4 binary
-      if: failure()
-      uses: actions/upload-artifact@v3
-      with:
-        name: binary
-        path: build/px4_sitl_default/bin/px4
-
-    - name: Store PX4 log
-      if: failure()
-      uses: actions/upload-artifact@v3
-      with:
-        name: px4_log
-        path: ~/.ros/log/*/*.ulg
-
-    - name: Store ROS log
-      if: failure()
-      uses: actions/upload-artifact@v3
-      with:
-        name: ros_log
-        path: ~/.ros/**/rostest-*.log
-
-    # Report test coverage
-    - name: Upload coverage
-      if: contains(matrix.config.build_type, 'Coverage')
-      run: |
-          git config --global credential.helper "" # disable the keychain credential helper
-          git config --global --add credential.helper store # enable the local store credential helper
-          echo "https://x-access-token:${{ secrets.ACCESS_TOKEN }}@github.com" >> ~/.git-credentials # add credential
-          git config --global url."https://github.com/".insteadof git@github.com: # credentials add credential
-          mkdir -p coverage
-          lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info
-    - name: Upload coverage information to Codecov
-      if: contains(matrix.config.build_type, 'Coverage')
-      uses: codecov/codecov-action@v1
-      with:
-        token: ${{ secrets.CODECOV_TOKEN }}
-        flags: mavros_offboard
-        file: coverage/lcov.info
+        image: px4io/px4-dev-ros-melodic:2021-09-08
+        options: -v ${{ github.workspace }}:/workspace
+        run: |
+          cd /workspace
+          git config --global --add safe.directory /workspace
+          make px4_sitl_default
+          make px4_sitl_default sitl_gazebo-classic
+          ./test/rostest_px4_run.sh ${{matrix.config.test_file}} vehicle:=${{matrix.config.vehicle}}
@@ -11,22 +11,27 @@ on:
 jobs:
  build:
    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2022-08-12
    strategy:
      matrix:
        config: [
-          px4_fmu-v5,
+          px4_fmu-v5_default,
          ]
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{secrets.ACCESS_TOKEN}}

-    - name: make ${{matrix.config}}
-      env:
-        PX4_EXTRA_NUTTX_CONFIG: "CONFIG_NSH_LOGIN_PASSWORD=\"test\";CONFIG_NSH_CONSOLE_LOGIN=y"
-      run: |
-        echo "PX4_EXTRA_NUTTX_CONFIG: $PX4_EXTRA_NUTTX_CONFIG"
-        make ${{matrix.config}} nuttx_context
-        # Check that the config option is set
-        grep CONFIG_NSH_LOGIN_PASSWORD build/${{matrix.config}}_default/NuttX/nuttx/.config
+    steps:
+    - uses: actions/checkout@v4
+      with:
+        fetch-depth: 0
+
+    - name: Build PX4 and Run Test [${{ matrix.config }}]
+      uses: addnab/docker-run-action@v3
+      with:
+        image: px4io/px4-dev-nuttx-focal:2022-08-12
+        options: -v ${{ github.workspace }}:/workspace
+        run: |
+          cd /workspace
+          git config --global --add safe.directory /workspace
+          export PX4_EXTRA_NUTTX_CONFIG="CONFIG_NSH_LOGIN_PASSWORD=\"test\";CONFIG_NSH_CONSOLE_LOGIN=y"
+          echo "PX4_EXTRA_NUTTX_CONFIG: $PX4_EXTRA_NUTTX_CONFIG"
+          make ${{ matrix.config }} nuttx_context
+          echo "Check that the config option is set"
+          grep CONFIG_NSH_LOGIN_PASSWORD build/${{ matrix.config }}/NuttX/nuttx/.config
@@ -12,14 +12,18 @@ jobs:
  build:
    runs-on: ubuntu-24.04
    steps:
-    - uses: actions/checkout@v1
+    - uses: actions/checkout@v4
      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
+        fetch-depth: 0
+
    - name: Install Python3
      run: sudo apt-get install python3 python3-setuptools python3-pip -y
+
    - name: Install tools
      run: python3 -m pip install mypy types-requests flake8 --break-system-packages
+
    - name: Check MAVSDK test scripts with mypy
      run: $HOME/.local/bin/mypy --strict test/mavsdk_tests/*.py
+
    - name: Check MAVSDK test scripts with flake8
      run: $HOME/.local/bin/flake8 test/mavsdk_tests/*.py
@@ -2,7 +2,7 @@

 [![Releases](https://img.shields.io/github/release/PX4/PX4-Autopilot.svg)](https://github.com/PX4/PX4-Autopilot/releases) [![DOI](https://zenodo.org/badge/22634/PX4/PX4-Autopilot.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/PX4-Autopilot)

-[![Nuttx Targets](https://github.com/PX4/PX4-Autopilot/workflows/Nuttx%20Targets/badge.svg)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22Nuttx+Targets%22?branch=master) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22)
+[![Build Targets](https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml/badge.svg?branch=main)](https://github.com/PX4/PX4-Autopilot/actions/workflows/build_all_targets.yml) [![SITL Tests](https://github.com/PX4/PX4-Autopilot/workflows/SITL%20Tests/badge.svg?branch=master)](https://github.com/PX4/PX4-Autopilot/actions?query=workflow%3A%22SITL+Tests%22)

 [![Discord Shield](https://discordapp.com/api/guilds/1022170275984457759/widget.png?style=shield)](https://discord.gg/dronecode)

@@ -38,4 +38,6 @@ param set-default PWM_MAIN_FUNC2 102
 param set-default PWM_MAIN_FUNC3 103
 param set-default PWM_MAIN_FUNC4 104

+param set-default EKF2_GPS_DELAY 0
+
 param set SIH_VEHICLE_TYPE 0
@@ -45,7 +45,9 @@ param set-default CA_SV_CS1_TRQ_P 1
 param set-default CA_SV_CS1_TYPE 3
 param set-default CA_SV_CS2_TRQ_Y 1
 param set-default CA_SV_CS2_TYPE 4
-param set-default PWM_MAIN_FUNC3 201
-param set-default PWM_MAIN_FUNC4 202
-param set-default PWM_MAIN_FUNC5 203
-param set-default PWM_MAIN_FUNC6 101
+param set-default PWM_MAIN_FUNC1 201
+param set-default PWM_MAIN_FUNC2 202
+param set-default PWM_MAIN_FUNC3 203
+param set-default PWM_MAIN_FUNC4 101
+
+param set-default EKF2_GPS_DELAY 0
@@ -11,6 +11,7 @@
 PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert}

+param set-default EKF2_GPS_DELAY 0
 param set-default EKF2_FUSE_BETA 0 # side slip fusion is currently not supported for tailsitters

 param set-default SENS_EN_GPSSIM 1
@@ -0,0 +1 @@
+mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p
@@ -26,7 +26,6 @@ param set-default RD_MAX_THR_SPD 2.15
 param set-default RD_SPEED_P 0.1
 param set-default RD_SPEED_I 0.01
 param set-default RD_MAX_YAW_RATE 180
-param set-default RD_MISS_SPD_DEF 2
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533
 param set-default RD_MAX_YAW_ACCEL 1000
@@ -29,7 +29,6 @@ param set-default RD_MAX_SPEED 8
 param set-default RD_YAW_P 5
 param set-default RD_YAW_I 0.1
 param set-default RD_MAX_YAW_RATE 30
-param set-default RD_MISS_SPD_DEF 8
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533

@@ -13,18 +13,18 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge

 # Rover parameters
 param set-default RM_WHEEL_TRACK 0.3
-param set-default RM_MAN_YAW_SCALE 0.1
-param set-default RM_YAW_RATE_I 0
-param set-default RM_YAW_RATE_P 0.01
+param set-default RM_YAW_RATE_I 0.1
+param set-default RM_YAW_RATE_P 0.1
 param set-default RM_MAX_ACCEL 3
+param set-default RM_MAX_DECEL 5
 param set-default RM_MAX_JERK 5
-param set-default RM_MAX_SPEED 4
-param set-default RM_MAX_THR_SPD 7
-param set-default RM_MAX_THR_YAW_R 7.5
+param set-default RM_MAX_SPEED 2
+param set-default RM_MAX_THR_SPD 2.2
+param set-default RM_MAX_THR_YAW_R 1.2
 param set-default RM_YAW_P 5
 param set-default RM_YAW_I 0.1
-param set-default RM_MAX_YAW_RATE 180
-param set-default RM_MISS_SPD_DEF 3
+param set-default RM_MAX_YAW_RATE 120
+param set-default RM_MAX_YAW_ACCEL 240
 param set-default RM_MISS_VEL_GAIN 1
 param set-default RM_SPEED_I 0.01
 param set-default RM_SPEED_P 0.1
@@ -42,23 +42,23 @@ param set-default SENS_EN_ARSPDSIM 0

 # Actuator mapping
 param set-default SIM_GZ_WH_FUNC1 102 # right wheel front
-param set-default SIM_GZ_WH_MIN1 0
-param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_MIN1 70
+param set-default SIM_GZ_WH_MAX1 130
 param set-default SIM_GZ_WH_DIS1 100

 param set-default SIM_GZ_WH_FUNC2 101 # left wheel front
-param set-default SIM_GZ_WH_MIN2 0
-param set-default SIM_GZ_WH_MAX2 200
+param set-default SIM_GZ_WH_MIN2 70
+param set-default SIM_GZ_WH_MAX2 130
 param set-default SIM_GZ_WH_DIS2 100

 param set-default SIM_GZ_WH_FUNC3 104 # right wheel back
-param set-default SIM_GZ_WH_MIN3 0
-param set-default SIM_GZ_WH_MAX3 200
+param set-default SIM_GZ_WH_MIN3 70
+param set-default SIM_GZ_WH_MAX3 130
 param set-default SIM_GZ_WH_DIS3 100

 param set-default SIM_GZ_WH_FUNC4 103 # left wheel back
-param set-default SIM_GZ_WH_MIN4 0
-param set-default SIM_GZ_WH_MAX4 200
+param set-default SIM_GZ_WH_MIN4 70
+param set-default SIM_GZ_WH_MAX4 130
 param set-default SIM_GZ_WH_DIS4 100

 param set-default SIM_GZ_WH_REV 10
@@ -0,0 +1,97 @@
+#!/bin/sh
+#
+# @name Quadrotor + Tailsitter
+#
+# @type VTOL Quad Tailsitter
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 0
+
+
+param set-default MAV_TYPE 20
+
+param set-default CA_AIRFRAME 4
+
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.23
+param set-default CA_ROTOR0_KM 0.05
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.23
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.23
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.23
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default CA_SV_CS_COUNT 0
+
+param set-default SIM_GZ_EC_FUNC1 101
+param set-default SIM_GZ_EC_FUNC2 102
+param set-default SIM_GZ_EC_FUNC3 103
+param set-default SIM_GZ_EC_FUNC4 104
+
+param set-default SIM_GZ_EC_MIN1 10
+param set-default SIM_GZ_EC_MIN2 10
+param set-default SIM_GZ_EC_MIN3 10
+param set-default SIM_GZ_EC_MIN4 10
+
+param set-default SIM_GZ_EC_MAX1 1500
+param set-default SIM_GZ_EC_MAX2 1500
+param set-default SIM_GZ_EC_MAX3 1500
+param set-default SIM_GZ_EC_MAX4 1500
+
+param set-default FD_FAIL_R 70
+
+param set-default FW_P_TC 0.6
+
+param set-default FW_PR_I 0.3
+param set-default FW_PR_P 0.5
+param set-default FW_PSP_OFF 2
+param set-default FW_RR_FF 0.1
+param set-default FW_RR_I 0.1
+param set-default FW_RR_P 0.2
+param set-default FW_YR_FF 0 # make yaw rate controller very weak, only keep default P
+param set-default FW_YR_I 0
+param set-default FW_THR_TRIM 0.35
+param set-default FW_THR_MAX 0.8
+param set-default FW_THR_MIN 0.05
+param set-default FW_T_CLMB_MAX 6
+param set-default FW_T_HRATE_FF 0.5
+param set-default FW_T_SINK_MAX 3
+param set-default FW_T_SINK_MIN 1.6
+param set-default FW_AIRSPD_STALL 10
+param set-default FW_AIRSPD_MIN 14
+param set-default FW_AIRSPD_TRIM 18
+param set-default FW_AIRSPD_MAX 22
+
+param set-default MC_AIRMODE 2
+param set-default MAN_ARM_GESTURE 0 # required for yaw airmode
+param set-default MC_ROLL_P 3
+param set-default MC_PITCH_P 3
+param set-default MC_ROLLRATE_P 0.3
+param set-default MC_PITCHRATE_P 0.3
+
+param set-default VT_ARSP_TRANS 15
+param set-default VT_B_TRANS_DUR 5
+param set-default VT_FW_DIFTHR_EN 7
+param set-default VT_FW_DIFTHR_S_Y 1
+param set-default VT_F_TRANS_DUR 1.5
+param set-default VT_TYPE 0
+
+param set-default WV_EN 0
+
+param set-default EKF2_FUSE_BETA 0
@@ -49,6 +49,7 @@ px4_add_romfs_files(
 	1022_gazebo-classic_uuv_bluerov2_heavy
 	1030_gazebo-classic_plane
 	1031_gazebo-classic_plane_cam
+	1031_gazebo-classic_plane_cam.post
 	1032_gazebo-classic_plane_catapult
 	1033_jsbsim_rascal
 	1034_flightgear_rascal-electric
@@ -89,6 +90,7 @@ px4_add_romfs_files(
 	4015_gz_r1_rover_mecanum
 	4016_gz_x500_lidar_down
 	4017_gz_x500_lidar_front
+	4018_gz_quadtailsitter

 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
@@ -18,22 +18,19 @@ param set UAVCAN_ENABLE 0
 param set-default CA_AIRFRAME 1
 param set-default CA_ROTOR_COUNT 1
 param set-default CA_ROTOR0_PX 0.3
-param set-default CA_SV_CS_COUNT 4
-param set-default CA_SV_CS0_TRQ_R 0.5
-param set-default CA_SV_CS0_TYPE 2
+param set-default CA_SV_CS_COUNT 3
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS0_TYPE 1
 param set-default CA_SV_CS1_TRQ_P 1
 param set-default CA_SV_CS1_TYPE 3
 param set-default CA_SV_CS2_TRQ_Y 1
 param set-default CA_SV_CS2_TYPE 4
-param set-default CA_SV_CS3_TYPE 10

-param set-default HIL_ACT_REV 2
-param set-default HIL_ACT_FUNC1 201
-param set-default HIL_ACT_FUNC2 202
-param set-default HIL_ACT_FUNC3 203
-param set-default HIL_ACT_FUNC4 101
-param set-default HIL_ACT_FUNC5 204
-param set-default HIL_ACT_FUNC6 400
+param set HIL_ACT_REV 1
+param set HIL_ACT_FUNC1 201
+param set HIL_ACT_FUNC2 202
+param set HIL_ACT_FUNC3 203
+param set HIL_ACT_FUNC4 101

 # set SYS_HITL to 2 to start the SIH and avoid sensors startup
 param set-default SYS_HITL 2
@@ -11,7 +11,6 @@

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

-
 # Disable PID gains for initial setup. These should be enabled after setting the FF gain.
 # P is expected to be lower than FF.
 param set-default MC_ROLLRATE_P 0
@@ -31,7 +31,6 @@ param set-default RD_MAX_THR_SPD 1.9
 param set-default RD_MAX_THR_YAW_R 0.7
 param set-default RD_MAX_YAW_ACCEL 600
 param set-default RD_MAX_YAW_RATE 250
-param set-default RD_MISS_SPD_DEF 1.5
 param set-default RD_SPEED_P 0.1
 param set-default RD_SPEED_I 0.01
 param set-default RD_TRANS_DRV_TRN 0.785398
@@ -13,8 +13,6 @@ param set-default MAV_TYPE 1
 #
 # Default parameters for fixed wing UAVs.
 #
-param set-default COM_POS_FS_DELAY 5
-
 # there is a 2.5 factor applied on the _FS thresholds if for invalidation
 param set-default COM_POS_FS_EPH 50
 param set-default COM_VEL_FS_EVH 3
@@ -211,6 +211,13 @@ then
 	spl06 -X -a 0x77 start
 fi

+# SPA06 sensor external I2C
+if param compare -s SENS_EN_SPA06 1
+then
+	spa06 -X start
+	spa06 -X -a 0x77 start
+fi
+
 # PCF8583 counter (RPM sensor)
 if param compare -s SENS_EN_PCF8583 1
 then
@@ -354,6 +354,11 @@ else
 	then
 		pps_capture start
 	fi
+	# RPM capture driver
+	if param greater -s RPM_CAP_ENABLE 0
+	then
+		rpm_capture start
+	fi
 	# Camera capture driver
 	if param greater -s CAM_CAP_FBACK 0
 	then
@@ -43,7 +43,7 @@ fi

 # install git pre-commit hook
 HOOK_FILE="$DIR/../../.git/hooks/pre-commit"
-if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ]; then
+if [ ! -f $HOOK_FILE ] && [ "$CI" != "true" ] && [ $- == *i* ]; then
 	echo ""
 	echo -e "\033[31mNinja tip: add a git pre-commit hook to automatically check code style\033[0m"
 	echo -e "Would you like to install one now? (\033[94mcp ./Tools/astyle/pre-commit .git/hooks/pre-commit\033[0m): [y/\033[1mN\033[0m]"
@@ -73,7 +73,7 @@ def process_target(px4board_file, target_name):
        px4board_file.endswith("bootloader.px4board"):
        kconf.load_config(px4board_file, replace=True)
    else: # Merge config with default.px4board
-        default_kconfig = re.sub(r'[a-zA-Z\d_]+\.px4board', 'default.px4board', px4board_file)
+        default_kconfig = re.sub(r'[a-zA-Z\d_-]+\.px4board', 'default.px4board', px4board_file)
        kconf.load_config(default_kconfig, replace=True)
        kconf.load_config(px4board_file, replace=False)

@@ -1,62 +1,91 @@
 #!/usr/bin/env python3

+import sys
+
+try:
+    from Crypto.Cipher import ChaCha20
+except ImportError as e:
+    print("Failed to import crypto: " + str(e))
+    print("")
+    print("You may need to install it using:")
+    print("    pip3 install --user pycryptodome")
+    print("")
+    sys.exit(1)
+
 from Crypto.PublicKey import RSA
 from Crypto.Cipher import PKCS1_OAEP
-from Crypto.Cipher import ChaCha20
 from Crypto.Hash import SHA256
-import binascii
+from pathlib import Path
 import argparse
-#from pathlib import Path
-import sys
+

 if __name__ == "__main__":

    parser = argparse.ArgumentParser(description="""CLI tool to decrypt an ulog file\n""")
-    parser.add_argument("ulog_file", help=".ulog file", nargs='?', default=None)
-    parser.add_argument("ulog_key", help=".ulogk, encrypted key", nargs='?', default=None)
+    parser.add_argument("ulog_file", help=".ulge/.ulgc, encrypted log file", nargs='?', default=None)
+    parser.add_argument("ulog_key", help=".ulgk, legacy encrypted key (give empty string '' to ignore for .ulge)", nargs='?', default=None)
    parser.add_argument("rsa_key", help=".pem format key for decrypting the ulog key", nargs='?', default=None)

    args = parser.parse_args()

-    # Only generate a key pair, don't sign
-    if not args.ulog_file or not args.ulog_key or not args.rsa_key:
-        print('Need all arguments, the encrypted ulog file, the key and the key decryption key')
-        sys.exit(1);
+    # Check all arguments are given
+    if not args.rsa_key:
+        print('Need all arguments, the encrypted ulog file, key file (or empty string if not needed) and the key decryption key (.pem)')
+        sys.exit(1)

    # Read the private RSA key to decrypt the cahcha key
    with open(args.rsa_key, 'rb') as f:
        r = RSA.importKey(f.read(), passphrase='')

-    # Read the encrypted xchacha key and the nonce
-    with open(args.ulog_key, 'rb') as f:
+    if args.ulog_key == "":
+        key_data_filename = args.ulog_file
+        magic = "ULogEnc"
+    else:
+        key_data_filename = args.ulog_key
+        magic = "ULogKey"
+
+    with open(key_data_filename, 'rb') as f:
+        # Read the encrypted xchacha key and the nonce
        ulog_key_header = f.read(22)

        # Parse the header
        try:
            # magic
-            if not ulog_key_header.startswith(bytearray("ULogKey".encode())):
+            if not ulog_key_header.startswith(bytearray(magic.encode())):
+                print("Incorrect header magic")
                raise Exception()
            # version
            if ulog_key_header[7] != 1:
+                print("Unsupported header version")
                raise Exception()
            # expected key exchange algorithm (RSA_OAEP)
            if ulog_key_header[16] != 4:
+                print("Unsupported key algorithm")
                raise Exception()
-            key_size = ulog_key_header[19] << 8 | ulog_key_header[18];
-            nonce_size = ulog_key_header[21] << 8 | ulog_key_header[20];
+            key_size = ulog_key_header[19] << 8 | ulog_key_header[18]
+            nonce_size = ulog_key_header[21] << 8 | ulog_key_header[20]
            ulog_key_cipher = f.read(key_size)
            nonce = f.read(nonce_size)
        except:
-            print("Keyfile format error")
-            sys.exit(1);
+            print("Keydata format error")
+            sys.exit(1)
+
+    if magic == "ULogEnc":
+        data_offset = 22 + key_size + nonce_size
+    else:
+        data_offset = 0

    # Decrypt the xchacha key
    cipher_rsa = PKCS1_OAEP.new(r,SHA256)
    ulog_key = cipher_rsa.decrypt(ulog_key_cipher)
    #print(binascii.hexlify(ulog_key))

-    # Read and decrypt the .ulgc
+    # Read and decrypt the ulog data
    cipher = ChaCha20.new(key=ulog_key, nonce=nonce)
+
+    outfilename = Path(args.ulog_file).stem + ".ulog"
    with open(args.ulog_file, 'rb') as f:
-        with open(args.ulog_file.rstrip(args.ulog_file[-1]), 'wb') as out:
+        if data_offset > 0:
+            f.seek(data_offset)
+        with open(outfilename, 'wb') as out:
            out.write(cipher.decrypt(f.read()))
@@ -137,7 +137,7 @@ div.frame_variant td, div.frame_variant th {
                            #print(output_name,value, attribstrs[0].strip(),attribstrs[1].strip())
                    outputs += '</ul>'
                    if has_outputs:
-                        outputs_entry = '<p><b>Specific Outputs:</b>' + outputs + '</p>'
+                        outputs_entry = '<br><b>Specific Outputs:</b>' + outputs
                    else:
                        outputs_entry = ''

@@ -0,0 +1 @@
+CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
@@ -11,4 +11,7 @@ then
 	icm42688p -R 6 -s start
 fi

-bmp280 -X start
+if ! bmp280 -X start
+then
+	spa06 -X start
+fi
@@ -12,4 +12,7 @@ then
  fi
 fi

-bmp280 -X start
+if ! bmp280 -X start
+then
+	spa06 -X start
+fi
@@ -9,4 +9,7 @@ then
 	icm42688p -R 0 -s start
 fi

-bmp280 -X start
+if ! bmp280 -X start
+then
+	spa06 -X start
+fi
@@ -67,7 +67,6 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
@@ -0,0 +1 @@
+CONFIG_BOARD_LINKER_PREFIX="flash-analysis"
@@ -0,0 +1,6 @@
+INCLUDE "script.ld"
+
+MEMORY
+{
+    FLASH_AXIM (rx) : ORIGIN = 0x08008000, LENGTH = 10080K
+}
@@ -68,7 +68,6 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
-CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
@@ -0,0 +1 @@
+CONFIG_BOARD_LINKER_PREFIX="flash-analysis"
@@ -0,0 +1,6 @@
+INCLUDE "script.ld"
+
+MEMORY
+{
+    FLASH (rx) : ORIGIN = 0x08020000, LENGTH = 10080K
+}
@@ -193,6 +193,7 @@ set(msg_files
 	RoverMecanumSetpoint.msg
 	RoverMecanumStatus.msg
 	Rpm.msg
+	RpmControlStatus.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg
 	SatelliteInfo.msg
@@ -1,4 +1,4 @@
-uint64 timestamp	# Time since system start (microseconds)
-uint64 error_count	# Timer overcapture error flag (AUX5 or MAIN5)
-uint32 pulse_width	# Pulse width, timer counts
-uint32 period		# Period, timer counts
+uint64 timestamp   # Time since system start (microseconds)
+uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5)
+uint32 pulse_width # Pulse width, timer counts (microseconds)
+uint32 period      # Period, timer counts (microseconds)
@@ -5,7 +5,7 @@ float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized forward s
 float32 lateral_speed_setpoint            # [m/s] Desired lateral speed
 float32 lateral_speed_setpoint_normalized # [-1, 1] Desired normalized lateral speed
 float32 yaw_rate_setpoint                 # [rad/s] Desired yaw rate
-float32 yaw_rate_setpoint_normalized      # [-1, 1] Desired normalized yaw rate
+float32 speed_diff_setpoint_normalized    # [-1, 1] Normalized speed difference between the left and right wheels
 float32 yaw_setpoint 	                  # [rad] Desired yaw (heading)

 # TOPICS rover_mecanum_setpoint
@@ -1,13 +1,17 @@
 uint64 timestamp # time since system start (microseconds)

-float32 measured_forward_speed        # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
-float32 measured_lateral_speed        # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
-float32 adjusted_yaw_rate_setpoint    # [rad/s] Yaw rate setpoint output of the closed loop yaw controller
-float32 measured_yaw_rate  	      # [rad/s] Measured yaw rate
-float32 measured_yaw        	      # [rad] Measured yaw
-float32 pid_yaw_rate_integral  	      # Integral of the PID for the closed loop yaw rate controller
-float32 pid_yaw_integral       	      # Integral of the PID for the closed loop yaw controller
-float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller
-float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller
+float32 measured_forward_speed          # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
+float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate
+float32 measured_lateral_speed          # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
+float32 adjusted_lateral_speed_setpoint # [m/s] Speed setpoint after applying slew rate
+float32 measured_yaw_rate  	        # [rad/s] Measured yaw rate
+float32 clyaw_yaw_rate_setpoint         # [rad/s] Yaw rate setpoint output by the closed loop yaw controller
+float32 adjusted_yaw_rate_setpoint      # [rad/s] Yaw rate setpoint from the closed loop yaw controller
+float32 measured_yaw        	        # [rad] Measured yaw
+float32 adjusted_yaw_setpoint           # [rad] Yaw setpoint after applying slew rate
+float32 pid_yaw_rate_integral  	        # Integral of the PID for the closed loop yaw rate controller
+float32 pid_yaw_integral       	        # Integral of the PID for the closed loop yaw controller
+float32 pid_forward_throttle_integral   # Integral of the PID for the closed loop forward speed controller
+float32 pid_lateral_throttle_integral   # Integral of the PID for the closed loop lateral speed controller

 # TOPICS rover_mecanum_status
@@ -1,4 +1,5 @@
 uint64 timestamp                      # time since system start (microseconds)

-float32 indicated_frequency_rpm       # indicated rotor Frequency in Revolution per minute
-float32 estimated_accurancy_rpm       # estimated accuracy in Revolution per minute
+float32 rpm_estimate # filtered revolutions per minute
+float32 rpm_raw # measured rpm
+float32 estimated_accurancy_rpm # estimated accuracy
@@ -0,0 +1,7 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 rpm_raw # measured rpm
+float32 rpm_estimate # filtered rpm
+float32 rpm_setpoint # desired rpm
+
+float32 output
@@ -11,6 +11,7 @@ menu "barometer"
        select DRIVERS_BAROMETER_MS5611
        select DRIVERS_BAROMETER_MAIERTEK_MPC2520
        select DRIVERS_BAROMETER_GOERTEK_SPL06
+        select DRIVERS_BAROMETER_GOERTEK_SPA06
        select DRIVERS_BAROMETER_INVENSENSE_ICP101XX
        select DRIVERS_BAROMETER_INVENSENSE_ICP201XX
        ---help---
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2022-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
@@ -32,3 +32,4 @@
 ############################################################################

 add_subdirectory(spl06)
+add_subdirectory(spa06)
@@ -0,0 +1,45 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE drivers__barometer__spa06
+	MAIN spa06
+	SRCS
+		SPA06.cpp
+		SPA06.hpp
+		SPA06_I2C.cpp
+		SPA06_SPI.cpp
+		spa06_main.cpp
+	DEPENDS
+		px4_work_queue
+	)
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_BAROMETER_GOERTEK_SPA06
+	bool "spa06"
+	default n
+	---help---
+		Enable support for spa06
@@ -0,0 +1,259 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "SPA06.hpp"
+
+SPA06::SPA06(const I2CSPIDriverConfig &config, spa06::ISPA06 *interface) :
+	I2CSPIDriver(config),
+	_interface(interface),
+	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
+	_measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")),
+	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors"))
+{
+}
+
+SPA06::~SPA06()
+{
+	// free perf counters
+	perf_free(_sample_perf);
+	perf_free(_measure_perf);
+	perf_free(_comms_errors);
+
+	delete _interface;
+}
+
+/*
+float
+SPA06::scale_factor(int oversampling_rate)
+{
+	float k;
+
+	switch (oversampling_rate) {
+	case 1:
+		k = 524288.0f;
+		break;
+
+	case 2:
+		k = 1572864.0f;
+		break;
+
+	case 4:
+		k = 3670016.0f;
+		break;
+
+	case 8:
+		k = 7864320.0f;
+		break;
+
+	case 16:
+		k = 253952.0f;
+		break;
+
+	case 32:
+		k = 516096.0f;
+		break;
+
+	case 64:
+		k = 1040384.0f;
+		break;
+
+	case 128:
+		k = 2088960.0f;
+		break;
+
+	default:
+		k = 0;
+		break;
+	}
+
+	return k;
+}
+*/
+
+int
+SPA06::calibrate()
+{
+	uint8_t buf[21];
+
+	_interface->read(SPA06_ADDR_CAL, buf, sizeof(buf));
+
+	_cal.c0 = (uint16_t)(buf[0]) << 4 | (uint16_t)(buf[1]) >> 4;
+	// If value is negative, we need to fill the missing bits.
+	_cal.c0 = (_cal.c0 & 1 << 11) ? (0xf000 | _cal.c0) : _cal.c0;
+
+	_cal.c1 = (uint16_t)(buf[1] & 0x0F) << 8 | buf[2];
+	_cal.c1 = (_cal.c1 & 1 << 11) ? (0xf000 | _cal.c1) : _cal.c1;
+
+	_cal.c00 = (uint32_t)(buf[3]) << 12 | (uint32_t)(buf[4]) << 4 | (buf[5]) >> 4;
+	_cal.c00 = (_cal.c00 & 1 << 19) ? (0xfff00000 | _cal.c00) : _cal.c00;
+
+	_cal.c10 = (uint32_t)(buf[5] & 0x0F) << 16 | (uint32_t)(buf[6]) << 8 | buf[7];
+	_cal.c10 = (_cal.c10 & 1 << 19) ? (0xfff00000 | _cal.c10) : _cal.c10;
+
+	_cal.c01 = (uint16_t)(buf[8]) << 8 | buf[9];
+
+	_cal.c11 = (uint16_t)(buf[10]) << 8 | buf[11];
+
+	_cal.c20 = (uint16_t)(buf[12]) << 8 | buf[13];
+
+	_cal.c21 = (uint16_t)(buf[14]) << 8 | buf[15];
+
+	_cal.c30 = (uint16_t)(buf[16]) << 8 | buf[17];
+
+	_cal.c31 = (uint16_t)(buf[18]) << 4 | (uint16_t)(buf[19] & 0xF0) >> 4;
+	_cal.c31 = (_cal.c31 & 1 << 11) ? (0xf000 | _cal.c31) : _cal.c31;
+
+	_cal.c40 = (uint16_t)(buf[19] & 0x0F) << 8 | buf[20];
+	_cal.c40 = (_cal.c40 & 1 << 11) ? (0xf000 | _cal.c40) : _cal.c40;
+
+	PX4_DEBUG("c0:%d\nc1:%d\nc00:%ld\nc10:%ld\nc01:%d\nc11:%d\nc20:%d\nc21:%d\nc30:%d\nc31:%d\nc40:%d\n",
+		  _cal.c0, _cal.c1,
+		  _cal.c00, _cal.c10,
+		  _cal.c01, _cal.c11, _cal.c20, _cal.c21, _cal.c30, _cal.c31, _cal.c40);
+
+	return OK;
+}
+int
+SPA06::init()
+{
+	int8_t tries = 5;
+	// reset sensor
+	_interface->set_reg(SPA06_VALUE_RESET, SPA06_ADDR_RESET);
+	usleep(10000);
+
+	// check id
+	if (_interface->get_reg(SPA06_ADDR_ID) != SPA06_VALUE_ID) {
+		PX4_DEBUG("id of your baro is not: 0x%02x", SPA06_VALUE_ID);
+		return -EIO;
+	}
+
+	while (tries--) {
+		uint8_t meas_cfg = _interface->get_reg(SPA06_ADDR_MEAS_CFG);
+
+		if (meas_cfg & (1 << 7) && meas_cfg & (1 << 6)) {
+			break;
+		}
+
+		usleep(10000);
+	}
+
+	if (tries < 0) {
+		PX4_DEBUG("spa06 sensor or coef not ready");
+		return -EIO;
+	}
+
+	// get calibration and pre process them
+	calibrate();
+
+	// set config, recommended settings
+	_interface->set_reg(_curr_prs_cfg, SPA06_ADDR_PRS_CFG);
+	kp = 253952.0f; // refer to scale_factor()
+	_interface->set_reg(_curr_tmp_cfg, SPA06_ADDR_TMP_CFG);
+	kt = 524288.0f;
+
+	// Enable FIFO
+	_interface->set_reg(1 << 2, SPA06_ADDR_CFG_REG);
+	// Continuous pressure and temperature mesasurement.
+	_interface->set_reg(7, SPA06_ADDR_MEAS_CFG);
+
+	Start();
+
+	return OK;
+}
+
+void
+SPA06::Start()
+{
+	// schedule a cycle to start things
+	ScheduleNow();
+}
+
+void
+SPA06::RunImpl()
+{
+	collect();
+
+	ScheduleDelayed(_measure_interval);
+}
+int
+SPA06::collect()
+{
+	perf_begin(_sample_perf);
+
+	// this should be fairly close to the end of the conversion, so the best approximation of the time
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
+	if (_interface->read(SPA06_ADDR_DATA, (uint8_t *)&_data, sizeof(_data)) != OK) {
+		perf_count(_comms_errors);
+		perf_cancel(_sample_perf);
+		return -EIO;
+	}
+
+	int32_t temp_raw = (uint32_t)_data.t_msb << 16 | (uint32_t)_data.t_lsb << 8 | (uint32_t)_data.t_xlsb;
+	temp_raw = (temp_raw & 1 << 23) ? (0xff000000 | temp_raw) : temp_raw;
+
+	int32_t press_raw = (uint32_t)_data.p_msb << 16 | (uint32_t) _data.p_lsb << 8 | (uint32_t) _data.p_xlsb;
+	press_raw = (press_raw & 1 << 23) ? (0xff000000 | press_raw) : press_raw;
+
+	// calculate
+	float ftsc = (float)temp_raw / kt;
+	float fpsc = (float)press_raw / kp;
+	float qua2 = (float)_cal.c10 + fpsc * ((float)_cal.c20 + fpsc * ((float)_cal.c30) + fpsc * (float)_cal.c40);
+	float qua3 = ftsc * fpsc * ((float)_cal.c11 + fpsc * ((float)_cal.c21) + fpsc * (float)_cal.c31);
+
+	float fp = (float)_cal.c00 + fpsc * qua2 + ftsc * (float)_cal.c01 + qua3;
+	float temperature = (float)_cal.c0 * 0.5f + (float)_cal.c1 * ftsc;
+
+
+	sensor_baro_s sensor_baro{};
+	sensor_baro.timestamp_sample = timestamp_sample;
+	sensor_baro.device_id = _interface->get_device_id();
+	sensor_baro.pressure = fp;
+	sensor_baro.temperature = temperature;
+	sensor_baro.error_count = perf_event_count(_comms_errors);
+	sensor_baro.timestamp = hrt_absolute_time();
+	_sensor_baro_pub.publish(sensor_baro);
+
+	perf_end(_sample_perf);
+
+	return OK;
+}
+
+void
+SPA06::print_status()
+{
+	I2CSPIDriverBase::print_status();
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_measure_perf);
+	perf_print_counter(_comms_errors);
+}
@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "spa06.h"
+
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/perf/perf_counter.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/sensor_baro.h>
+
+class SPA06 : public I2CSPIDriver<SPA06>
+{
+public:
+	SPA06(const I2CSPIDriverConfig &config, spa06::ISPA06 *interface);
+	virtual ~SPA06();
+
+	static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
+	static void print_usage();
+
+	int			init();
+	void			print_status();
+
+	void			RunImpl();
+private:
+	void			Start();
+	// float scale_factor(int oversampling_rate);
+
+	int			collect(); //get results and publish
+	int			calibrate();
+
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
+
+	spa06::ISPA06		*_interface;
+	spa06::data_s		_data;
+	spa06::calibration_s	_cal{};
+
+	// set config, recommended settings
+	//
+	// oversampling rate  : single | 2       | 4       | 8       | 16     | 32     | 64      | 128
+	// scale factor(KP/KT): 524288 | 1572864 | 3670016 | 7864320 | 253952 | 516096 | 1040384 | 2088960
+
+	// configuration of pressure measurement rate (PM_RATE) and resolution (PM_PRC)
+	//
+	// PM_RATE[7:4]    : 0 | 1 | 2 | 3 | 4  | 5  | 6  | 7   | 8     | 9    | 10   | 11   | 12 | 13 | 14  | 15
+	// measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 | 25/16 | 25/8 | 25/4 | 25/2 | 25 | 50 | 100 | 200
+	// note: applicable for measurements in background mode only
+	//
+	// PM_PRC[3:0]         : 0      | 1   | 2   | 3    | 4    | 5    | 6     | 7
+	// oversampling (times): single | 2   | 4   | 8    | 16   | 32   | 64    | 128
+	// measurement time(ms): 3.6    | 5.2 | 8.4 | 14.8 | 27.6 | 53.2 | 104.4 | 206.8
+	// precision(PaRMS)    : 5.0    |     | 2.5 |      | 1.2  | 0.9  | 0.5   |
+	// note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register
+	//
+	// -> 32 measurements per second, 16 oversampling
+	static constexpr uint8_t	_curr_prs_cfg{5 << 4 | 4};
+
+	// configuration of temperature measurment rate (TMP_RATE) and resolution (TMP_PRC)
+	//
+	// temperature measurement: internal sensor (in ASIC) | external sensor (in pressure sensor MEMS element)
+	// PM_RATE[7:4]    : 0 | 1 | 2 | 3 | 4  | 5  | 6  | 7   | 8     | 9    | 10   | 11   | 12 | 13 | 14  | 15
+	// measurement rate: 1 | 2 | 4 | 8 | 16 | 32 | 64 | 128 | 25/16 | 25/8 | 25/4 | 25/2 | 25 | 50 | 100 | 200
+	// note: applicable for measurements in background mode only
+	//
+	// TMP_PRC[2:0]        : 0      | 1 | 2 | 3 | 4  | 5  | 6  | 7
+	// oversampling (times): single | 2 | 4 | 8 | 16 | 32 | 64 | 128
+	// note: single(default) measurement time 3.6ms, other settings are optional, and may not be relevant
+	// note: use in combination with a bit shift when the oversampling rate is > 8 times. see CFG_REG(0x19) register
+
+	// -> 32 measurements per second, single oversampling
+	static constexpr uint8_t	_curr_tmp_cfg{5 << 4 | 0};
+
+	bool			_collect_phase{false};
+	float kp;
+	float kt;
+
+	perf_counter_t		_sample_perf;
+	perf_counter_t		_measure_perf;
+	perf_counter_t		_comms_errors;
+
+	static constexpr uint32_t _sample_rate{32};
+	static constexpr uint32_t _measure_interval{1000000 / _sample_rate};
+};
@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file SPA06_I2C.cpp
+ *
+ * SPI interface for Goertek SPA06
+ */
+
+#include "spa06.h"
+
+#include <px4_platform_common/px4_config.h>
+#include <drivers/device/i2c.h>
+
+#if defined(CONFIG_I2C)
+
+class SPA06_I2C: public device::I2C, public spa06::ISPA06
+{
+public:
+	SPA06_I2C(uint8_t bus, uint32_t device, int bus_frequency);
+	virtual ~SPA06_I2C() override = default;
+
+	int init() override { return I2C::init(); }
+
+	uint8_t	get_reg(uint8_t addr) override;
+	int	set_reg(uint8_t value, uint8_t addr) override;
+
+	int read(uint8_t addr, uint8_t *buf, uint8_t len) override;
+	//spa06::data_s		*get_data(uint8_t addr) override;
+	//spa06::calibration_s	*get_calibration(uint8_t addr) override;
+
+	uint32_t get_device_id() const override { return device::I2C::get_device_id(); }
+
+	uint8_t get_device_address() const override { return device::I2C::get_device_address(); }
+private:
+	spa06::calibration_s	_cal{};
+	spa06::data_s		_data{};
+};
+
+spa06::ISPA06 *spa06_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency)
+{
+	return new SPA06_I2C(busnum, device, bus_frequency);
+}
+
+SPA06_I2C::SPA06_I2C(uint8_t bus, uint32_t device, int bus_frequency) :
+	I2C(DRV_BARO_DEVTYPE_SPA06, MODULE_NAME, bus, device, bus_frequency)
+{
+}
+
+uint8_t
+SPA06_I2C::get_reg(uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr), 0};
+	transfer(&cmd[0], 1, &cmd[1], 1);
+
+	return cmd[1];
+}
+
+int
+SPA06_I2C::set_reg(uint8_t value, uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr), value};
+	return transfer(cmd, sizeof(cmd), nullptr, 0);
+}
+
+int
+SPA06_I2C::read(uint8_t addr, uint8_t *buf, uint8_t len)
+{
+	return transfer(&addr, 1, buf, len);
+}
+
+#endif // CONFIG_I2C
@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file SPA06_SPI.cpp
+ *
+ * SPI interface for Goertek SPA06
+ */
+
+#include "spa06.h"
+
+#include <px4_platform_common/px4_config.h>
+#include <drivers/device/spi.h>
+
+#if defined(CONFIG_SPI)
+
+/* SPI protocol address bits */
+#define DIR_READ			(1<<7)  //for set
+#define DIR_WRITE			~(1<<7) //for clear
+
+class SPA06_SPI: public device::SPI, public spa06::ISPA06
+{
+public:
+	SPA06_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
+	virtual ~SPA06_SPI() override = default;
+
+	int init() override { return SPI::init(); }
+
+	uint8_t	get_reg(uint8_t addr) override;
+	int	set_reg(uint8_t value, uint8_t addr) override;
+
+	int read(uint8_t addr, uint8_t *buf, uint8_t len) override;
+
+	uint32_t get_device_id() const override { return device::SPI::get_device_id(); }
+
+	uint8_t get_device_address() const override { return device::SPI::get_device_address(); }
+};
+
+spa06::ISPA06 *
+spa06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode)
+{
+	return new SPA06_SPI(busnum, device, bus_frequency, spi_mode);
+}
+
+SPA06_SPI::SPA06_SPI(uint8_t bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
+	SPI(DRV_BARO_DEVTYPE_SPA06, MODULE_NAME, bus, device, spi_mode, bus_frequency)
+{
+}
+
+uint8_t
+SPA06_SPI::get_reg(uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr | DIR_READ), 0}; // set MSB bit
+	transfer(&cmd[0], &cmd[0], 2);
+
+	return cmd[1];
+}
+
+int
+SPA06_SPI::set_reg(uint8_t value, uint8_t addr)
+{
+	uint8_t cmd[2] = { (uint8_t)(addr & DIR_WRITE), value}; // clear MSB bit
+	return transfer(&cmd[0], nullptr, 2);
+}
+
+int
+SPA06_SPI::read(uint8_t addr, uint8_t *buf, uint8_t len)
+{
+	uint8_t tx_buf[len + 1] = {(uint8_t)(addr | DIR_READ)}; // GCC support VLA, let's use it
+
+	return transfer(tx_buf, buf, len);
+}
+
+#endif // CONFIG_SPI
@@ -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.
+ *
+ ****************************************************************************/
+
+/**
+ * Goertek SPA06 Barometer (external I2C)
+ *
+ * @reboot_required true
+ * @group Sensors
+ * @boolean
+  */
+PARAM_DEFINE_INT32(SENS_EN_SPA06, 0);
@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file spa06.h
+ *
+ * Shared defines for the SPA06 driver.
+ */
+#pragma once
+
+#include <px4_platform_common/i2c_spi_buses.h>
+
+#define SPA06_ADDR_ID	0x0d
+#define SPA06_ADDR_RESET 0x0c   // set to reset
+#define SPA06_ADDR_CAL 0x10
+#define SPA06_ADDR_PRS_CFG  0x06
+#define SPA06_ADDR_TMP_CFG  0x07
+#define SPA06_ADDR_MEAS_CFG	0x08
+#define SPA06_ADDR_CFG_REG	0x09
+#define SPA06_ADDR_DATA 0x00
+
+
+#define SPA06_VALUE_RESET 9
+#define SPA06_VALUE_ID    0x11
+
+namespace spa06
+{
+
+#pragma pack(push,1)
+struct calibration_s {
+	int16_t c0, c1;
+	int32_t c00, c10;
+	int16_t c01, c11, c20, c21, c30, c31, c40;
+};
+
+struct data_s {
+	uint8_t p_msb;
+	uint8_t p_lsb;
+	uint8_t p_xlsb;
+
+	uint8_t t_msb;
+	uint8_t t_lsb;
+	uint8_t t_xlsb;
+};
+#pragma pack(pop)
+
+class ISPA06
+{
+public:
+	virtual ~ISPA06() = default;
+
+	virtual int init() = 0;
+
+	// read reg value
+	virtual uint8_t get_reg(uint8_t addr) = 0;
+
+	// write reg value
+	virtual int set_reg(uint8_t value, uint8_t addr) = 0;
+
+	// bulk read of data into buffer, return same pointer
+	virtual int read(uint8_t addr, uint8_t *buf, uint8_t len) = 0;
+	// bulk read of calibration data into buffer, return same pointer
+
+	virtual uint32_t get_device_id() const = 0;
+
+	virtual uint8_t get_device_address() const = 0;
+};
+
+} // namespace spa06
+
+
+#if defined(CONFIG_SPI)
+extern spa06::ISPA06 *spa06_spi_interface(uint8_t busnum, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
+#endif // CONFIG_SPI
+#if defined(CONFIG_I2C)
+extern spa06::ISPA06 *spa06_i2c_interface(uint8_t busnum, uint32_t device, int bus_frequency);
+#endif // CONFIG_I2C
@@ -0,0 +1,141 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+#include "SPA06.hpp"
+
+#include <drivers/drv_sensor.h>
+
+extern "C" { __EXPORT int spa06_main(int argc, char *argv[]); }
+
+void
+SPA06::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("spa06", "driver");
+	PRINT_MODULE_USAGE_SUBCATEGORY("baro");
+	PRINT_MODULE_USAGE_COMMAND("start");
+#if defined(CONFIG_I2C)
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x76);
+#else
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true);
+#endif
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+I2CSPIDriverBase *SPA06::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
+{
+	spa06::ISPA06 *interface = nullptr;
+
+#if defined(CONFIG_I2C)
+
+	if (config.bus_type == BOARD_I2C_BUS) {
+		interface = spa06_i2c_interface(config.bus, config.i2c_address, config.bus_frequency);
+
+	}
+
+#endif // CONFIG_I2C
+#if defined(CONFIG_SPI)
+
+	if (config.bus_type == BOARD_SPI_BUS) {
+		interface = spa06_spi_interface(config.bus, config.spi_devid, config.bus_frequency, config.spi_mode);
+	}
+
+#endif // CONFIG_SPI
+
+	if (interface == nullptr) {
+		PX4_ERR("failed creating interface for bus %i", config.bus);
+		return nullptr;
+	}
+
+	if (interface->init() != OK) {
+		delete interface;
+		PX4_DEBUG("no device on bus %i", config.bus);
+		return nullptr;
+	}
+
+	SPA06 *dev = new SPA06(config, interface);
+
+	if (dev == nullptr) {
+		delete interface;
+		return nullptr;
+	}
+
+	if (OK != dev->init()) {
+		delete dev;
+		return nullptr;
+	}
+
+	return dev;
+}
+
+int
+spa06_main(int argc, char *argv[])
+{
+	using ThisDriver = SPA06;
+	BusCLIArguments cli{true, true};
+#if defined(CONFIG_I2C)
+	cli.i2c_address = 0x76;
+	cli.default_i2c_frequency = 100 * 1000;
+#endif // CONFIG_I2C
+#if defined(CONFIG_SPI)
+	cli.default_spi_frequency = 10 * 1000 * 1000;
+#endif // CONFIG_SPI
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_BARO_DEVTYPE_SPA06);
+
+	if (!strcmp(verb, "start")) {
+		return ThisDriver::module_start(cli, iterator);
+	}
+
+	if (!strcmp(verb, "stop")) {
+		return ThisDriver::module_stop(iterator);
+	}
+
+	if (!strcmp(verb, "status")) {
+		return ThisDriver::module_status(iterator);
+	}
+
+	ThisDriver::print_usage();
+	return -1;
+}
@@ -109,7 +109,7 @@ LidarLitePWM::measure()
 		return PX4_ERROR;
 	}

-	const float current_distance = float(_pwm.pulse_width) * 1e-3f;   /* 10 usec = 1 cm distance for LIDAR-Lite */
+	const float current_distance = float(_pwm.pulse_width) * 1e-3f; // 1us = 1mm distance for LIDAR-Lite

 	/* Due to a bug in older versions of the LidarLite firmware, we have to reset sensor on (distance == 0) */
 	if (current_distance <= 0.0f) {
@@ -247,6 +247,7 @@

 #define DRV_DIFF_PRESS_DEVTYPE_AUAV	0xE6
 #define DRV_BARO_DEVTYPE_AUAV		0xE7
+#define DRV_BARO_DEVTYPE_SPA06          0xE8

 #define DRV_DEVTYPE_UNUSED		0xff

@@ -195,12 +195,12 @@ void PCF8583::RunImpl()
 	}

 	// Calculate RPM and accuracy estimation
-	float indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1000000.f)) * 60.f;
-	float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1000000) * 60.f;
+	float indicated_rpm = (((float)diffCount / _param_pcf8583_magnet.get()) / ((float)diffTime / 1e6f)) * 60.f;
+	float estimated_accurancy = 1 / (float)_param_pcf8583_magnet.get() / ((float)diffTime / 1e6f) * 60.f;

 	// publish data to uorb
 	rpm_s msg{};
-	msg.indicated_frequency_rpm = indicated_rpm;
+	msg.rpm_estimate = indicated_rpm;
 	msg.estimated_accurancy_rpm = estimated_accurancy;
 	msg.timestamp = hrt_absolute_time();
 	_rpm_pub.publish(msg);
@@ -66,7 +66,7 @@ int rpm_simulator_main(int argc, char *argv[])

 	// prpepare RPM data message
 	rpm.timestamp = timestamp_us;
-	rpm.indicated_frequency_rpm = frequency;
+	rpm.rpm_estimate = frequency;
 	rpm.estimated_accurancy_rpm = frequency / 100.0f;

 	// Publish data and let the user know what was published
@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (c) 2021 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.
+#
+############################################################################
+
+set(PARAM_PREFIX PWM_MAIN)
+
+if(CONFIG_BOARD_IO)
+	set(PARAM_PREFIX PWM_AUX)
+endif()
+
+px4_add_module(
+	MODULE drivers__rpm_capture
+	MAIN rpm_capture
+	COMPILE_FLAGS
+		-DPARAM_PREFIX="${PARAM_PREFIX}"
+	SRCS
+		RPMCapture.cpp
+	)
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_RPM_CAPTURE
+	bool "rpm_capture"
+	default n
+	---help---
+		Enable support for rpm_capture
@@ -0,0 +1,220 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+#include "RPMCapture.hpp"
+#include <px4_arch/io_timer.h>
+#include <board_config.h>
+#include <parameters/param.h>
+#include <px4_platform_common/events.h>
+#include <systemlib/mavlink_log.h>
+
+RPMCapture::RPMCapture() :
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
+	ModuleParams(nullptr)
+{
+	_pwm_input_pub.advertise();
+	ScheduleNow();
+}
+
+RPMCapture::~RPMCapture()
+{
+	if (_channel >= 0) {
+		io_timer_unallocate_channel(_channel);
+		px4_arch_gpiosetevent(_rpm_capture_gpio, false, false, false, nullptr, nullptr);
+	}
+}
+
+bool RPMCapture::init()
+{
+	bool success = false;
+
+	for (unsigned i = 0; i < 16; ++i) {
+		char param_name[17];
+		snprintf(param_name, sizeof(param_name), "%s_%s%d", PARAM_PREFIX, "FUNC", i + 1);
+		param_t function_handle = param_find(param_name);
+		int32_t function;
+
+		if (function_handle != PARAM_INVALID && param_get(function_handle, &function) == 0) {
+			if (function == 2070) { // RPM_Input
+				_channel = i;
+			}
+		}
+	}
+
+	if (_channel == -1) {
+		PX4_WARN("No RPM channel configured");
+		return false;
+	}
+
+	int ret = io_timer_allocate_channel(_channel, IOTimerChanMode_Other); // TODO: add IOTimerChanMode_RPM
+
+	if (ret != PX4_OK) {
+		PX4_ERR("gpio alloc failed (%i) for RPM at channel (%d)", ret, _channel);
+		return false;
+	}
+
+	_rpm_capture_gpio = PX4_MAKE_GPIO_EXTI(io_timer_channel_get_as_pwm_input(_channel));
+	int ret_val = px4_arch_gpiosetevent(_rpm_capture_gpio, true, false, true, &RPMCapture::gpio_interrupt_callback, this);
+
+	if (ret_val == PX4_OK) {
+		success = true;
+	}
+
+	success = success && _rpm_pub.advertise();
+	return success;
+}
+
+void RPMCapture::Run()
+{
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
+	}
+
+	hrt_abstime now = hrt_absolute_time();
+
+	if (_interrupt_happened.load()) {
+		// There was an interrupt
+		_period = _hrt_timestamp - _hrt_timestamp_prev;
+		_hrt_timestamp_prev = _hrt_timestamp;
+		_interrupt_happened.store(false);
+
+		pwm_input_s pwm_input{};
+		pwm_input.timestamp = now;
+		pwm_input.period = _period;
+		pwm_input.error_count = _error_count;
+		_pwm_input_pub.publish(pwm_input);
+
+		ScheduleClear(); // Do not run on previously scheduled timeout
+
+	} else {
+		// Timeout for no interrupts
+		_period = UINT32_MAX;
+	}
+
+	ScheduleDelayed(RPM_PULSE_TIMEOUT); // Schule a new timeout
+	float rpm_raw{0.f};
+
+	if (_period < RPM_PULSE_TIMEOUT) {
+		// 1'000'000 / [us] -> pulses per second * 60 -> pulses per minute
+		rpm_raw = 60.f * 1e6f / static_cast<float>(_param_rpm_puls_per_rev.get() * _period);
+
+	} else {
+		_rpm_filter.reset(rpm_raw);
+	}
+
+	if (rpm_raw < RPM_MAX_VALUE) {
+		// Don't update RPM filter with outliers
+		const float dt = math::min((now - _timestamp_last_update) * 1e-6f, 1.f);
+		_timestamp_last_update = now;
+		_rpm_filter.setParameters(dt, 0.5f);
+		_rpm_filter.update(_rpm_median_filter.apply(rpm_raw));
+	}
+
+	rpm_s rpm{};
+	rpm.timestamp = now;
+	rpm.rpm_raw = rpm_raw;
+	rpm.rpm_estimate = _rpm_filter.getState();
+	_rpm_pub.publish(rpm);
+}
+
+int RPMCapture::gpio_interrupt_callback(int irq, void *context, void *arg)
+{
+	RPMCapture *instance = static_cast<RPMCapture *>(arg);
+
+	if (instance->_interrupt_happened.load()) {
+		++instance->_error_count;
+	}
+
+	instance->_hrt_timestamp = hrt_absolute_time();
+	instance->_interrupt_happened.store(true);
+	instance->ScheduleNow();
+
+	return PX4_OK;
+}
+
+int RPMCapture::task_spawn(int argc, char *argv[])
+{
+	RPMCapture *instance = new RPMCapture();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int RPMCapture::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int RPMCapture::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_USAGE_NAME("rpm_capture", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+void RPMCapture::stop()
+{
+	exit_and_cleanup();
+}
+
+extern "C" __EXPORT int rpm_capture_main(int argc, char *argv[])
+{
+	if (argc >= 2 && !strcmp(argv[1], "stop") && RPMCapture::is_running()) {
+		RPMCapture::stop();
+	}
+
+	return RPMCapture::main(argc, argv);
+}
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <lib/mathlib/math/filter/MedianFilter.hpp>
+#include <px4_arch/micro_hal.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/pwm_input.h>
+#include <uORB/topics/rpm.h>
+
+using namespace time_literals;
+
+class RPMCapture : public ModuleBase<RPMCapture>, public px4::ScheduledWorkItem, public ModuleParams
+{
+public:
+	RPMCapture();
+	virtual ~RPMCapture();
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+	static int gpio_interrupt_callback(int irq, void *context, void *arg);
+
+	/** RPMCapture is an interrupt-driven task and needs to be manually stopped */
+	static void stop();
+
+private:
+	static constexpr hrt_abstime RPM_PULSE_TIMEOUT = 1_s;
+	static constexpr float RPM_MAX_VALUE = 50e3f;
+
+	void Run() override;
+
+	int _channel{-1};
+	uint32_t _rpm_capture_gpio{0};
+	uORB::Publication<pwm_input_s> _pwm_input_pub{ORB_ID(pwm_input)};
+	uORB::PublicationMulti<rpm_s> _rpm_pub{ORB_ID(rpm)};
+
+	hrt_abstime _hrt_timestamp{0};
+	hrt_abstime _hrt_timestamp_prev{0};
+	uint32_t _period{UINT32_MAX};
+	uint32_t _error_count{0};
+	px4::atomic<bool> _interrupt_happened{false};
+
+	hrt_abstime _timestamp_last_update{0}; ///< to caluclate dt
+	AlphaFilter<float> _rpm_filter;
+	MedianFilter<float, 5> _rpm_median_filter;
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::RPM_PULS_PER_REV>) _param_rpm_puls_per_rev
+	)
+};
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021 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.
+ *
+ ****************************************************************************/
+
+/**
+ * RPM Capture Enable
+ *
+ * Enables the RPM capture module on FMU channel 5.
+ *
+ * @boolean
+ * @group System
+ * @reboot_required true
+ */
+PARAM_DEFINE_INT32(RPM_CAP_ENABLE, 0);
+
+/**
+ * Voltage pulses per revolution
+ *
+ * Number of voltage pulses per one rotor revolution on the capturing pin.
+ *
+ * @group System
+ * @min 1
+ * @max 50
+ * @reboot_required true
+ */
+PARAM_DEFINE_INT32(RPM_PULS_PER_REV, 1);
@@ -54,6 +54,7 @@ add_subdirectory(geo EXCLUDE_FROM_ALL)
 add_subdirectory(heatshrink EXCLUDE_FROM_ALL)
 add_subdirectory(hysteresis EXCLUDE_FROM_ALL)
 add_subdirectory(l1 EXCLUDE_FROM_ALL)
+add_subdirectory(lat_lon_alt EXCLUDE_FROM_ALL)
 add_subdirectory(led EXCLUDE_FROM_ALL)
 add_subdirectory(matrix EXCLUDE_FROM_ALL)
 add_subdirectory(mathlib EXCLUDE_FROM_ALL)
@@ -34,8 +34,52 @@
 #include "lat_lon_alt.hpp"

 using matrix::Vector3f;
+using matrix::Vector3d;
 using matrix::Vector2d;

+LatLonAlt LatLonAlt::fromEcef(const Vector3d &p_ecef)
+{
+	// Convert position using Borkowski closed-form exact solution
+	// P. D. Groves, "Principles of GNSS, inertial, and multisensor integrated navigation systems, 2nd edition (appendix C)
+	const double k1 = sqrt(1 - Wgs84::eccentricity2) * std::abs(p_ecef(2));
+	const double k2 = Wgs84::eccentricity2 * Wgs84::equatorial_radius;
+	const double beta = sqrt(p_ecef(0) * p_ecef(0) + p_ecef(1) * p_ecef(1));
+	const double E = (k1 - k2) / beta;
+	const double F = (k1 + k2) / beta;
+
+	const double P = 4.0 / 3.0 * (E * F + 1);
+	const double Q = 2 * (E * E - F * F);
+	const double D = P * P * P + Q * Q;
+	const double V = pow(sqrt(D) - Q, 1.0 / 3.0) - pow(sqrt(D) + Q, 1.0 / 3.0);
+	const double G = 0.5 * (sqrt(E * E + V) + E);
+	const double T = sqrt(G * G + (F - V * G) / (2 * G - E)) - G;
+
+	const double lon = atan2(p_ecef(1), p_ecef(0));
+	const double lat = matrix::sign(p_ecef(2)) * atan((1.0 - T * T) / (2.0 * T * sqrt(1.0 - Wgs84::eccentricity2)));
+	const double alt = (beta - Wgs84::equatorial_radius * T) * cos(lat) +
+			   (p_ecef(2) - matrix::sign(p_ecef(2)) * Wgs84::equatorial_radius * sqrt(1.0 - Wgs84::eccentricity2)) * sin(lat);
+
+	LatLonAlt lla;
+	lla.setLatLonRad(lat, lon);
+	lla.setAltitude(static_cast<float>(alt));
+	return lla;
+}
+
+Vector3d LatLonAlt::toEcef() const
+{
+	const double cos_lat = cos(_latitude_rad);
+	const double sin_lat = sin(_latitude_rad);
+	const double cos_lon = cos(_longitude_rad);
+	const double sin_lon = sin(_longitude_rad);
+
+	const double r_e = Wgs84::equatorial_radius / sqrt(1.0 - std::pow(Wgs84::eccentricity * sin_lat, 2.0));
+	const double r_total = r_e + static_cast<double>(_altitude);
+
+	return Vector3d(r_total * cos_lat * cos_lon,
+			r_total * cos_lat * sin_lon,
+			((1.0 - Wgs84::eccentricity2) * r_e + static_cast<double>(_altitude)) * sin_lat);
+}
+
 Vector3f LatLonAlt::computeAngularRateNavFrame(const Vector3f &v_ned) const
 {
 	double r_n;
@@ -54,6 +54,10 @@ public:
 		_altitude = altitude_m;
 	}

+
+	static LatLonAlt fromEcef(const matrix::Vector3d &p_ecef);
+	matrix::Vector3d toEcef() const;
+
 	void setZero() { _latitude_rad = 0.0; _longitude_rad = 0.0; _altitude = 0.f; }

 	double latitude_deg() const { return math::degrees(latitude_rad()); }
@@ -94,6 +98,14 @@ public:
 	 */
 	matrix::Vector3f computeAngularRateNavFrame(const matrix::Vector3f &v_ned) const;

+	struct Wgs84 {
+		static constexpr double equatorial_radius = 6378137.0;
+		static constexpr double eccentricity = 0.0818191908425;
+		static constexpr double eccentricity2 = eccentricity * eccentricity;
+		static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity2);
+		static constexpr double gravity_equator = 9.7803253359;
+	};
+
 private:
 	// Convert between curvilinear and cartesian errors
 	static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude);
@@ -101,12 +113,6 @@ private:
 	static void computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature,
 					    double &transverse_radius_of_curvature);

-	struct Wgs84 {
-		static constexpr double equatorial_radius = 6378137.0;
-		static constexpr double eccentricity = 0.0818191908425;
-		static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity *eccentricity);
-	};
-
 	double _latitude_rad{0.0};
 	double _longitude_rad{0.0};
 	float _altitude{0.0};
@@ -105,3 +105,17 @@ TEST(TestLatLonAlt, subLatLonAlt)
 	EXPECT_NEAR(delta_pos(1), delta_pos_true(1), 1e-2);
 	EXPECT_EQ(delta_pos(2), delta_pos_true(2));
 }
+
+TEST(TestLatLonAlt, fromAndToECEF)
+{
+	for (double lat = -M_PI; lat < M_PI; lat += M_PI / 4.0) {
+		for (double lon = -M_PI; lon < M_PI; lon += M_PI / 4.0) {
+			for (float alt = -500.f; alt < 8000.f; alt += 500.f) {
+				LatLonAlt lla(lat, lon, alt);
+
+				LatLonAlt res = LatLonAlt::fromEcef(lla.toEcef());
+				EXPECT_TRUE(!(lla - res).longerThan(10e-6f)) << "lat: " << lat << ", lon: " << lon << ", alt: " << alt;
+			}
+		}
+	}
+}
@@ -55,6 +55,16 @@ public:
 		}
 	}

+	template<typename S>
+	Matrix(const Matrix<S, M, N> &aa)
+	{
+		for (size_t i = 0; i < M; i++) {
+			for (size_t j = 0; j < N; j++) {
+				_data[i][j] = static_cast<Type>(aa(i, j));
+			}
+		}
+	}
+
 	template<size_t P, size_t Q>
 	Matrix(const Slice<Type, M, N, P, Q> &in_slice)
 	{
@@ -60,3 +60,9 @@ functions:
        condition: "PPS_CAP_ENABLE==0"
        text: "PPS needs to be enabled via PPS_CAP_ENABLE parameter."
      exclude_from_actuator_testing: true
+    RPM_Input:
+      start: 2070
+      note:
+        condition: "RPM_CAP_ENABLE==0"
+        text: "RPM needs to be enabled via RPM_CAP_ENABLE parameter."
+      exclude_from_actuator_testing: true
@@ -106,16 +106,32 @@ float PositionSmoothing::_getMaxXYSpeed(const Vector3f(&waypoints)[3]) const

 float PositionSmoothing::_getMaxZSpeed(const Vector3f(&waypoints)[3]) const
 {
+	const Vector3f &start_position = {_trajectory[0].getCurrentPosition(),
+					  _trajectory[1].getCurrentPosition(),
+					  _trajectory[2].getCurrentPosition()
+					 };
+	const Vector3f &target = waypoints[1];
+	const Vector3f &next_target = waypoints[2];

-	const auto &target = waypoints[1];
+	const Vector2f start_position_xy_z = {start_position.xy().norm(), start_position(2)};
+	const Vector2f target_xy_z = {target.xy().norm(), target(2)};
+	const Vector2f next_target_xy_z = {next_target.xy().norm(), next_target(2)};

-	Vector3f pos_traj(_trajectory[0].getCurrentPosition(),
-			  _trajectory[1].getCurrentPosition(),
-			  _trajectory[2].getCurrentPosition());
+	float arrival_z_speed = 0.0f;
+	const bool target_next_different = fabsf(target(2) - next_target(2)) > 0.001f;

-	const float distance_start_target = fabs(target(2) - pos_traj(2));
-	const float arrival_z_speed = 0.f;
+	if (target_next_different) {
+		const float alpha = acosf(Vector2f((target_xy_z - start_position_xy_z)).unit_or_zero().dot(
+						  Vector2f((target_xy_z - next_target_xy_z)).unit_or_zero()));

+		const float safe_alpha = math::constrain(alpha, 0.f, M_PI_F - FLT_EPSILON);
+		float accel_tmp = _trajectory[2].getMaxAccel();
+		float max_speed_in_turn = math::trajectory::computeMaxSpeedInWaypoint(safe_alpha, accel_tmp,
+					  _vertical_acceptance_radius);
+		arrival_z_speed = math::min(max_speed_in_turn, _trajectory[2].getMaxVel());
+	}
+
+	const float distance_start_target = fabs(target(2) - start_position(2));
 	float max_speed = math::min(_trajectory[2].getMaxVel(), math::trajectory::computeMaxSpeedFromDistance(
 					    _trajectory[2].getMaxJerk(), _trajectory[2].getMaxAccel(),
 					    distance_start_target, arrival_z_speed));
@@ -151,14 +151,17 @@ TEST_F(PositionSmoothingTest, reachesTargetVelocityIntegration)

 TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity)
 {
-	const int N_ITER = 2000;
+	const int N_ITER = 20000;
 	const float DELTA_T = 0.02f;
 	const Vector3f INITIAL_POSITION{0.f, 0.f, 0.f};
 	const Vector3f TARGET{12.f, 17.f, 8.f};
 	const Vector3f NEXT_TARGET{8.f, 12.f, 80.f};

+	const float XY_ACC_RAD = 10.f;
+	const float Z_ACC_RAD = 0.8f;

-	Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, NEXT_TARGET};
+
+	Vector3f waypoints[3] = {INITIAL_POSITION, TARGET, TARGET};
 	Vector3f ff_velocity{1.f, 0.1f, 0.3f};

 	Vector3f position{0.f, 0.f, 0.f};
@@ -180,12 +183,13 @@ TEST_F(PositionSmoothingTest, reachesTargetInitialVelocity)
 		ff_velocity = {0.f, 0.f, 0.f};
 		expectDynamicsLimitsRespected(out);

-		if (position == TARGET) {
+		if (Vector2f(position.xy() - TARGET.xy()).norm() < XY_ACC_RAD && fabsf(position(2) - TARGET(2)) < Z_ACC_RAD) {
 			printf("Converged in %d iterations\n", iteration);
 			break;
 		}
 	}

-	EXPECT_EQ(TARGET, position);
+	EXPECT_LT(Vector2f(position.xy() - TARGET.xy()).norm(), XY_ACC_RAD);
+	EXPECT_LT(fabsf(position(2) - TARGET(2)), Z_ACC_RAD);
 	EXPECT_LT(iteration, N_ITER) << "Took too long to converge\n";
 }
@@ -74,15 +74,11 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co

 	const bool target_next_different = distance_target_next  > 0.001f;
 	const bool waypoint_overlap = distance_target_next < config.xy_accept_rad;
-	const bool has_reached_altitude = fabsf(target(2) - start_position(2)) < config.z_accept_rad;
-	const bool altitude_stays_same = fabsf(next_target(2) - target(2)) < config.z_accept_rad;

 	float speed_at_target = 0.0f;

 	if (target_next_different &&
-	    !waypoint_overlap &&
-	    has_reached_altitude &&
-	    altitude_stays_same
+	    !waypoint_overlap
 	   ) {
 		const float alpha = acosf(Vector2f((target - start_position).xy()).unit_or_zero().dot(
 						  Vector2f((target - next_target).xy()).unit_or_zero()));
@@ -108,15 +104,15 @@ inline float computeStartXYSpeedFromWaypoints(const Vector3f &start_position, co
 *
 * @return the maximum speed at waypoint[0] which allows it to follow the trajectory while respecting the dynamic limits
 */
-template <size_t N>
+template <int N>
 float computeXYSpeedFromWaypoints(const Vector3f waypoints[N], const VehicleDynamicLimits &config)
 {
 	static_assert(N >= 2, "Need at least 2 points to compute speed");

 	float max_speed = 0.f;

-	for (size_t j = 0; j < N - 1; j++) {
-		size_t i = N - 2 - j;
+	// go backwards through the waypoints
+	for (int i = (N - 2); i >= 0; i--) {
 		max_speed = computeStartXYSpeedFromWaypoints(waypoints[i],
 				waypoints[i + 1],
 				waypoints[min(i + 2, N - 1)],
@@ -1126,19 +1126,8 @@ int parameter_flashfs_init(sector_descriptor_t *fconfig, uint8_t *buffer, uint16
 	/*  No paramaters */

 	if (pf == NULL) {
-		size_t total_size = size + sizeof(flash_entry_header_t);
-		size_t alignment = 31;//32-byte flash line - 1
-		size_t  size_adjust = ((total_size + alignment) & ~alignment) - total_size;
-		total_size += size_adjust;
-
-		/* Do we have free space ?*/
-
-		if (find_free(total_size) == NULL) {
-
-			/* No parameters and no free space => need erase */
-
-			rv  = parameter_flashfs_erase();
-		}
+		// Parameters can't be found, assume sector is corrupt or empty
+		rv = parameter_flashfs_erase();
 	}

 	return rv;
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2018-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
@@ -31,4 +31,10 @@
 #
 ############################################################################

-px4_add_library(pid pid.cpp)
+px4_add_library(PID
+	PID.cpp
+	PID.hpp
+)
+target_include_directories(PID PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC PIDTest.cpp LINKLIBS PID)
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "PID.hpp"
+#include "lib/mathlib/math/Functions.hpp"
+
+void PID::setGains(const float P, const float I, const float D)
+{
+	_gain_proportional = P;
+	_gain_integral = I;
+	_gain_derivative = D;
+}
+
+float PID::update(const float feedback, const float dt, const bool update_integral)
+{
+	const float error = _setpoint - feedback;
+	const float output = (_gain_proportional * error) + _integral + (_gain_derivative * updateDerivative(feedback, dt));
+
+	if (update_integral) {
+		updateIntegral(error, dt);
+	}
+
+	_last_feedback = feedback;
+	return math::constrain(output, -_limit_output, _limit_output);
+}
+
+void PID::updateIntegral(float error, const float dt)
+{
+	const float integral_new = _integral + _gain_integral * error * dt;
+
+	if (std::isfinite(integral_new)) {
+		_integral = math::constrain(integral_new, -_limit_integral, _limit_integral);
+	}
+}
+
+float PID::updateDerivative(float feedback, const float dt)
+{
+	float feedback_change = 0.f;
+
+	if ((dt > FLT_EPSILON) && std::isfinite(_last_feedback)) {
+		feedback_change = (feedback - _last_feedback) / dt;
+	}
+
+	return feedback_change;
+}
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <cmath>
+
+class PID
+{
+public:
+	PID() = default;
+	virtual ~PID() = default;
+	void setOutputLimit(const float limit) { _limit_output = limit; }
+	void setIntegralLimit(const float limit) { _limit_integral = limit; }
+	void setGains(const float P, const float I, const float D);
+	void setSetpoint(const float setpoint) { _setpoint = setpoint; }
+	float update(const float feedback, const float dt, const bool update_integral = true);
+	float getIntegral() { return _integral; }
+	void resetIntegral() { _integral = 0.f; };
+	void resetDerivative() { _last_feedback = NAN; };
+private:
+	void updateIntegral(float error, const float dt);
+	float updateDerivative(float feedback, const float dt);
+
+	float _setpoint{0.f}; ///< current setpoint to track
+	float _integral{0.f}; ///< integral state
+	float _last_feedback{NAN};
+
+	// Gains, Limits
+	float _gain_proportional{0.f};
+	float _gain_integral{0.f};
+	float _gain_derivative{0.f};
+	float _limit_integral{0.f};
+	float _limit_output{0.f};
+};
@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include <PID.hpp>
+
+TEST(PIDTest, AllZeroCase)
+{
+	PID pid;
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
+}
+
+TEST(PIDTest, OutputLimit)
+{
+	PID pid;
+	pid.setOutputLimit(.01f);
+	pid.setGains(.1f, 0.f, 0.f);
+	pid.setSetpoint(1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .01f);
+	EXPECT_FLOAT_EQ(pid.update(.9f, 0.f, false), .01f);
+	EXPECT_NEAR(pid.update(.95f, 0.f, false), .005f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
+	EXPECT_NEAR(pid.update(1.05f, 0.f, false), -.005f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(1.1f, 0.f, false), -.01f);
+	EXPECT_FLOAT_EQ(pid.update(1.15f, 0.f, false), -.01f);
+	EXPECT_FLOAT_EQ(pid.update(2.f, 0.f, false), -.01f);
+}
+
+TEST(PIDTest, ProportinalOnly)
+{
+	PID pid;
+	pid.setOutputLimit(1.f);
+	pid.setGains(.1f, 0.f, 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), 0.f);
+	pid.setSetpoint(1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.f, false), .1f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, false), 0.f);
+
+	float plant = 0.f;
+	float output = 10000.f;
+	int i; // need function scope to check how many steps
+
+	for (i = 1000; i > 0; i--) {
+		const float output_new = pid.update(plant, 0.f, false);
+		plant += output_new;
+
+		// expect the output to get smaller with each iteration
+		if (output_new >= output) {
+			break;
+		}
+
+		output = output_new;
+	}
+
+	EXPECT_FLOAT_EQ(plant, 1.f);
+	EXPECT_GT(i, 0); // it shouldn't have taken longer than an iteration timeout to converge
+}
+
+TEST(PIDTest, InteralOpenLoop)
+{
+	PID pid;
+	pid.setOutputLimit(1.f);
+	pid.setGains(0.f, .1f, 0.f);
+	pid.setIntegralLimit(.05f);
+	pid.setSetpoint(1.f);
+
+	// Zero error
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(1.f, 0.f, true), 0.f);
+
+	// Open loop ramp up
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .01f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+
+	// Open loop ramp down
+	pid.setSetpoint(-1.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .03f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), .02f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), .01f, 1e-6f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), 0.f, 1e-6f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.01f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.02f);
+	EXPECT_NEAR(pid.update(0.f, 0.1f, true), -.03f, 1e-6f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.04f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.05f);
+	pid.resetIntegral();
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), 0.f);
+	EXPECT_FLOAT_EQ(pid.update(0.f, 0.1f, true), -.01f);
+}
@@ -1,185 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * 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.
- *
- ****************************************************************************/
-
-/**
- * @file pid.cpp
- *
- * Implementation of generic PID controller.
- *
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#include "pid.h"
-#include <math.h>
-#include <px4_platform_common/defines.h>
-
-#define SIGMA 0.000001f
-
-__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
-{
-	pid->mode = mode;
-	pid->dt_min = dt_min;
-	pid->kp = 0.0f;
-	pid->ki = 0.0f;
-	pid->kd = 0.0f;
-	pid->integral = 0.0f;
-	pid->integral_limit = 0.0f;
-	pid->output_limit = 0.0f;
-	pid->error_previous = 0.0f;
-	pid->last_output = 0.0f;
-}
-
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit)
-{
-	int ret = 0;
-
-	if (PX4_ISFINITE(kp)) {
-		pid->kp = kp;
-
-	} else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(ki)) {
-		pid->ki = ki;
-
-	} else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(kd)) {
-		pid->kd = kd;
-
-	} else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(integral_limit)) {
-		pid->integral_limit = integral_limit;
-
-	}  else {
-		ret = 1;
-	}
-
-	if (PX4_ISFINITE(output_limit)) {
-		pid->output_limit = output_limit;
-
-	}  else {
-		ret = 1;
-	}
-
-	return ret;
-}
-
-__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt)
-{
-	if (!PX4_ISFINITE(sp) || !PX4_ISFINITE(val) || !PX4_ISFINITE(val_dot) || !PX4_ISFINITE(dt)) {
-		return pid->last_output;
-	}
-
-	float i, d;
-
-	/* current error value */
-	float error = sp - val;
-
-	/* current error derivative */
-	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
-		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
-		pid->error_previous = error;
-
-	} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
-		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
-		pid->error_previous = -val;
-
-	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
-		d = -val_dot;
-
-	} else {
-		d = 0.0f;
-	}
-
-	if (!PX4_ISFINITE(d)) {
-		d = 0.0f;
-	}
-
-	/* calculate PD output */
-	float output = (error * pid->kp) + (d * pid->kd);
-
-	if (pid->ki > SIGMA) {
-		// Calculate the error integral and check for saturation
-		i = pid->integral + (error * dt);
-
-		/* check for saturation */
-		if (PX4_ISFINITE(i)) {
-			if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) &&
-			    fabsf(i) <= pid->integral_limit) {
-				/* not saturated, use new integral value */
-				pid->integral = i;
-			}
-		}
-
-		/* add I component to output */
-		output += pid->integral * pid->ki;
-	}
-
-	/* limit output */
-	if (PX4_ISFINITE(output)) {
-		if (pid->output_limit > SIGMA) {
-			if (output > pid->output_limit) {
-				output = pid->output_limit;
-
-			} else if (output < -pid->output_limit) {
-				output = -pid->output_limit;
-			}
-		}
-
-		pid->last_output = output;
-	}
-
-	return pid->last_output;
-}
-
-
-__EXPORT void pid_reset_integral(PID_t *pid)
-{
-	pid->integral = 0.0f;
-}
@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
- *   Author: Laurens Mackay <mackayl@student.ethz.ch>
- *           Tobias Naegeli <naegelit@student.ethz.ch>
- *           Martin Rutschmann <rutmarti@student.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
- *           Julian Oes <joes@student.ethz.ch>
- *
- * 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.
- *
- ****************************************************************************/
-
-/**
- * @file pid.h
- *
- * Definition of generic PID controller.
- *
- * @author Laurens Mackay <mackayl@student.ethz.ch>
- * @author Tobias Naegeli <naegelit@student.ethz.ch>
- * @author Martin Rutschmann <rutmarti@student.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#ifndef PID_H_
-#define PID_H_
-
-#include <stdint.h>
-
-__BEGIN_DECLS
-
-typedef enum PID_MODE {
-	/* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */
-	PID_MODE_DERIVATIV_NONE = 0,
-	/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error,
-	 * val_dot in pid_calculate() will be ignored */
-	PID_MODE_DERIVATIV_CALC,
-	/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value,
-	 * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */
-	PID_MODE_DERIVATIV_CALC_NO_SP,
-	/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
-	PID_MODE_DERIVATIV_SET
-} pid_mode_t;
-
-typedef struct {
-	pid_mode_t mode;
-	float dt_min;
-	float kp;
-	float ki;
-	float kd;
-	float integral;
-	float integral_limit;
-	float output_limit;
-	float error_previous;
-	float last_output;
-} PID_t;
-
-__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min);
-__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
-__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
-__EXPORT void pid_reset_integral(PID_t *pid);
-
-__END_DECLS
-
-#endif /* PID_H_ */
@@ -77,7 +77,7 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)

 	esc_status_s esc_status;

-	if (_esc_status_sub.copy(&esc_status) && now - esc_status.timestamp < esc_telemetry_timeout) {
+	if (_esc_status_sub.copy(&esc_status) && (now < esc_status.timestamp + esc_telemetry_timeout)) {

 		checkEscStatus(context, reporter, esc_status);
 		reporter.setIsPresent(health_component_t::motors_escs);
@@ -813,7 +813,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f


 	// altitude
-	failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + (_param_com_pos_fs_delay.get() * 1_s));
+	failsafe_flags.local_altitude_invalid = !lpos.z_valid || (now > lpos.timestamp + 1_s);


 	// attitude
@@ -864,7 +864,7 @@ bool EstimatorChecks::checkPosVelValidity(const hrt_abstime &now, const bool dat
 		const bool was_valid) const
 {
 	bool valid = was_valid;
-	const bool data_stale = (now > data_timestamp_us + _param_com_pos_fs_delay.get() * 1_s) || (data_timestamp_us == 0);
+	const bool data_stale = (now > data_timestamp_us + 1_s) || (data_timestamp_us == 0);
 	const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy);
 	const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy);

@@ -118,7 +118,6 @@ private:
 					(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
 					(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
 					(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
-					(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
 					(ParamFloat<px4::params::COM_POS_LOW_EPH>) _param_com_low_eph,
 					(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act
 				       )
@@ -512,19 +512,6 @@ PARAM_DEFINE_INT32(COM_ARM_AUTH_MET, 0);
 */
 PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1);

-/**
- * Loss of position failsafe activation delay.
- *
- * This sets number of seconds that the position checks need to be failed before the failsafe will activate.
- * The default value has been optimised for rotary wing applications. For fixed wing applications, a larger value between 5 and 10 should be used.
- *
- * @unit s
- * @group Commander
- * @min 1
- * @max 100
- */
-PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1);
-
 /**
 * Horizontal position error threshold.
 *
@@ -134,9 +134,12 @@ void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float,
 {
 	_saturation_flags = {};

+	const float spoolup_progress = throttleSpoolupProgress();
+	_rpm_control.setSpoolupProgress(spoolup_progress);
+
 	// throttle/collective pitch curve
-	const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
-			       _geometry.throttle_curve) * throttleSpoolupProgress();
+	const float throttle = (math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
+				_geometry.throttle_curve) + _rpm_control.getActuatorCorrection()) * spoolup_progress;
 	const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve);

 	// actuator mapping
@@ -41,6 +41,8 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/manual_control_switches.h>

+#include "RpmControl.hpp"
+
 class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness
 {
 public:
@@ -131,4 +133,6 @@ private:
 	bool _main_motor_engaged{true};

 	const ActuatorType _tail_actuator_type;
+
+	RpmControl _rpm_control{this};
 };
@@ -62,6 +62,7 @@ px4_add_library(ActuatorEffectiveness
 	ActuatorEffectivenessTailsitterVTOL.hpp
 	ActuatorEffectivenessRoverAckermann.hpp
 	ActuatorEffectivenessRoverAckermann.cpp
+	RpmControl.hpp
 )

 target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -69,6 +70,7 @@ target_include_directories(ActuatorEffectiveness PUBLIC ${CMAKE_CURRENT_SOURCE_D
 target_link_libraries(ActuatorEffectiveness
 	PRIVATE
 		mathlib
+		PID
 )

 px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness)
@@ -0,0 +1,132 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file RpmControl.hpp
+ *
+ * Control rpm of a helicopter rotor.
+ * Input: PWM input pulse period from an rpm sensor
+ * Output: Duty cycle command for the ESC
+ *
+ * @author Matthias Grob <maetugr@gmail.com>
+ */
+
+#pragma once
+
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <lib/pid/PID.hpp>
+
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/pwm_input.h>
+#include <uORB/topics/rpm.h>
+#include <uORB/topics/rpm_control_status.h>
+
+using namespace time_literals;
+
+class RpmControl : public ModuleParams
+{
+public:
+	RpmControl(ModuleParams *parent) :  ModuleParams(parent) {};
+	~RpmControl() = default;
+
+	void setSpoolupProgress(float spoolup_progress)
+	{
+		_spoolup_progress = spoolup_progress;
+		_pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get());
+
+		if (_spoolup_progress < .8f) {
+			_pid.resetIntegral();
+		}
+	}
+
+	float getActuatorCorrection()
+	{
+		if (_rpm_sub.updated()) {
+			rpm_s rpm_input{};
+			if (_rpm_sub.copy(&rpm_input)) {
+				_rpm_estimate = rpm_input.rpm_estimate;
+				_rpm_raw = rpm_input.rpm_raw;
+				_timestamp_last_rpm_measurement = rpm_input.timestamp;
+			}
+		}
+
+		hrt_abstime current_time = hrt_absolute_time();
+		const float dt = math::constrain((current_time - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f);
+		_timestamp_last_update = current_time;
+
+		const bool rpm_measurement_timeout = (current_time - _timestamp_last_rpm_measurement) < 1_s;
+		const bool no_excessive_rpm = _rpm_estimate < RPM_MAX_VALUE;
+
+		if (rpm_measurement_timeout && no_excessive_rpm) {
+			const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f;
+			_pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f);
+
+		} else {
+			_pid.setGains(0.f, 0.f, 0.f);
+		}
+
+		_pid.setOutputLimit(.5f);
+		_pid.setIntegralLimit(.5f);
+
+		float output = _pid.update(_rpm_estimate, dt, true);
+
+		rpm_control_status_s rpm_control_status{};
+		rpm_control_status.rpm_raw = _rpm_raw;
+		rpm_control_status.rpm_estimate = _rpm_estimate;;
+		rpm_control_status.rpm_setpoint = _param_ca_heli_rpm_sp.get();
+		rpm_control_status.output = output;
+		rpm_control_status.timestamp = hrt_absolute_time();
+		_rpm_control_status_pub.publish(rpm_control_status);
+
+		return output;
+	}
+
+private:
+	static constexpr float RPM_MAX_VALUE = 1800.f;
+	uORB::Subscription _rpm_sub{ORB_ID(rpm)};
+	uORB::Publication<rpm_control_status_s> _rpm_control_status_pub{ORB_ID(rpm_control_status)};
+
+	float _rpm_estimate{0.f};
+	float _rpm_raw{0.f};
+	float _spoolup_progress{0.f};
+	PID _pid;
+	hrt_abstime _timestamp_last_rpm_measurement{0};
+	hrt_abstime _timestamp_last_update{0};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::CA_HELI_RPM_SP>) _param_ca_heli_rpm_sp,
+		(ParamFloat<px4::params::CA_HELI_RPM_P>) _param_ca_heli_rpm_p,
+		(ParamFloat<px4::params::CA_HELI_RPM_I>) _param_ca_heli_rpm_i
+	)
+};
@@ -314,7 +314,7 @@ ControlAllocator::Run()
 #endif

 	// Check if parameters have changed
-	if (_parameter_update_sub.updated() && !_armed) {
+	if (_parameter_update_sub.updated()) {
 		// clear update
 		parameter_update_s param_update;
 		_parameter_update_sub.copy(&param_update);
@@ -528,6 +528,41 @@ parameters:
              which is mostly the case when the main rotor turns counter-clockwise.
          type: boolean
          default: 0
+        CA_HELI_RPM_SP:
+          description:
+            short: Setpoint for main rotor rpm
+            long: |
+              Requires rpm feedback for the controller.
+          type: float
+          decimal: 0
+          increment: 1
+          min: 100
+          max: 10000
+          default: 1500
+        CA_HELI_RPM_P:
+          description:
+            short: Proportional gain for rpm control
+            long: |
+              Ratio between rpm error devided by 1000 to how much normalized output gets added to correct for it.
+
+              motor_command = throttle_curve + CA_HELI_RPM_P * (rpm_setpoint - rpm_measurement) / 1000
+          type: float
+          decimal: 3
+          increment: 0.1
+          min: 0
+          max: 10
+          default: 0.0
+        CA_HELI_RPM_I:
+          description:
+            short: Integral gain for rpm control
+            long: |
+              Same definition as the proportional gain but for integral.
+          type: float
+          decimal: 3
+          increment: 0.1
+          min: 0
+          max: 10
+          default: 0.0

        # Others
        CA_FAILURE_MODE:
@@ -33,7 +33,6 @@

 add_subdirectory(bias_estimator)
 add_subdirectory(output_predictor)
-add_subdirectory(lat_lon_alt)

 set(EKF_LIBS)
 set(EKF_SRCS)
@@ -110,8 +110,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed

 				} else {
 					// Try to initialize using measurement
-					if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph,
-								      sample.epv)) {
+					if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var,
+								      sq(sample.epv))) {
 						ekf.enableControlStatusAuxGpos();
 						_reset_counters.lat_lon = sample.lat_lon_reset_counter;
 						_state = State::active;
@@ -66,8 +66,6 @@
 # include "aid_sources/aux_global_position/aux_global_position.hpp"
 #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION

-#include "lat_lon_alt/lat_lon_alt.hpp"
-
 enum class Likelihood { LOW, MEDIUM, HIGH };
 class ExternalVisionVel;

@@ -193,9 +191,9 @@ public:
 	void getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const;
 	bool checkLatLonValidity(double latitude, double longitude);
 	bool checkAltitudeValidity(float altitude);
-	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN);
-	bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float eph = NAN,
-				   float epv = NAN);
+	bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float hpos_var = NAN, float vpos_var = NAN);
+	bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float hpos_var = NAN,
+				   float vpos_var = NAN);

 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
 	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const;
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
oravla5	6736ec61a8	RpmControl: class clean up	2024-12-11 18:55:57 +01:00
Matthias Grob	644e6462d7	ControlAllocator: introduce helicopter rotor rpm controller	2024-12-11 16:57:43 +01:00
Matthias Grob	bdec521b0a	RPMCapture: fix dt should not have a minimum which would limit to 6000rpm	2024-12-10 19:53:57 +01:00
Matthias Grob	18de0789ab	RPMCapture: add back filter reset on timeout	2024-12-10 19:52:04 +01:00
Matthias Grob	139cfa64dd	RPMCapture: publish outliers in rpm message but don't update the filters and estimate	2024-12-10 19:50:35 +01:00
Matthias Grob	f155b33db7	RPMCapture: refactor to clarify scheduling	2024-12-10 19:45:03 +01:00
Matthias Grob	e6067800e2	RPMCapture: refactor to clarify when an interrupt happened	2024-12-10 19:44:25 +01:00
Matthias Grob	0943e0e96f	RPMCapture: define period as UINT32_MAX when timed out	2024-12-10 19:44:16 +01:00
Matthias Grob	74dac19fd9	RPMCapture: fix pulses per revolution parameter name, description, type -> use module params	2024-12-10 19:30:25 +01:00
oravla5	dbbdb74e94	RPMCapture: added pulse per rev parameter. Do not publish if pulses are to fast	2024-12-10 19:21:16 +01:00
bresch	3b1c44cd3e	RpmControl: remove raw RPM spikes using median filter	2024-12-10 19:18:14 +01:00
Silvan Fuhrer	8ef8cc0480	RPM capture: publish every 1s without new data Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2024-12-10 19:14:54 +01:00
Matthias Grob	42acadc882	mavlink: stream RAW_RPM message for ONBOAR_LOW_BANDWIDTH	2024-12-10 19:14:54 +01:00
Matthias Grob	37055f6d26	RPMCapture: much better timeout scheduling Not timing out based on a random interval but based on the time after the last inerrupt.	2024-12-10 19:14:54 +01:00
Matthias Grob	71d0013d5e	RPM: clean up message	2024-12-10 19:14:48 +01:00
Matthias Grob	6e90be1129	RPMCapture: switch to PublicationMulti to be compatible with the existing rpm drivers	2024-12-10 16:19:58 +01:00
Matthias Grob	d32aa8dd1d	RPMCapture: Detect rpm sensing timeout	2024-12-10 16:19:58 +01:00
Matthias Grob	643dc1a05e	RPMCapture: add hardcoded rpm processing Such that we can continue development on this part. The implementation was already used before porting things into the RPM capture module.	2024-12-10 16:19:58 +01:00
Beat Küng	f7832d95a7	rpm_capture: add rpm capture driver, publising pwm_input (for now)	2024-12-10 16:19:58 +01:00
Alexander Lerach	bdbf7fd1dd	Also consider targets that include a '-' (#24087 )	2024-12-10 11:05:55 +01:00
Alexander Lerach	f9ecc0fcd1	Added various features to flash analysis (#24072 ) * Added various features to flash analysis (also run with FLASH overflow, summary in comment output, newer bloaty version, only add comment if change is large enough, ...) * Added feedback from review * Use wildcards * Removed backward-compat logic and use correct base	2024-12-10 10:15:49 +01:00
Riccardo Mengoli	09cd42911d	Mission: Replay gimbal cached items before reaching mission waypoint (#24085 ) When flying patterns, photos are sometimes taken while the gimbal is pitching up or down. To address this, we orient the gimbal before reaching the mission waypoint, allowing more time to complete the action. Additionally, we verify if the vehicle is climbing to avoid orienting the gimbal while on the ground.	2024-12-10 09:20:53 +01:00
PX4 BuildBot	430f0888b3	update all px4board kconfig	2024-12-06 22:09:46 -05:00
Jaeyoung Lim	197aed8bdd	Add mavlink stream for plane camera (#24081 )	2024-12-06 14:52:54 +01:00
Jari Nippula	814b243931	Tools/decrypt_ulog.py: support for .ulge log files	2024-12-05 07:25:29 +01:00
Jari Nippula	2da944a834	Logger: combined key & cipher file Use one single .ulge file to store both wrapped symmetric key and encrypted ulog data instead of creating separate .ulgk/ulgc files	2024-12-05 07:25:29 +01:00
Jacob Dahl	ae61b4bfe0	params: flash: erase corrupt sector (#24065 )	2024-12-05 07:24:56 +01:00
Julian Oes	1778692ca2	gps: heading fixes for NMEA/Unicore This updates the GPS submodule which includes NMEA/Unicore fixes: - Add correct return value for sat infos - Only publish on position updates - Request required topics at 5 Hz for Unicore	2024-12-05 08:58:22 +13:00
Ramon Roche	28487350d3	ci: update actions and images Updates some actions to run build steps in container instead of the whole workflow.	2024-12-04 12:59:27 -05:00
Matthias Grob	d416cd2a6c	Commander: remove `COM_POS_FS_DELAY` A user configurable delay for the internal `vehicle_local_position` seems confusing in my eyes. It's a different timeout for fixed-wing and multirotor which might have made sense earlier but not really anymore since the topic is constantly published by the estimator and not expected to time out on either vehicle type and the parameter description is also misleading because it's outdated.	2024-12-03 17:31:56 +01:00
Perre	dfa48f988d	ESC check: Avoid unsigned timestamp underflow in telemtry timeout (#24069 ) * Avoid unsigned integer underflow * ESC check: add brackets to timeout for readability --------- Co-authored-by: Matthias Grob <maetugr@gmail.com>	2024-12-03 17:14:09 +01:00
bresch	8626019ae0	EKF2: reset global position using variance	2024-12-03 13:04:25 +01:00
bresch	6b637f82f8	lla; fix conversion to ECEF	2024-12-03 13:03:54 +01:00
Perre	4696338d29	Add gz model for quadtailsitter (#23943 ) * Add gazebo airspeed plugin and add a tailsitter model --------- Co-authored-by: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com>	2024-12-02 17:27:23 +01:00
Claudio Chies	1a165a4956	Added possibility to modify the start position from external sources, as its done in the _getMaxXYSpeed	2024-12-02 14:52:31 +01:00
Matthias Grob	7dcfeb2f77	PositionSmoothing: refactor _getMaxZSpeed()	2024-12-02 14:52:31 +01:00
Matthias Grob	092e5e8f9d	TrajectoryConstraints: clarify waypoint indexing	2024-12-02 14:52:31 +01:00
Claudio Chies	06dde4ede8	MPC: PositonSmoothing, change test to reflect that we have to come inwithing the acceptance radius, and not exact position.	2024-12-02 14:52:31 +01:00
Claudio Chies	72e758950b	Included turning radius calculations for vertical changes and removed the requirement for the same altitude in the 2D turning radius logic.	2024-12-02 14:52:31 +01:00
Hamish Willee	5ce2bf662b	airframes markdownout.py - br rather than p for generated code	2024-12-02 11:55:25 +01:00
chfriedrich98	be2a3afb83	mecanum: update parameter description	2024-12-02 10:54:43 +01:00
chfriedrich98	ff55313b0b	mecanum: update current position in main file	2024-12-02 10:54:43 +01:00
chfriedrich98	a1b68fcac2	mecanum: update SITL airframe parameters	2024-12-02 10:54:43 +01:00
chfriedrich98	369ce37d65	mecanum: adjust speed setpoints to always be feasible	2024-12-02 10:54:43 +01:00
chfriedrich98	2eda5659eb	mecanum: fix inverse kinematics	2024-12-02 10:54:43 +01:00
chfriedrich98	5dcccd999c	mecanum: add slew rates to yaw, yaw rate and speed setpoints	2024-12-02 10:54:43 +01:00
chfriedrich98	54abc59283	mecanum: deprecate RM_MAN_YAW_SCALE	2024-12-02 10:54:43 +01:00
chfriedrich98	6cce443005	mecanum: deprecate RM_MISS_SPD_DEF	2024-12-02 10:54:43 +01:00
chfriedrich98	7e705bbf55	differential: add slow down effect in mission mode	2024-12-02 10:44:22 +01:00
chfriedrich98	8880569b31	differential: adjust speed setpoint based on yaw rate setpoint	2024-12-02 10:44:22 +01:00
bresch	b06ff99a3e	disable SIH on x21-777 and v6u to save flash	2024-11-29 14:21:29 +01:00
Marco Hauswirth	db0160bf7c	fix sih hitl plane airframe	2024-11-29 14:21:29 +01:00
bresch	8b1975cb98	SIH: lower IMU noise before takeoff This speeds-up the EKF alignment	2024-11-29 14:21:29 +01:00
bresch	b30ea40c6d	SIH: set GNSS delay to 0 as delay is not simulated	2024-11-29 14:21:29 +01:00
bresch	cd18138b1c	SIH: add transport rate acceleration to local acceleration	2024-11-29 14:21:29 +01:00
bresch	674aa474e7	SIH: use LatLonAlt class	2024-11-29 14:21:29 +01:00
bresch	189122d553	lla: add gravity constant at equator	2024-11-29 14:21:29 +01:00
bresch	b6658df169	lla: move to lib directory	2024-11-29 14:21:29 +01:00
bresch	7cf42727fb	lla: add functions to convert from and to ECEF	2024-11-29 14:21:29 +01:00
bresch	7ee69d616d	SIH: rework FW ground contact	2024-11-29 14:21:29 +01:00
bresch	5d33971712	SIH: refactor MC ground contact	2024-11-29 14:21:29 +01:00
bresch	9b172d36a2	matrix: allow casting float<->double	2024-11-29 14:21:29 +01:00
Marco Hauswirth	5d7b734bc9	SIH: ellipsoidal earth model SIH: use projection functions and constants from geo lib SIH: remove unnecessary member variable SIH: clarify names of rotation matrices and frames SIH: do not store DCM corresponding to quaternion attitude Using DCM is more efficient when more than 1 rotation needs to be done, which is not the case here. SIH: don't store local variable as member SIH: use Wgs84 constants everywhere SIH: do not store delta_quaternion Converting an AxisAngle to a Quaternion uses the exponenial SIH: organise ECEF member variables SIH: add earth spin rate to gyro data Co-authored-by: bresch <brescianimathieu@gmail.com>	2024-11-29 14:21:29 +01:00
Matthias Grob	ce3fcd503f	navigator: unify timeout waiting for payload to execute mission item command Used for winch, gripper, gimbal to reach the desired state before continuing the mission. Ideally we'd have feedback from all these components and not just a feed-forward delay.	2024-11-28 20:23:36 +01:00
Matthias Grob	17c24bafbc	mission_block: simplify timeout check	2024-11-28 20:23:36 +01:00
Matthias Grob	ec1cf04bc9	mission_block: fix style, shorten debug message strings	2024-11-28 20:23:36 +01:00
Stefano Colli	8b3c78a0a4	Navigator: add optional delay after gimbal mission items	2024-11-28 20:23:36 +01:00
Sergei Grichine	ab320017cc	boards: emlid_navio - support 64-bit OS on Raspberry Pi 4,5 (#24006 ) adds the `emlid_navio2_arm64` build target - supports 64-bit OS on Raspberry Pi 4,5 (#24006)	2024-11-28 08:44:44 -08:00
Julian Oes	95b5859913	boards: add SPA06 to KakuteH7/H7mini/H7v2 boards	2024-11-28 12:18:44 +13:00
Julian Oes	def6ab5a6b	drivers: add SPA06	2024-11-28 12:18:44 +13:00
Julian Oes	f7b62961cb	drivers: Copy SPL06 to SPA06	2024-11-28 12:18:44 +13:00
Ramon Roche	1b6215fcf3	readme: update ci badge	2024-11-27 17:41:43 -05:00
Daniel Agar	990b067b25	uxrce_dds_client: update cmake requirements to match Micro-XRCE-DDS-Client submodule	2024-11-27 14:09:13 -08:00
Daniel Agar	68cbbaab92	Tools/astyle: check_code_style_all.sh skip pre-commit hook if non-interactive	2024-11-27 13:51:10 -08:00
Ramon Roche	22c1f07f0c	container: use PX4 tags whiel tagging images	2024-11-27 16:42:13 -05:00
Ramon Roche	f2bbb6f407	ci: disable publishing PR images to docker hub Docker hub is rate limiting our API access, as a result tests are failing for no apparent reason. This change will decrease the API calls by at least 80% We have applied for an Open Source account with greater API limits, I will come back to this and update as necessary when and if they grant us access to their program.	2024-11-27 16:42:13 -05:00
bresch	85bc8ef885	SIH-plane: fix actuator mapping	2024-11-27 11:14:56 -05:00
bresch	8a9bac29a2	SIH-FW: allow pitching up during takeoff Otherwise difficult to get lift	2024-11-27 11:14:56 -05:00
bresch	7236ef2d17	SIH-FW: fix aileron and elevator signs This broken when changing from mixer files to the control allocation module.	2024-11-27 11:14:56 -05:00
bresch	1dad25b763	SIH: do not assume being a tailsitter when creating airspeed measurement	2024-11-27 11:14:56 -05:00
Matthias Grob	a280d67be8	PID: Fix test to respect integral updates being applied in the next iteration Co-authored-by: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com>	2024-11-26 16:13:48 +01:00
Matthias Grob	f9bcbc31ae	PID: protect from division by zero because of dt Co-authored-by: chfriedrich98 <125505139+chfriedrich98@users.noreply.github.com>	2024-11-26 16:13:48 +01:00
Matthias Grob	b89c53d28c	Replace old pid library with new one	2024-11-26 16:13:48 +01:00
Matthias Grob	e047972cde	Add new C++ PID library	2024-11-26 16:13:48 +01:00
Daniel Agar	e194a52907	ekf2: derivation.py remove sideslip small angle approximation	2024-11-25 08:53:57 +01:00
				`@@ -0,0 +1 @@`
				`mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530 -p`
				`@@ -0,0 +1 @@`
				`CONFIG_BOARD_LINKER_PREFIX="flash-analysis"`