diff --git a/.clang-tidy b/.clang-tidy
index 2f9fdf2765..81f2d62596 100644
--- a/.clang-tidy
+++ b/.clang-tidy
@@ -1,11 +1,17 @@
 ---
 Checks:          '*,
+                  -*-avoid-c-arrays,
+                  -*-uppercase-literal-suffix,
+                  -*-magic-numbers,
+                  -altera-id-dependent-backward-branch,
+                  -altera-unroll-loops,
                   -android*,
                   -bugprone-integer-division,
                   -cert-dcl50-cpp,
                   -cert-env33-c,
                   -cert-err34-c,
                   -cert-err58-cpp,
+                  -cert-flp30-c,
                   -cert-msc30-c,
                   -cert-msc50-cpp,
                   -clang-analyzer-core.CallAndMessage,
@@ -18,6 +24,7 @@ Checks:          '*,
                   -clang-analyzer-deadcode.DeadStores,
                   -clang-analyzer-optin.cplusplus.VirtualCall,
                   -clang-analyzer-optin.performance.Padding,
+                  -clang-analyzer-security.FloatLoopCounter,
                   -clang-analyzer-security.insecureAPI.strcpy,
                   -clang-analyzer-unix.API,
                   -clang-analyzer-unix.cstring.BadSizeArg,
@@ -37,8 +44,7 @@ Checks:          '*,
                   -cppcoreguidelines-pro-type-union-access,
                   -cppcoreguidelines-pro-type-vararg,
                   -cppcoreguidelines-special-member-functions,
-                  -fuchsia-default-arguments,
-                  -fuchsia-overloaded-operator,
+                  -fuchsia-*,
                   -google-build-using-namespace,
                   -google-explicit-constructor,
                   -google-global-names-in-headers,
@@ -62,6 +68,7 @@ Checks:          '*,
                   -hicpp-use-equals-delete,
                   -hicpp-use-override,
                   -hicpp-vararg,
+                  -llvmlibc-*,
                   -llvm-header-guard,
                   -llvm-include-order,
                   -llvm-namespace-comment,
@@ -84,6 +91,7 @@ Checks:          '*,
                   -modernize-use-override,
                   -modernize-use-trailing-return-type,
                   -modernize-use-using,
+                  -modernize-use-trailing-return-type,
                   -performance-inefficient-string-concatenation,
                   -readability-avoid-const-params-in-decls,
                   -readability-container-size-empty,
diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md
index 7c68bee341..d41196fbf2 100644
--- a/.github/PULL_REQUEST_TEMPLATE.md
+++ b/.github/PULL_REQUEST_TEMPLATE.md
@@ -1,17 +1,14 @@
-Please use [PX4 Discuss](http://discuss.px4.io/) or [Slack](http://slack.px4.io/) to align on pull requests if necessary. You can then open draft pull requests to get early feedback.
+**How this PR solves the problem**
+Depending on how complex the PR is, something between a line and a few paragraphs with logs and videos.
 
-## Describe problem solved by this pull request
-A clear and concise description of the problem this proposed change will solve. Or, what it will improve.
-E.g. For this use case I ran into...
+**JIRA ID in title, and link here**
+Add a link and the title of the JIRA for context
 
-## Describe your solution
-A clear and concise description of what you have implemented.
+**Upstream/develop PR that this was backported from**
+If applicable, link to PR and/or commit
 
-## Describe possible alternatives
-A clear and concise description of alternative solutions or features you've considered.
+**Critical bug that needs backporting?**
+Yes/No, link to backport PR when applicable
 
-## Test data / coverage
-How was it tested? What cases were covered? Logs uploaded to https://review.px4.io/ and screenshots of the important plot parts.
-
-## Additional context
+**Additional context**
 Add any other related context or media.
diff --git a/.github/workflows/cflite_batch.yml b/.github/workflows/cflite_batch.yml
deleted file mode 100644
index d1321cc6b7..0000000000
--- a/.github/workflows/cflite_batch.yml
+++ /dev/null
@@ -1,34 +0,0 @@
-name: ClusterFuzzLite batch fuzzing
-on:
-  schedule:
-    - cron: '0 6 * * *'  # UTC 6am every day.
-permissions: read-all
-jobs:
-  BatchFuzzing:
-    runs-on: ubuntu-latest
-    strategy:
-      fail-fast: false
-      matrix:
-        sanitizer:
-        - address
-        - undefined
-        - memory
-    steps:
-    - name: Build Fuzzers (${{ matrix.sanitizer }})
-      id: build
-      uses: google/clusterfuzzlite/actions/build_fuzzers@v1
-      with:
-        sanitizer: ${{ matrix.sanitizer }}
-    - name: Run Fuzzers (${{ matrix.sanitizer }})
-      id: run
-      uses: google/clusterfuzzlite/actions/run_fuzzers@v1
-      with:
-        github-token: ${{ secrets.GITHUB_TOKEN }}
-        fuzz-seconds: 1800 # 30 mins
-        mode: 'batch'
-        sanitizer: ${{ matrix.sanitizer }}
-        # Optional but recommended: For storing certain artifacts from fuzzing.
-        # See later section on "Git repo for storage".
-        # storage-repo: https://${{ secrets.PERSONAL_ACCESS_TOKEN }}@github.com/OWNER/STORAGE-REPO-NAME.git
-        # storage-repo-branch: main   # Optional. Defaults to "main"
-        # storage-repo-branch-coverage: gh-pages  # Optional. Defaults to "gh-pages".
diff --git a/.github/workflows/checks.yml b/.github/workflows/checks.yml
deleted file mode 100644
index bcae79ad69..0000000000
--- a/.github/workflows/checks.yml
+++ /dev/null
@@ -1,51 +0,0 @@
-name: Checks
-
-on:
-  push:
-    branches:
-    - 'master'
-  pull_request:
-    branches:
-    - '*'
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    strategy:
-      fail-fast: false
-      matrix:
-        check: [
-          "check_format",
-          "tests",
-          "tests_coverage",
-          "px4_fmu-v2_default stack_check",
-          "validate_module_configs",
-          "shellcheck_all",
-          "NO_NINJA_BUILD=1 px4_fmu-v5_default",
-          "NO_NINJA_BUILD=1 px4_sitl_default",
-          "BUILD_MICRORTPS_AGENT=1 px4_sitl_rtps",
-          "airframe_metadata",
-          "module_documentation",
-          "parameters_metadata",
-        ]
-    container:
-      image: px4io/px4-dev-nuttx-focal:2021-09-08
-      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - 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
diff --git a/.github/workflows/clang-tidy.yml b/.github/workflows/clang-tidy.yml
deleted file mode 100644
index 69569170c8..0000000000
--- a/.github/workflows/clang-tidy.yml
+++ /dev/null
@@ -1,21 +0,0 @@
-name: Clang Tidy
-
-on:
-  push:
-    branches:
-    - 'master'
-  pull_request:
-    branches:
-    - '*'
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-clang:2021-09-08
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{secrets.ACCESS_TOKEN}}
-
-    - name: make clang-tidy-quiet
-      run: make clang-tidy-quiet
diff --git a/.github/workflows/compile_linux.yml b/.github/workflows/compile_linux.yml
deleted file mode 100644
index 54ed88d913..0000000000
--- a/.github/workflows/compile_linux.yml
+++ /dev/null
@@ -1,54 +0,0 @@
-name: Linux Targets
-
-on:
-  push:
-    branches:
-    - 'master'
-  pull_request:
-    branches:
-    - '*'
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-armhf:2021-09-08
-    strategy:
-      matrix:
-        config: [
-          beaglebone_blue_default,
-          emlid_navio2_default,
-          px4_raspberrypi_default,
-          scumaker_pilotpi_default,
-          ]
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{secrets.ACCESS_TOKEN}}
-
-    - name: Prepare ccache timestamp
-      id: ccache_cache_timestamp
-      shell: cmake -P {0}
-      run: |
-        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
-        message("::set-output name=timestamp::${current_date}")
-    - name: ccache cache files
-      uses: actions/cache@v2
-      with:
-        path: ~/.ccache
-        key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
-        restore-keys: ${{matrix.config}}-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: make ${{matrix.config}}
-      run: make ${{matrix.config}}
-    - name: ccache post-run
-      run: ccache -s
diff --git a/.github/workflows/compile_linux_arm64.yml b/.github/workflows/compile_linux_arm64.yml
deleted file mode 100644
index 5aef9adf27..0000000000
--- a/.github/workflows/compile_linux_arm64.yml
+++ /dev/null
@@ -1,51 +0,0 @@
-name: Linux ARM64 Targets
-
-on:
-  push:
-    branches:
-    - 'master'
-  pull_request:
-    branches:
-    - '*'
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-aarch64:2021-09-08
-    strategy:
-      matrix:
-        config: [
-          scumaker_pilotpi_arm64,
-          ]
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{secrets.ACCESS_TOKEN}}
-
-    - name: Prepare ccache timestamp
-      id: ccache_cache_timestamp
-      shell: cmake -P {0}
-      run: |
-        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
-        message("::set-output name=timestamp::${current_date}")
-    - name: ccache cache files
-      uses: actions/cache@v2
-      with:
-        path: ~/.ccache
-        key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
-        restore-keys: ${{matrix.config}}-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: make ${{matrix.config}}
-      run: make ${{matrix.config}}
-    - name: ccache post-run
-      run: ccache -s
diff --git a/.github/workflows/compile_macos.yml b/.github/workflows/compile_macos.yml
deleted file mode 100644
index 9be5fc8977..0000000000
--- a/.github/workflows/compile_macos.yml
+++ /dev/null
@@ -1,56 +0,0 @@
-name: MacOS build
-
-on:
-  push:
-    branches:
-    - 'master'
-  pull_request:
-    branches:
-    - '*'
-
-jobs:
-  build:
-    runs-on: macos-10.15
-    strategy:
-      matrix:
-        config: [
-          px4_fmu-v5_default,
-          px4_sitl
-          #tests, # includes px4_sitl
-          ]
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{secrets.ACCESS_TOKEN}}
-
-    - name: setup
-      run: ./Tools/setup/macos.sh; ./Tools/setup/macos.sh
-
-    - name: Prepare ccache timestamp
-      id: ccache_cache_timestamp
-      shell: cmake -P {0}
-      run: |
-        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
-        message("::set-output name=timestamp::${current_date}")
-    - name: ccache cache files
-      uses: actions/cache@v2
-      with:
-        path: ~/.ccache
-        key: macos_${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
-        restore-keys: macos_${{matrix.config}}-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 = 40M" >> ~/.ccache/ccache.conf
-          echo "hash_dir = false" >> ~/.ccache/ccache.conf
-          ccache -s
-          ccache -z
-
-    - name: make ${{matrix.config}}
-      run: |
-          ccache -z
-          make ${{matrix.config}}
-          ccache -s
diff --git a/.github/workflows/compile_nuttx.yml b/.github/workflows/compile_nuttx.yml
deleted file mode 100644
index f871cd60a6..0000000000
--- a/.github/workflows/compile_nuttx.yml
+++ /dev/null
@@ -1,126 +0,0 @@
-name: Nuttx Targets
-
-on:
-  push:
-    branches:
-    - 'master'
-  pull_request:
-    branches:
-    - '*'
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2021-09-08
-    strategy:
-      fail-fast: false
-      matrix:
-        config: [
-          airmind_mindpx-v2,
-          ark_can-flow,
-          ark_can-gps,
-          ark_can-rtk-gps,
-          ark_cannode,
-          atl_mantis-edu,
-          av_x-v1,
-          bitcraze_crazyflie,
-          bitcraze_crazyflie21,
-          cuav_can-gps-v1,
-          cuav_nora,
-          cuav_x7pro,
-          cubepilot_cubeorange,
-          cubepilot_cubeyellow,
-          diatone_mamba-f405-mk2,
-          freefly_can-rtk-gps,
-          holybro_can-gps-v1,
-          holybro_durandal-v1,
-          holybro_kakutef7,
-          holybro_kakuteh7,
-          holybro_pix32v5,
-          matek_gnss-m9n-f4,
-          matek_h743,
-          matek_h743-mini,
-          matek_h743-slim,
-          modalai_fc-v1,
-          modalai_fc-v2,
-          mro_ctrl-zero-f7,
-          mro_ctrl-zero-f7-oem,
-          mro_ctrl-zero-h7,
-          mro_ctrl-zero-h7-oem,
-          mro_pixracerpro,
-          mro_x21,
-          mro_x21-777,
-          nxp_fmuk66-e,
-          nxp_fmuk66-v3,
-          nxp_fmurt1062-v1,
-          nxp_ucans32k146,
-          omnibus_f4sd,
-          px4_fmu-v2,
-          px4_fmu-v3,
-          px4_fmu-v4,
-          px4_fmu-v4pro,
-          px4_fmu-v5,
-          px4_fmu-v5x,
-          px4_fmu-v6c,
-          px4_fmu-v6u,
-          px4_fmu-v6x,
-          raspberrypi_pico,
-          sky-drones_smartap-airlink,
-          spracing_h7extreme,
-          uvify_core
-          ]
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{secrets.ACCESS_TOKEN}}
-
-    - name: Prepare ccache timestamp
-      id: ccache_cache_timestamp
-      shell: cmake -P {0}
-      run: |
-        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
-        message("::set-output name=timestamp::${current_date}")
-    - name: ccache cache files
-      uses: actions/cache@v2
-      with:
-        path: ~/.ccache
-        key: ${{matrix.config}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
-        restore-keys: ${{matrix.config}}-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 = 120M" >> ~/.ccache/ccache.conf
-          echo "hash_dir = false" >> ~/.ccache/ccache.conf
-          ccache -s
-          ccache -z
-
-    - name: make all_variants_${{matrix.config}}
-      run: make all_variants_${{matrix.config}}
-      timeout-minutes: 45
-    - name: make ${{matrix.config}} bloaty_compileunits
-      run: make ${{matrix.config}} bloaty_compileunits || true
-    - name: make ${{matrix.config}} bloaty_inlines
-      run: make ${{matrix.config}} bloaty_inlines || true
-    - name: make ${{matrix.config}} bloaty_segments
-      run: make ${{matrix.config}} bloaty_segments || true
-    - name: make ${{matrix.config}} bloaty_symbols
-      run: make ${{matrix.config}} bloaty_symbols || true
-    - name: make ${{matrix.config}} bloaty_templates
-      run: make ${{matrix.config}} bloaty_templates || true
-    - name: make ${{matrix.config}} bloaty_ram
-      run: make ${{matrix.config}} bloaty_ram || true
-    - name: make ${{matrix.config}} bloaty_compare_master
-      run: make ${{matrix.config}} bloaty_compare_master || true
-    - name: ccache post-run
-      run: ccache -s
-
-    - name: Upload px4 package
-      uses: actions/upload-artifact@v2
-      with:
-        name: px4_package_${{matrix.config}}
-        path: |
-          build/**/*.px4
-          build/**/*.bin
diff --git a/.github/workflows/deploy_all.yml b/.github/workflows/deploy_all.yml
deleted file mode 100644
index 18480ac036..0000000000
--- a/.github/workflows/deploy_all.yml
+++ /dev/null
@@ -1,54 +0,0 @@
-name: Deploy metadata for all targets
-
-on:
-  push:
-    branches:
-    - 'master'
-    - 'release/*'
-    - 'pr-metadata-test'
-
-jobs:
-  enumerate_targets:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
-    outputs:
-      matrix: ${{ steps.set-matrix.outputs.matrix }}
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{secrets.ACCESS_TOKEN}}
-    - id: set-matrix
-      run: echo "::set-output name=matrix::$(./Tools/generate_board_targets_json.py)"
-  build:
-    runs-on: ubuntu-latest
-    needs: enumerate_targets
-    strategy:
-      matrix: ${{fromJson(needs.enumerate_targets.outputs.matrix)}}
-    container: px4io/px4-dev-${{ matrix.container }}:2021-09-08
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{secrets.ACCESS_TOKEN}}
-
-    - name: make ${{matrix.target}}
-      run: make ${{matrix.target}}
-
-    - name: parameter & events metadata
-      run: |
-        make ${{matrix.target}} ver_gen events_json actuators_json
-        ./src/lib/version/get_git_tag_or_branch_version.sh build/${{ matrix.target }} >> $GITHUB_ENV
-        cd build/${{ matrix.target }}
-        mkdir _metadata || true
-        cp parameters.* events/*.xz actuators.json* _metadata
-
-    - uses: jakejarvis/s3-sync-action@master
-      with:
-        args: --acl public-read
-      env:
-        AWS_S3_BUCKET: 'px4-travis'
-        AWS_ACCESS_KEY_ID: ${{ secrets.AWS_ACCESS_KEY_ID }}
-        AWS_SECRET_ACCESS_KEY: ${{ secrets.AWS_SECRET_ACCESS_KEY }}
-        AWS_REGION: 'us-west-1'
-        SOURCE_DIR: 'build/${{ matrix.target }}/_metadata/'
-        DEST_DIR: 'Firmware/${{ env.version }}/${{ matrix.target }}/'
-
diff --git a/.github/workflows/ekf_functional_change_indicator.yml b/.github/workflows/ekf_functional_change_indicator.yml
deleted file mode 100644
index a5fa9db599..0000000000
--- a/.github/workflows/ekf_functional_change_indicator.yml
+++ /dev/null
@@ -1,21 +0,0 @@
-name: EKF Change Indicator
-
-on: pull_request
-
-jobs:
-  unit_tests:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
-    steps:
-    - uses: actions/checkout@v2.3.1
-      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
-    - name: Check if there is a functional change
-      run: git diff --exit-code
-      working-directory: src/modules/ekf2/test/change_indication
diff --git a/.github/workflows/ekf_update_change_indicator.yml b/.github/workflows/ekf_update_change_indicator.yml
deleted file mode 100644
index 7de35207f0..0000000000
--- a/.github/workflows/ekf_update_change_indicator.yml
+++ /dev/null
@@ -1,29 +0,0 @@
-name: EKF Update Change Indicator
-
-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
-      with:
-        fetch-depth: 0
-    - name: main test updates change indication files
-      run: 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
-      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'
-        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
-      run: exit 1
diff --git a/.github/workflows/mavros_mission_tests.yml b/.github/workflows/mavros_mission_tests.yml
deleted file mode 100644
index 8401f61b11..0000000000
--- a/.github/workflows/mavros_mission_tests.yml
+++ /dev/null
@@ -1,139 +0,0 @@
-name: MAVROS Mission Tests
-
-on:
-  push:
-    branches:
-    - 'master'
-  pull_request:
-    branches:
-    - '*'
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    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"}
-
-    container:
-      image: px4io/px4-dev-ros-melodic:2021-09-08
-      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: Prepare ccache timestamp
-      id: ccache_cache_timestamp
-      shell: cmake -P {0}
-      run: |
-        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
-        message("::set-output name=timestamp::${current_date}")
-    - name: ccache cache files
-      uses: actions/cache@v2
-      with:
-        path: ~/.ccache
-        key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
-        restore-keys: sitl_tests-${{matrix.config.build_type}}-ccache-
-    - name: setup ccache
-      run: |
-          mkdir -p ~/.ccache
-          echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf
-          echo "compression = true" >> ~/.ccache/ccache.conf
-          echo "compression_level = 6" >> ~/.ccache/ccache.conf
-          echo "max_size = 100M" >> ~/.ccache/ccache.conf
-          echo "hash_dir = false" >> ~/.ccache/ccache.conf
-          ccache -s
-          ccache -z
-
-    - name: check environment
-      run: |
-          export
-          ulimit -a
-    - name: Build PX4 and sitl_gazebo
-      env:
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: |
-        ccache -z
-        make px4_sitl_default
-        make px4_sitl_default sitl_gazebo
-        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@v2-preview
-      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@v2
-      with:
-        name: binary
-        path: build/px4_sitl_default/bin/px4
-
-    - name: Store PX4 log
-      if: failure()
-      uses: actions/upload-artifact@v2
-      with:
-        name: px4_log
-        path: ~/.ros/log/*/*.ulg
-
-    - name: Store ROS log
-      if: failure()
-      uses: actions/upload-artifact@v2
-      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
diff --git a/.github/workflows/mavros_offboard_tests.yml b/.github/workflows/mavros_offboard_tests.yml
deleted file mode 100644
index 0979c5a175..0000000000
--- a/.github/workflows/mavros_offboard_tests.yml
+++ /dev/null
@@ -1,134 +0,0 @@
-name: MAVROS Offboard Tests
-
-on:
-  push:
-    branches:
-    - 'master'
-  pull_request:
-    branches:
-    - '*'
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    strategy:
-      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"}
-
-    container:
-      image: px4io/px4-dev-ros-melodic:2021-09-08
-      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: Prepare ccache timestamp
-      id: ccache_cache_timestamp
-      shell: cmake -P {0}
-      run: |
-        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
-        message("::set-output name=timestamp::${current_date}")
-    - name: ccache cache files
-      uses: actions/cache@v2
-      with:
-        path: ~/.ccache
-        key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
-        restore-keys: sitl_tests-${{matrix.config.build_type}}-ccache-
-    - name: setup ccache
-      run: |
-          mkdir -p ~/.ccache
-          echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf
-          echo "compression = true" >> ~/.ccache/ccache.conf
-          echo "compression_level = 6" >> ~/.ccache/ccache.conf
-          echo "max_size = 100M" >> ~/.ccache/ccache.conf
-          echo "hash_dir = false" >> ~/.ccache/ccache.conf
-          ccache -s
-          ccache -z
-
-    - name: check environment
-      run: |
-          export
-          ulimit -a
-    - name: Build PX4 and sitl_gazebo
-      env:
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: |
-        ccache -z
-        make px4_sitl_default
-        make px4_sitl_default sitl_gazebo
-        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@v2-preview
-      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@v2
-      with:
-        name: binary
-        path: build/px4_sitl_default/bin/px4
-
-    - name: Store PX4 log
-      if: failure()
-      uses: actions/upload-artifact@v2
-      with:
-        name: px4_log
-        path: ~/.ros/log/*/*.ulg
-
-    - name: Store ROS log
-      if: failure()
-      uses: actions/upload-artifact@v2
-      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
diff --git a/.github/workflows/metadata.yml b/.github/workflows/metadata.yml
deleted file mode 100644
index 3adb3e565e..0000000000
--- a/.github/workflows/metadata.yml
+++ /dev/null
@@ -1,157 +0,0 @@
-name: Metadata
-
-on:
-  push:
-    branches:
-    - 'master'
-    - 'release/*'
-    - 'pr-metadata-test'
-
-jobs:
-
-  airframe:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: airframe metadata
-      run: |
-        make airframe_metadata
-        ./src/lib/version/get_git_tag_or_branch_version.sh build/px4_sitl_default >> $GITHUB_ENV
-        cd build/px4_sitl_default/docs
-      # TODO: deploy to userguide gitbook
-
-    - uses: jakejarvis/s3-sync-action@master
-      with:
-        args: --acl public-read
-      env:
-        AWS_S3_BUCKET: 'px4-travis'
-        AWS_ACCESS_KEY_ID: ${{ secrets.AWS_ACCESS_KEY_ID }}
-        AWS_SECRET_ACCESS_KEY: ${{ secrets.AWS_SECRET_ACCESS_KEY }}
-        AWS_REGION: 'us-west-1'
-        SOURCE_DIR: 'build/px4_sitl_default/docs/'
-        DEST_DIR: 'Firmware/${{ env.version }}/_general/'
-
-  module:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: module documentation
-      run: |
-        make module_documentation
-        cd build/px4_sitl_default/docs
-        ls -ls *
-      # TODO: deploy to userguide gitbook and s3
-
-  parameter:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: parameter metadata
-      run: |
-        make parameters_metadata
-        ./src/lib/version/get_git_tag_or_branch_version.sh build/px4_sitl_default >> $GITHUB_ENV
-
-    - uses: jakejarvis/s3-sync-action@master
-      with:
-        args: --acl public-read
-      env:
-        AWS_S3_BUCKET: 'px4-travis'
-        AWS_ACCESS_KEY_ID: ${{ secrets.AWS_ACCESS_KEY_ID }}
-        AWS_SECRET_ACCESS_KEY: ${{ secrets.AWS_SECRET_ACCESS_KEY }}
-        AWS_REGION: 'us-west-1'
-        SOURCE_DIR: 'build/px4_sitl_default/docs/'
-        DEST_DIR: 'Firmware/${{ env.version }}/_general/'
-
-  events:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: events metadata
-      run: |
-        make extract_events
-        ./src/lib/version/get_git_tag_or_branch_version.sh build/px4_sitl_default >> $GITHUB_ENV
-        cd build/px4_sitl_default
-        mkdir _events_full || true
-        cp events/all_events_full.json.xz _events_full/all_events.json.xz
-
-    - uses: jakejarvis/s3-sync-action@master
-      with:
-        args: --acl public-read
-      env:
-        AWS_S3_BUCKET: 'px4-travis'
-        AWS_ACCESS_KEY_ID: ${{ secrets.AWS_ACCESS_KEY_ID }}
-        AWS_SECRET_ACCESS_KEY: ${{ secrets.AWS_SECRET_ACCESS_KEY }}
-        AWS_REGION: 'us-west-1'
-        SOURCE_DIR: 'build/px4_sitl_default/_events_full/'
-        DEST_DIR: 'Firmware/${{ env.version }}/_general/'
-
-  uorb_graph:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-nuttx-focal:2021-09-08
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: uORB graph
-      run: |
-        make uorb_graphs
-        cd Tools/uorb_graph
-        ls -ls *
-      # TODO: deploy graph_px4_sitl.json to S3 px4-travis:Firmware/master/
-
-  micrortps_agent:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: microRTPS agent
-      run: |
-        make px4_sitl_rtps
-        git clone https://github.com/PX4/micrortps_agent.git
-        cp -R build/px4_sitl_rtps/src/modules/micrortps_bridge/micrortps_agent/* micrortps_agent
-
-  ROS_msgs:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: PX4 ROS msgs
-      run: |
-        git clone https://github.com/PX4/px4_msgs.git
-        python3 msg/tools/uorb_to_ros_msgs.py msg/ px4_msgs/msg/
-
-  ROS2_bridge:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: PX4 ROS2 bridge
-      run: |
-        git clone https://github.com/PX4/px4_ros_com.git
-        ./msg/tools/uorb_to_ros_urtps_topics.py -i msg/tools/urtps_bridge_topics.yaml -o px4_ros_com/templates/urtps_bridge_topics.yaml
diff --git a/.github/workflows/python_checks.yml b/.github/workflows/python_checks.yml
deleted file mode 100644
index ddad50475c..0000000000
--- a/.github/workflows/python_checks.yml
+++ /dev/null
@@ -1,25 +0,0 @@
-name: Python CI Checks
-
-on:
-  push:
-    branches:
-    - 'master'
-  pull_request:
-    branches:
-    - '*'
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-    - name: Install Python3
-      run: sudo apt-get install python3 python3-setuptools python3-pip -y
-    - name: Install tools
-      run: pip3 install --user mypy types-requests flake8
-    - 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
diff --git a/.github/workflows/sitl_tests.yml b/.github/workflows/sitl_tests.yml
deleted file mode 100644
index 1f6b023531..0000000000
--- a/.github/workflows/sitl_tests.yml
+++ /dev/null
@@ -1,135 +0,0 @@
-name: SITL Tests
-
-on:
-  push:
-    branches:
-    - 'master'
-  pull_request:
-    branches:
-    - '*'
-
-jobs:
-  build:
-    runs-on: ubuntu-latest
-    strategy:
-      fail-fast: false
-      matrix:
-        config:
-          - {model: "iris",          latitude:  "59.617693", longitude: "-151.145316", altitude:  "48", build_type: "RelWithDebInfo" } # Alaska
-          - {model: "standard_vtol", latitude: "-38.071235", longitude:  "145.281220", altitude:  "31", build_type: "AddressSanitizer" } # Australia
-          - {model: "tailsitter" ,   latitude:  "29.660316", longitude:  "-82.316658", altitude:  "30", build_type: "RelWithDebInfo" } # Florida
-          - {model: "standard_vtol", latitude:  "47.397742", longitude:    "8.545594", altitude: "488", build_type: "Coverage" } # Zurich
-
-    container:
-      image: px4io/px4-dev-simulation-focal:2021-09-08
-      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: Download MAVSDK
-      run: wget "https://github.com/mavlink/MAVSDK/releases/download/v$(cat test/mavsdk_tests/MAVSDK_VERSION)/libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
-    - name: Install MAVSDK
-      run: dpkg -i "libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
-
-    - name: Prepare ccache timestamp
-      id: ccache_cache_timestamp
-      shell: cmake -P {0}
-      run: |
-        string(TIMESTAMP current_date "%Y-%m-%d-%H;%M;%S" UTC)
-        message("::set-output name=timestamp::${current_date}")
-    - name: ccache cache files
-      uses: actions/cache@v2
-      with:
-        path: ~/.ccache
-        key: sitl_tests-${{matrix.config.build_type}}-ccache-${{steps.ccache_cache_timestamp.outputs.timestamp}}
-        restore-keys: sitl_tests-${{matrix.config.build_type}}-ccache-
-    - name: setup ccache
-      run: |
-          mkdir -p ~/.ccache
-          echo "base_dir = ${GITHUB_WORKSPACE}" > ~/.ccache/ccache.conf
-          echo "compression = true" >> ~/.ccache/ccache.conf
-          echo "compression_level = 6" >> ~/.ccache/ccache.conf
-          echo "max_size = 100M" >> ~/.ccache/ccache.conf
-          echo "hash_dir = false" >> ~/.ccache/ccache.conf
-          ccache -s
-          ccache -z
-
-    - name: check environment
-      env:
-        PX4_HOME_LAT: ${{matrix.config.latitude}}
-        PX4_HOME_LON: ${{matrix.config.longitude}}
-        PX4_HOME_ALT: ${{matrix.config.altitude}}
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: |
-          export
-          ulimit -a
-    - name: Build PX4
-      env:
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: make px4_sitl_default
-    - name: ccache post-run px4/firmware
-      run: ccache -s
-    - name: Build SITL Gazebo
-      env:
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: make px4_sitl_default sitl_gazebo
-    - name: ccache post-run sitl_gazebo
-      run: ccache -s
-    - name: Build MAVSDK tests
-      env:
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-        DONT_RUN: 1
-      run: make px4_sitl_default gazebo mavsdk_tests
-    - name: ccache post-run mavsdk_tests
-      run: ccache -s
-
-    - name: Core dump settings
-      run: |
-          ulimit -c unlimited
-          echo "`pwd`/%e.core" > /proc/sys/kernel/core_pattern
-
-    - name: Run SITL tests
-      env:
-        PX4_HOME_LAT: ${{matrix.config.latitude}}
-        PX4_HOME_LON: ${{matrix.config.longitude}}
-        PX4_HOME_ALT: ${{matrix.config.altitude}}
-        PX4_CMAKE_BUILD_TYPE: ${{matrix.config.build_type}}
-      run: test/mavsdk_tests/mavsdk_test_runner.py --speed-factor 20 --abort-early --model ${{matrix.config.model}} --upload test/mavsdk_tests/configs/sitl.json --verbose
-      timeout-minutes: 45
-
-    - name: Look at core files
-      if: failure()
-      run: gdb build/px4_sitl_default/bin/px4 px4.core -ex "thread apply all bt" -ex "quit"
-    - name: Upload px4 coredump
-      if: failure()
-      uses: actions/upload-artifact@v2-preview
-      with:
-        name: coredump
-        path: px4.core
-
-    - name: Upload px4 binary
-      if: failure()
-      uses: actions/upload-artifact@v2-preview
-      with:
-        name: binary
-        path: build/px4_sitl_default/bin/px4
-
-    # Report test coverage
-    - name: Upload coverage
-      if: contains(matrix.config.build_type, 'Coverage')
-      run: |
-          git config --global credential.helper "" # disable the keychain credential helper
-          git config --global --add credential.helper store # enable the local store credential helper
-          echo "https://x-access-token:${{ secrets.ACCESS_TOKEN }}@github.com" >> ~/.git-credentials # add credential
-          git config --global url."https://github.com/".insteadof git@github.com: # credentials add credential
-          mkdir -p coverage
-          lcov --directory build/px4_sitl_default --base-directory build/px4_sitl_default --gcov-tool gcov --capture -o coverage/lcov.info
-    - name: Upload coverage information to Codecov
-      if: contains(matrix.config.build_type, 'Coverage')
-      uses: codecov/codecov-action@v1
-      with:
-        token: ${{ secrets.CODECOV_TOKEN }}
-        flags: mavsdk
-        file: coverage/lcov.info
diff --git a/.gitmodules b/.gitmodules
index 22ef494793..255e759ced 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,6 +1,6 @@
 [submodule "src/modules/mavlink/mavlink"]
 	path = src/modules/mavlink/mavlink
-	url = https://github.com/mavlink/mavlink.git
+	url = https://github.com/Auterion/mavlink.git
 	branch = master
 [submodule "src/drivers/uavcan/libuavcan"]
 	path = src/drivers/uavcan/libuavcan
@@ -12,8 +12,8 @@
 	branch = master
 [submodule "Tools/sitl_gazebo"]
 	path = Tools/sitl_gazebo
-	url = https://github.com/PX4/PX4-SITL_gazebo.git
-	branch = master
+	url = git@github.com:Auterion/sitl_gazebo.git
+	branch = develop
 [submodule "src/drivers/gps/devices"]
 	path = src/drivers/gps/devices
 	url = https://github.com/PX4/PX4-GPSDrivers.git
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c1bc1b494e..9d14c9f9dd 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -175,6 +175,12 @@ include(kconfig)
 message(STATUS "PX4 config: ${PX4_CONFIG}")
 message(STATUS "PX4 platform: ${PX4_PLATFORM}")
 
+if($ENV{CLION_IDE})
+	# CLion automatically executes some compiler commands after configuring the
+	# project. This would fail on NuttX, as visibility.h tries to (indirectly)
+	# include nuttx/config.h, which at that point does not exist yet
+	add_definitions(-DPX4_DISABLE_GCC_POISON)
+endif()
 
 if(${PX4_PLATFORM} STREQUAL "posix")
 	if(ENABLE_LOCKSTEP_SCHEDULER)
@@ -185,6 +191,18 @@ if(${PX4_PLATFORM} STREQUAL "posix")
 	endif()
 endif()
 
+
+option(PX4_RESTRICTED_BUILD "Enable restricted build (limit param access)" OFF)
+if(PX4_RESTRICTED_BUILD)
+	add_definitions(-DPX4_RESTRICTED_BUILD)
+	message(STATUS "Enabling restricted build")
+endif()
+
+if(PX4_EXPORT_CONTROLLED_BUILD)
+	add_definitions(-DPX4_EXPORT_CONTROLLED_BUILD)
+	message(STATUS "Enabling export controlled build (with flight time and wind restrictions)")
+endif()
+
 # external modules
 set(EXTERNAL_MODULES_LOCATION "" CACHE STRING "External modules source location")
 
diff --git a/Makefile b/Makefile
index 32a6e17c0d..260a80137b 100644
--- a/Makefile
+++ b/Makefile
@@ -129,6 +129,22 @@ else
 	BUILD_DIR_SUFFIX :=
 endif
 
+ifdef PX4_RESTRICTED_BUILD
+	CMAKE_ARGS += -DPX4_RESTRICTED_BUILD=ON
+	BUILD_DIR_SUFFIX := $(BUILD_DIR_SUFFIX)_restricted
+endif
+
+# pass the PX4_EXPORT_CONTROLLED_BUILD on to CMAKE_ARGS, and set it to 0 (OFF) by default
+PX4_EXPORT_CONTROLLED_BUILD ?= 0
+ifeq ($(PX4_EXPORT_CONTROLLED_BUILD),1)
+	CMAKE_ARGS += -DPX4_EXPORT_CONTROLLED_BUILD=ON
+	BUILD_DIR_SUFFIX := $(BUILD_DIR_SUFFIX)_export_controlled
+else ifeq ($(PX4_EXPORT_CONTROLLED_BUILD),0)
+	CMAKE_ARGS += -DPX4_EXPORT_CONTROLLED_BUILD=OFF
+else
+        $(error ERROR: Invalid value of flag PX4_EXPORT_CONTROLLED_BUILD, has to be '0' or '1')
+endif
+
 # additional config parameters passed to cmake
 ifdef EXTERNAL_MODULES_LOCATION
 	CMAKE_ARGS += -DEXTERNAL_MODULES_LOCATION:STRING=$(EXTERNAL_MODULES_LOCATION)
@@ -245,6 +261,12 @@ define deprecation_warning
 	$(warning $(1) has been deprecated and will be removed, please use $(2)!)
 endef
 
+skynode:
+	$(MAKE) px4_fmu-v5x $(ARGS)
+
+skynode_rtps:
+	$(MAKE) px4_fmu-v5x_rtps $(ARGS)
+
 # All targets with just dependencies but no recipe must either be marked as phony (or have the special @: as recipe).
 .PHONY: all px4_sitl_default all_config_targets all_default_targets
 
@@ -314,7 +336,9 @@ uorb_graphs:
 	@$(MAKE) --no-print-directory px4_fmu-v5_default uorb_graph
 	@$(MAKE) --no-print-directory px4_sitl_default uorb_graph
 
-px4io_update: px4_io-v2_default cubepilot_io-v2_default
+px4io_update:
+	@$(MAKE) --no-print-directory px4_io-v2_default
+	@$(MAKE) --no-print-directory cubepilot_io-v2_default
 	# px4_io-v2_default
 	cp build/px4_io-v2_default/px4_io-v2_default.bin boards/holybro/durandal-v1/extras/px4_io-v2_default.bin
 	cp build/px4_io-v2_default/px4_io-v2_default.bin boards/holybro/pix32v5/extras/px4_io-v2_default.bin
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10020_if750a.post b/ROMFS/px4fmu_common/init.d-posix/airframes/10020_if750a.post
new file mode 100644
index 0000000000..80264373dc
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10020_if750a.post
@@ -0,0 +1,10 @@
+
+mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix
+
+mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530
+
+# shellcheck disable=SC2154
+mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local
+# shellcheck disable=SC2154
+mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local
+mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10028_quadrotor_x b/ROMFS/px4fmu_common/init.d-posix/airframes/10028_quadrotor_x
new file mode 100644
index 0000000000..81e2505111
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10028_quadrotor_x
@@ -0,0 +1,12 @@
+#!/bin/sh
+#
+# @name Quadrotor SITL model for JSBSim
+#
+# @type Quadrotor Wide
+#
+# @maintainer Jaeyoung Lim <jaeyoung@auterion.com>
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+set MIXER quad_w
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10029_hexarotor_x b/ROMFS/px4fmu_common/init.d-posix/airframes/10029_hexarotor_x
new file mode 100644
index 0000000000..d614bbf553
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10029_hexarotor_x
@@ -0,0 +1,30 @@
+#!/bin/sh
+#
+# @name Hexacopter SITL model for JSBSim
+#
+# @type Hexarotor x
+#
+# @maintainer Jaeyoung Lim <jaeyoung@auterion.com>
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+param set-default MC_PITCHRATE_P 0.1
+param set-default MC_PITCHRATE_I 0.05
+param set-default MC_PITCH_P 6.0
+param set-default MC_ROLLRATE_P 0.15
+param set-default MC_ROLLRATE_I 0.1
+param set-default MC_ROLL_P 6.0
+param set-default MPC_XY_VEL_I_ACC 4
+param set-default MPC_XY_VEL_P_ACC 3
+
+param set-default RTL_DESCEND_ALT 10
+
+param set-default TRIG_INTERFACE 3
+param set-default TRIG_MODE 4
+param set-default MNT_MODE_IN 4
+param set-default MNT_DO_STAB 2
+
+param set-default MAV_TYPE 13
+
+set MIXER hexa_x
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1025_iris_depth_camera b/ROMFS/px4fmu_common/init.d-posix/airframes/1025_iris_depth_camera
new file mode 100644
index 0000000000..1c1c111a5d
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1025_iris_depth_camera
@@ -0,0 +1,11 @@
+#!/bin/sh
+#
+# @name 3DR Iris Quadrotor SITL (Depth Camera)
+#
+# @type Quadrotor Wide
+#
+
+. ${R}etc/init.d-posix/airframes/10016_iris
+
+# param set COM_OBS_AVOID 1
+param set-default MPC_XY_CRUISE 5.0
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane
index b84da715cf..47e29ede25 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1030_plane
@@ -10,16 +10,12 @@ param set-default EKF2_MAG_YAWLIM 0
 
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8
-param set-default FW_THR_LND_MAX 0
 
 param set-default FW_L1_PERIOD 12
 
-param set-default FW_MAN_P_MAX 30
-
 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.2
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 
 param set-default FW_RR_FF 0.1
@@ -29,7 +25,7 @@ param set-default FW_SPOILERS_LND 0.4
 
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
-param set-default FW_THR_CRUISE 0.25
+param set-default FW_THR_TRIM 0.25
 
 param set-default FW_T_CLMB_MAX 8
 param set-default FW_T_SINK_MAX 2.7
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal
index df50472545..80aad7ab12 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1033_rascal
@@ -7,12 +7,9 @@
 
 param set-default FW_LND_AIRSPD_SC 1.1
 param set-default FW_LND_ANG 5
-param set-default FW_THR_LND_MAX 0
-param set-default FW_LND_HHDIST 30
 param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5
-param set-default FW_LND_TLALT 15
 
 param set-default FW_L1_PERIOD 25
 
@@ -38,7 +35,6 @@ param set-default NAV_DLL_ACT 2
 param set-default RWTO_TKOFF 1
 
 param set-default RWTO_MAX_PITCH 20
-param set-default RWTO_MAX_ROLL 10
 
 param set-default RWTO_PSP 8
 param set-default RWTO_AIRSPD_SCL 1.8
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric
index 0206ebea2f..4344116652 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1034_rascal-electric
@@ -7,12 +7,9 @@
 
 param set-default FW_LND_AIRSPD_SC 1.1
 param set-default FW_LND_ANG 5
-param set-default FW_THR_LND_MAX 0
-param set-default FW_LND_HHDIST 30
 param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5
-param set-default FW_LND_TLALT 15
 
 param set-default FW_L1_PERIOD 25
 
@@ -38,7 +35,6 @@ param set-default NAV_DLL_ACT 2
 param set-default RWTO_TKOFF 1
 
 param set-default RWTO_MAX_PITCH 20
-param set-default RWTO_MAX_ROLL 10
 
 param set-default RWTO_PSP 8
 param set-default RWTO_AIRSPD_SCL 1.8
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod
index 30d7b926b0..83476e9e93 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1035_techpod
@@ -10,7 +10,6 @@ param set-default EKF2_MAG_YAWLIM 0
 
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8
-param set-default FW_THR_LND_MAX 0
 
 param set-default FW_L1_PERIOD 15
 
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo
index a6bfc13246..915940a146 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1036_malolo
@@ -7,12 +7,9 @@
 
 param set-default FW_LND_AIRSPD_SC 1.1
 param set-default FW_LND_ANG 5
-param set-default FW_THR_LND_MAX 0
-param set-default FW_LND_HHDIST 30
 param set-default FW_LND_FL_PMIN 9.5
 param set-default FW_LND_FL_PMAX 20
 param set-default FW_LND_FLALT 5
-param set-default FW_LND_TLALT 15
 
 param set-default FW_L1_PERIOD 25
 
@@ -37,7 +34,6 @@ param set-default NAV_DLL_ACT 2
 
 param set-default RWTO_TKOFF 1
 param set-default RWTO_MAX_PITCH 20
-param set-default RWTO_MAX_ROLL 10
 param set-default RWTO_PSP 8
 param set-default RWTO_AIRSPD_SCL 1.8
 
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer
index 1ad89de157..08763918f1 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1037_believer
@@ -10,16 +10,12 @@ param set-default EKF2_MAG_YAWLIM 0
 
 param set-default FW_LND_AIRSPD_SC 1
 param set-default FW_LND_ANG 8
-param set-default FW_THR_LND_MAX 0
 
 param set-default FW_L1_PERIOD 12
 
-param set-default FW_MAN_P_MAX 30
-
 param set-default FW_PR_P 0.9
 param set-default FW_PR_FF 0.2
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 
 param set-default FW_RR_FF 0.1
@@ -27,7 +23,7 @@ param set-default FW_RR_P 0.3
 
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
-param set-default FW_THR_CRUISE 0.25
+param set-default FW_THR_TRIM 0.25
 
 param set-default FW_T_ALT_TC 2
 param set-default FW_T_CLMB_MAX 8
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1038_glider b/ROMFS/px4fmu_common/init.d-posix/airframes/1038_glider
index 27f6648911..d2f7fc8bb2 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1038_glider
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1038_glider
@@ -5,5 +5,5 @@
 
 . ${R}etc/init.d-posix/airframes/1030_plane
 
-param set-default FW_THR_CRUISE 0.0
+param set-default FW_THR_TRIM 0.0
 param set-default RWTO_TKOFF 0
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol
index ff820bd50d..b15ed11d49 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1040_standard_vtol
@@ -49,21 +49,20 @@ param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_FUNC8 203
 
 param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_FF 0.1
 param set-default FW_RR_P 0.3
-param set-default FW_THR_CRUISE 0.25
+param set-default FW_THR_TRIM 0.25
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
 param set-default FW_T_CLMB_MAX 8
 param set-default FW_T_SINK_MAX 2.7
 param set-default FW_T_SINK_MIN 2.2
 
+param set-default MC_AIRMODE 1
 param set-default MC_ROLLRATE_P 0.3
 param set-default MC_YAW_P 1.6
 
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter
index e5f809af74..6e92601e0e 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1041_tailsitter
@@ -44,14 +44,12 @@ param set-default PWM_MAIN_FUNC7 202
 param set-default PWM_MAIN_REV 96 # invert both elevons
 
 param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PR_I 0.2
 param set-default FW_PR_P 0.2
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_P 0.2
-param set-default FW_THR_CRUISE 0.33
+param set-default FW_THR_TRIM 0.33
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
 param set-default FW_T_ALT_TC 2
@@ -61,6 +59,7 @@ param set-default FW_T_SINK_MAX 2.7
 param set-default FW_T_SINK_MIN 2.2
 param set-default FW_T_TAS_TC 2
 
+param set-default MC_AIRMODE 1
 param set-default MC_ROLLRATE_P 0.3
 
 param set-default MPC_ACC_HOR_MAX 2
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor
index 81af841a6d..bf59d3e030 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1042_tiltrotor
@@ -35,9 +35,7 @@ param set-default CA_SV_CS1_TYPE 2
 param set-default CA_SV_CS2_TRQ_P 1.0
 param set-default CA_SV_CS2_TYPE 3
 param set-default CA_SV_CS_COUNT 3
-param set-default CA_SV_TL0_CT 0
 param set-default CA_SV_TL1_CT 0
-param set-default CA_SV_TL2_CT 0
 param set-default CA_SV_TL3_CT 0
 param set-default CA_SV_TL_COUNT 4
 
@@ -54,15 +52,13 @@ param set-default PWM_MAIN_FUNC10 202
 param set-default PWM_MAIN_FUNC11 203
 
 param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_FF 0.1
 param set-default FW_RR_P 0.3
-param set-default FW_THR_CRUISE 0.38
+param set-default FW_THR_TRIM 0.38
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
 param set-default FW_T_CLMB_MAX 8
@@ -70,6 +66,9 @@ param set-default FW_T_SINK_MAX 2.7
 param set-default FW_T_SINK_MIN 2.2
 
 param set-default MC_YAW_P 1.6
+param set-default MC_AIRMODE 1
+param set-default MC_YAWRATE_P 0.3
+param set-default MC_YAWRATE_I 0.3
 
 param set-default MIS_TAKEOFF_ALT 10
 
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1043_standard_vtol_drop b/ROMFS/px4fmu_common/init.d-posix/airframes/1043_standard_vtol_drop
index d4a274d2ba..ef2cdf42a8 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/1043_standard_vtol_drop
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1043_standard_vtol_drop
@@ -8,15 +8,13 @@
 . ${R}etc/init.d/rc.vtol_defaults
 
 param set-default FW_L1_PERIOD 12
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PR_FF 0.2
 param set-default FW_PR_P 0.9
 param set-default FW_PSP_OFF 2
-param set-default FW_P_LIM_MAX 32
 param set-default FW_P_LIM_MIN -15
 param set-default FW_RR_FF 0.1
 param set-default FW_RR_P 0.3
-param set-default FW_THR_CRUISE 0.25
+param set-default FW_THR_TRIM 0.25
 param set-default FW_THR_MAX 0.6
 param set-default FW_THR_MIN 0.05
 param set-default FW_T_ALT_TC 2
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1058_standard_vtol_gimbal b/ROMFS/px4fmu_common/init.d-posix/airframes/1058_standard_vtol_gimbal
new file mode 100644
index 0000000000..eeee403eda
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1058_standard_vtol_gimbal
@@ -0,0 +1,17 @@
+#!/bin/sh
+#
+# @name Standard VTOL with gimbal
+#
+# @type Standard VTOL with gimbal
+#
+
+. ${R}etc/init.d-posix/1040_standard_vtol
+
+param set-default TRIG_INTERFACE 3
+param set-default TRIG_MODE 4
+param set-default MNT_MODE_IN 0
+
+param set-default MAV_TYPE 22
+
+set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix
+set MIXER custom
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1058_standard_vtol_gimbal.post b/ROMFS/px4fmu_common/init.d-posix/airframes/1058_standard_vtol_gimbal.post
new file mode 100644
index 0000000000..80264373dc
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1058_standard_vtol_gimbal.post
@@ -0,0 +1,10 @@
+
+mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix
+
+mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530
+
+# shellcheck disable=SC2154
+mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local
+# shellcheck disable=SC2154
+mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local
+mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1059_tiltrotor_tri b/ROMFS/px4fmu_common/init.d-posix/airframes/1059_tiltrotor_tri
new file mode 100644
index 0000000000..1cfc5b6347
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1059_tiltrotor_tri
@@ -0,0 +1,54 @@
+#!/bin/sh
+#
+# @name VTOL Tiltrotor Tricopter
+#
+# @type VTOL Tiltrotor Tricopter
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set-default FW_AIRSPD_MAX 25
+param set-default FW_AIRSPD_MIN 14
+param set-default FW_AIRSPD_TRIM 16
+param set-default FW_L1_PERIOD 12
+
+param set-default MC_ROLLRATE_P 0.3
+
+param set-default MIS_LTRMIN_ALT 10
+param set-default MIS_TAKEOFF_ALT 10
+param set-default MIS_YAW_TMT 10
+
+param set-default MC_PITCHRATE_P 0.3
+param set-default MC_PITCH_P 5.0
+param set-default MC_ROLLRATE_P 0.4
+param set-default MC_ROLL_P 5.0
+param set-default MC_YAWRATE_P 0.8
+param set-default MC_YAW_P 1.5
+
+param set-default MPC_ACC_HOR_MAX 2
+param set-default MPC_ACC_HOR_MAX 2
+param set-default MPC_MAN_Y_MAX 70
+param set-default MPC_THR_MIN 0.1
+param set-default MPC_TKO_SPEED 1
+param set-default MPC_XY_P 0.15
+param set-default MPC_XY_VEL_D 0.005
+param set-default MPC_XY_VEL_I 0.2
+param set-default MPC_XY_VEL_P 0.05
+param set-default MPC_Z_VEL_MAX_DN 1.5
+param set-default MPC_Z_VEL_P 0.8
+
+param set-default NAV_ACC_RAD 5
+param set-default NAV_LOITER_RAD 80
+
+param set-default VT_F_TRANS_DUR 5.0
+param set-default VT_F_TRANS_THR 0.75
+param set-default VT_TILT_FW 3.1415
+param set-default VT_TILT_TRANS 1.2
+param set-default VT_ELEV_MC_LOCK 0
+param set-default VT_TYPE 1
+param set-default VT_B_TRANS_DUR 8
+
+param set-default MAV_TYPE 24
+
+set MIXER_FILE etc/mixers-sitl/tiltrotor_tri_sitl.main.mix
+set MIXER custom
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1063_rover_gimbal b/ROMFS/px4fmu_common/init.d-posix/airframes/1063_rover_gimbal
new file mode 100644
index 0000000000..68c73285ee
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1063_rover_gimbal
@@ -0,0 +1,39 @@
+#!/bin/sh
+#
+# @name Rover
+#
+
+. ${R}etc/init.d/rc.rover_defaults
+
+param set-default GND_L1_DIST 5
+param set-default GND_SP_CTRL_MODE 1
+param set-default GND_SPEED_D 0.001
+param set-default GND_SPEED_I 3
+param set-default GND_SPEED_IMAX 0.125
+param set-default GND_SPEED_P 0.25
+param set-default GND_SPEED_THR_SC 1
+param set-default GND_SPEED_TRIM 4
+param set-default GND_THR_CRUISE 0.3
+param set-default GND_THR_IDLE 0
+param set-default GND_THR_MAX 0.5
+param set-default GND_THR_MIN 0
+
+param set-default MIS_LTRMIN_ALT 0.01
+param set-default MIS_TAKEOFF_ALT 0.01
+param set-default NAV_ACC_RAD 0.5
+param set-default NAV_LOITER_RAD 2
+
+param set-default CBRK_AIRSPD_CHK 162128
+
+param set-default GND_MAX_ANG 0.6
+param set-default GND_WHEEL_BASE 2.0
+
+param set-default TRIG_INTERFACE 3
+param set-default TRIG_MODE 4
+param set-default MNT_MODE_IN 4
+param set-default MNT_MODE_OUT 2
+param set-default MNT_DO_STAB 2
+
+param set-default MAV_TYPE 10
+
+set MIXER_FILE etc/mixers-sitl/rover_ackermann_sitl.main.mix
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1063_rover_gimbal.post b/ROMFS/px4fmu_common/init.d-posix/airframes/1063_rover_gimbal.post
new file mode 100644
index 0000000000..80264373dc
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1063_rover_gimbal.post
@@ -0,0 +1,10 @@
+
+mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix
+
+mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530
+
+# shellcheck disable=SC2154
+mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local
+# shellcheck disable=SC2154
+mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local
+mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1 b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1
index 117041cae4..0a4a023144 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/17001_tf-g1
@@ -38,10 +38,9 @@ param set-default FW_P_LIM_MAX 25
 param set-default FW_P_LIM_MIN -5
 param set-default FW_R_LIM 30
 
-param set-default FW_MAN_P_MAX 30.0
 param set-default FW_MAN_R_MAX 30.0
 
-param set-default FW_THR_CRUISE 0.8
+param set-default FW_THR_TRIM 0.8
 param set-default FW_THR_IDLE 0
 param set-default COM_DISARM_PRFLT 0
 
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc
index 39e360f8be..4f5082f48c 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6012_typhoon_h480_ctrlalloc
@@ -20,8 +20,14 @@ param set-default MC_ROLLRATE_I 0.0400
 param set-default MC_ROLLRATE_D 0.0010
 param set-default MC_ROLL_P 9.0
 
-param set-default MPC_XY_VEL_I_ACC 4
-param set-default MPC_XY_VEL_P_ACC 3
+param set-default MC_PITCHRATE_P 0.0800
+param set-default MC_PITCHRATE_I 0.0400
+param set-default MC_PITCHRATE_D 0.0010
+param set-default MC_PITCH_P 9.0
+param set-default MC_ROLLRATE_P 0.0800
+param set-default MC_ROLLRATE_I 0.0400
+param set-default MC_ROLLRATE_D 0.0010
+param set-default MC_ROLL_P 9.0
 
 param set-default RTL_DESCEND_ALT 10
 param set-default RTL_LAND_DELAY 0
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
index fba035b61e..582e8d6d7c 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
@@ -37,6 +37,9 @@ px4_add_romfs_files(
 	10018_iris_foggy_lidar
 	10019_omnicopter
 	10020_if750a
+	10020_if750a.post
+	10028_quadrotor_x
+	10029_hexarotor_x
 	10030_px4vision
 	10040_quadx
 	10041_airplane
@@ -56,6 +59,7 @@ px4_add_romfs_files(
 	1020_uuv_generic
 	1021_uuv_hippocampus
 	1022_uuv_bluerov2_heavy
+	1025_iris_depth_camera
 	1030_plane
 	1031_plane_cam
 	1032_plane_catapult
@@ -71,9 +75,14 @@ px4_add_romfs_files(
 	1042_tiltrotor
 	1043_standard_vtol_drop
 	1043_standard_vtol_drop.post
+	1058_standard_vtol_gimbal
+	1058_standard_vtol_gimbal.post
+	1059_tiltrotor_tri
 	1060_rover
 	1061_r1_rover
 	1062_tf-r1
+	1063_rover_gimbal
+	1063_rover_gimbal.post
 	1070_boat
 	3010_quadrotor_x
 	3011_hexarotor_x
diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink
index 0867fa5c64..f21da39240 100644
--- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink
+++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink
@@ -24,6 +24,7 @@ mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u $udp_gcs_port_local
 
 # API/Offboard link
 mavlink start -x -u $udp_offboard_port_local -r 4000000 -f -m onboard -o $udp_offboard_port_remote
+mavlink stream -r 2 -s COMMAND_CANCEL -u $udp_offboard_port_local
 
 # Onboard link to camera
 mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $udp_onboard_payload_port_remote
diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS
index 25dc9c2fcf..0c35a70fa5 100644
--- a/ROMFS/px4fmu_common/init.d-posix/rcS
+++ b/ROMFS/px4fmu_common/init.d-posix/rcS
@@ -30,6 +30,7 @@ set MIXER_FILE                  none
 set OUTPUT_MODE                 sim
 set EXTRA_MIXER_MODE            none
 set PWM_OUT                     none
+set PWM_AUX_OUT                 none
 set SDCARD_MIXERS_PATH          etc/mixers
 set USE_IO                      no
 set VEHICLE_TYPE                none
@@ -103,6 +104,7 @@ fi
 # multi-instance setup
 # shellcheck disable=SC2154
 param set MAV_SYS_ID $((px4_instance+1))
+param set MNT_MAV_SYSID $((px4_instance+1))
 
 if [ $AUTOCNF = yes ]
 then
@@ -255,6 +257,12 @@ then
 	gyro_calibration start
 fi
 
+# Payload deliverer module if gripper is enabled
+if param compare -s PD_GRIPPER_EN 1
+then
+	payload_deliverer start
+fi
+
 #user defined mavlink streams for instances can be in PATH
 . px4-rc.mavlink
 
diff --git a/ROMFS/px4fmu_common/init.d/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/CMakeLists.txt
index 248350cd4e..d3e4f5427e 100644
--- a/ROMFS/px4fmu_common/init.d/CMakeLists.txt
+++ b/ROMFS/px4fmu_common/init.d/CMakeLists.txt
@@ -39,8 +39,10 @@ px4_add_romfs_files(
 	rc.balloon_apps
 	rc.balloon_defaults
 	rc.boat_defaults
+	rc.autostart_ext
 	rc.fw_apps
 	rc.fw_defaults
+	rc.heli_defaults
 	rc.interface
 	rc.logging
 	rc.mc_apps
diff --git a/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil
index c96535bea4..a3650bc621 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil
+++ b/ROMFS/px4fmu_common/init.d/airframes/1000_rc_fw_easystar.hil
@@ -29,9 +29,6 @@ param set-default FW_L1_DAMPING 0.74
 param set-default FW_L1_PERIOD 16
 param set-default FW_LND_ANG 15
 param set-default FW_LND_FLALT 5
-param set-default FW_LND_HVIRT 13
-param set-default FW_LND_TLALT 5
-param set-default FW_THR_LND_MAX 0
 param set-default FW_PR_FF 0.35
 param set-default FW_PR_P 0.2
 param set-default FW_RR_FF 0.6
diff --git a/ROMFS/px4fmu_common/init.d/airframes/10015_tbs_discovery b/ROMFS/px4fmu_common/init.d/airframes/10015_tbs_discovery
deleted file mode 100644
index 0b01898f4c..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/10015_tbs_discovery
+++ /dev/null
@@ -1,36 +0,0 @@
-#!/bin/sh
-#
-# @name Team Blacksheep Discovery
-#
-# @type Quadrotor Wide
-# @class Copter
-#
-# @output MAIN1 motor 1
-# @output MAIN2 motor 2
-# @output MAIN3 motor 3
-# @output MAIN4 motor 4
-# @output MAIN5 feed-through of RC AUX1 channel
-# @output MAIN6 feed-through of RC AUX2 channel
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-# @output AUX4 feed-through of RC FLAPS channel
-#
-# @maintainer Lorenz Meier <lorenz@px4.io>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.mc_defaults
-
-param set-default MC_ROLLRATE_P 0.1
-param set-default MC_ROLLRATE_I 0.05
-param set-default MC_ROLLRATE_D 0.0017
-param set-default MC_PITCHRATE_P 0.14
-param set-default MC_PITCHRATE_I 0.1
-param set-default MC_PITCHRATE_D 0.0025
-param set-default MC_YAWRATE_P 0.28
-
-set MIXER quad_w
diff --git a/ROMFS/px4fmu_common/init.d/airframes/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/airframes/10016_3dr_iris
deleted file mode 100644
index cbc9a768e3..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/10016_3dr_iris
+++ /dev/null
@@ -1,40 +0,0 @@
-#!/bin/sh
-#
-# @name 3DR Iris Quadrotor
-#
-# @type Quadrotor Wide
-# @class Copter
-#
-# @output MAIN1 motor 1
-# @output MAIN2 motor 2
-# @output MAIN3 motor 3
-# @output MAIN4 motor 4
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-# @output AUX4 feed-through of RC FLAPS channel
-#
-# @maintainer Lorenz Meier <lorenz@px4.io>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.mc_defaults
-
-# TODO tune roll/pitch separately
-param set-default MC_ROLL_P 7
-param set-default MC_ROLLRATE_I 0.05
-param set-default MC_ROLLRATE_D 0.004
-param set-default MC_PITCH_P 7
-param set-default MC_PITCHRATE_I 0.05
-param set-default MC_PITCHRATE_D 0.004
-param set-default MC_YAW_P 2.5
-param set-default MC_YAWRATE_P 0.25
-param set-default MC_YAWRATE_I 0.25
-
-param set-default BAT1_V_DIV 12.27559
-param set-default BAT1_A_PER_V 15.391030303103
-
-set MIXER quad_w
diff --git a/ROMFS/px4fmu_common/init.d/airframes/10018_tbs_endurance b/ROMFS/px4fmu_common/init.d/airframes/10018_tbs_endurance
deleted file mode 100644
index bbf00f86bc..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/10018_tbs_endurance
+++ /dev/null
@@ -1,43 +0,0 @@
-#!/bin/sh
-#
-# @name Team Blacksheep Discovery Endurance
-#
-# @type Quadrotor Wide
-# @class Copter
-#
-# @output MAIN1 motor 1
-# @output MAIN2 motor 2
-# @output MAIN3 motor 3
-# @output MAIN4 motor 4
-# @output MAIN5 feed-through of RC AUX1 channel
-# @output MAIN6 feed-through of RC AUX2 channel
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-# @output AUX4 feed-through of RC FLAPS channel
-#
-# @maintainer Simon Wilks <simon@uaventure.com>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.mc_defaults
-
-param set-default BAT1_N_CELLS 6
-param set-default BAT1_V_EMPTY 3.5
-
-param set-default MC_ROLL_P 7
-param set-default MC_ROLLRATE_P 0.08
-param set-default MC_ROLLRATE_I 0.02
-param set-default MC_PITCH_P 7
-param set-default MC_PITCHRATE_P 0.13
-param set-default MC_PITCHRATE_I 0.02
-param set-default MC_PITCHRATE_D 0.005
-
-param set-default MPC_XY_VEL_MAX 2
-
-param set-default PWM_MAIN_MIN 1080
-
-set MIXER quad_w
diff --git a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil
index 3b9d7f7aa3..4803c0a393 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil
+++ b/ROMFS/px4fmu_common/init.d/airframes/1002_standard_vtol.hil
@@ -45,7 +45,6 @@ param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_P_ACC 3
 param set-default MPC_Z_VEL_P_ACC 12
 param set-default MPC_Z_VEL_I_ACC 3
-param set-default MPC_Z_VEL_MAX_DN 1.5
 
 param set-default NAV_ACC_RAD 5
 param set-default NAV_DLL_ACT 2
diff --git a/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard b/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard
index 4bdbb30015..9a4221f1a6 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard
+++ b/ROMFS/px4fmu_common/init.d/airframes/13000_generic_vtol_standard
@@ -1,22 +1,10 @@
 #!/bin/sh
 #
-# @name Generic Quadplane VTOL
+# @name Generic Standard VTOL
 #
 # @type Standard VTOL
 # @class VTOL
-#
-# @maintainer
-#
-# @output MAIN1 motor 1
-# @output MAIN2 motor 2
-# @output MAIN3 motor 3
-# @output MAIN4 motor 4
-# @output AUX1 Aileron 1
-# @output AUX2 Aileron 2
-# @output AUX3 Elevator
-# @output AUX4 Rudder
-# @output AUX5 Throttle
-#
+
 # @board px4_fmu-v2 exclude
 # @board bitcraze_crazyflie exclude
 # @board holybro_kakutef7 exclude
@@ -24,21 +12,21 @@
 
 . ${R}etc/init.d/rc.vtol_defaults
 
+param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 2
 param set-default CA_ROTOR_COUNT 5
-param set-default CA_ROTOR0_PX 0.15
-param set-default CA_ROTOR0_PY 0.15
-param set-default CA_ROTOR1_PX -0.15
-param set-default CA_ROTOR1_PY -0.15
-param set-default CA_ROTOR2_PX 0.15
-param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR0_PX 1
+param set-default CA_ROTOR0_PY 1
+param set-default CA_ROTOR1_PX -1
+param set-default CA_ROTOR1_PY -1
+param set-default CA_ROTOR2_PX 1
+param set-default CA_ROTOR2_PY -1
 param set-default CA_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -0.15
-param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_PX -1
+param set-default CA_ROTOR3_PY 1
 param set-default CA_ROTOR3_KM -0.05
 param set-default CA_ROTOR4_AX 1.0
 param set-default CA_ROTOR4_AZ 0.0
-param set-default CA_ROTOR4_PX 0.2
 param set-default CA_SV_CS_COUNT 4
 param set-default CA_SV_CS0_TYPE 1
 param set-default CA_SV_CS0_TRQ_R -0.5
@@ -48,16 +36,5 @@ param set-default CA_SV_CS2_TYPE 3
 param set-default CA_SV_CS2_TRQ_P 1.0
 param set-default CA_SV_CS3_TRQ_Y 1.0
 param set-default CA_SV_CS3_TYPE 4
-
-param set-default PWM_AUX_DIS5 950
-
 param set-default VT_TYPE 2
-param set-default VT_MOT_ID 1234
-param set-default VT_FW_MOT_OFFID 1234
-
 param set-default MAV_TYPE 22
-
-set MIXER quad_x
-set MIXER_AUX vtol_AAERT
-
-set PWM_OUT 1234
diff --git a/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol
deleted file mode 100644
index 85e55bbe9d..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/13001_caipirinha_vtol
+++ /dev/null
@@ -1,43 +0,0 @@
-#!/bin/sh
-#
-# @name Caipiroshka Duo Tailsitter
-#
-# @type VTOL Duo Tailsitter
-# @class VTOL
-#
-# @output MAIN1 motor right
-# @output MAIN2 motor left
-# @output MAIN5 elevon right
-# @output MAIN6 elevon left
-#
-# @maintainer Roman Bapst <roman@px4.io>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-# @board holybro_kakutef7 exclude
-#
-
-. ${R}etc/init.d/rc.vtol_defaults
-
-param set-default MAV_TYPE 19
-
-param set-default MC_ROLL_P 6
-param set-default MC_ROLLRATE_P 0.12
-param set-default MC_ROLLRATE_I 0.002
-
-param set-default MC_PITCH_P 4.5
-param set-default MC_PITCHRATE_P 0.3
-param set-default MC_PITCHRATE_I 0.002
-
-param set-default MC_YAW_P 3.8
-param set-default MC_YAWRATE_P 0.22
-param set-default MC_YAWRATE_I 0.02
-
-param set-default VT_IDLE_PWM_MC  1080
-param set-default VT_ELEV_MC_LOCK 0
-param set-default VT_MOT_ID 12
-param set-default VT_TYPE 0
-
-set MIXER vtol_tailsitter_duo
-
-set PWM_OUT 123456
diff --git a/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6 b/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6
deleted file mode 100644
index 583a045696..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/13002_firefly6
+++ /dev/null
@@ -1,53 +0,0 @@
-#!/bin/sh
-#
-# @name BirdsEyeView Aerobotics FireFly6
-# @type VTOL Tiltrotor
-# @class VTOL
-#
-# @maintainer Roman Bapst <roman@uaventure.com>
-#
-# @output MAIN1 Front right motor bottom
-# @output MAIN2 Front right motor top
-# @output MAIN3 Back motor bottom
-# @output MAIN4 Back motor top
-# @output MAIN5 Front left motor bottom
-# @output MAIN6 Front left motor top
-# @output AUX1 Tilt servo
-# @output AUX2 Elevon 1
-# @output AUX3 Elevon 2
-# @output AUX4 Gear
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.vtol_defaults
-
-param set-default MAV_TYPE 21
-
-param set-default MC_ROLL_P 7
-param set-default MC_ROLLRATE_P 0.19
-param set-default MC_ROLLRATE_I 0.002
-param set-default MC_ROLLRATE_D 0.005
-param set-default MC_PITCH_P 7
-param set-default MC_PITCHRATE_P 0.14
-param set-default MC_PITCHRATE_I 0.002
-param set-default MC_PITCHRATE_D 0.004
-param set-default MC_YAW_P 4
-param set-default MC_YAWRATE_P 0.22
-param set-default MC_YAWRATE_I 0.02
-
-param set-default VT_FW_MOT_OFFID 34
-param set-default VT_IDLE_PWM_MC 1080
-param set-default VT_MOT_ID 123456
-param set-default VT_FW_MOT_OFFID 56
-param set-default VT_TILT_MC 0.08
-param set-default VT_TILT_TRANS 0.5
-param set-default VT_TILT_FW 0.9
-param set-default VT_ELEV_MC_LOCK 0
-param set-default VT_TYPE 1
-
-set MIXER firefly6
-set MIXER_AUX firefly6
-
-set PWM_OUT 12345678
diff --git a/ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad b/ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad
index 8949f18f20..507b3e9ecf 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad
+++ b/ROMFS/px4fmu_common/init.d/airframes/13005_vtol_AAERT_quad
@@ -43,7 +43,7 @@ param set-default MPC_YAWRAUTO_MAX 40
 param set-default FW_PR_I 0.02
 param set-default FW_RR_FF 0.6
 param set-default FW_RR_I 0.01
-param set-default FW_THR_CRUISE 0.75
+param set-default FW_THR_TRIM 0.75
 
 param set-default VT_ARSP_BLEND 6
 param set-default VT_ARSP_TRANS 12
diff --git a/ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger b/ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger
deleted file mode 100644
index 94e2a1a913..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/13008_QuadRanger
+++ /dev/null
@@ -1,53 +0,0 @@
-#!/bin/sh
-#
-# @name QuadRanger
-#
-# @type Standard VTOL
-# @class VTOL
-#
-# @maintainer Sander Smeets <sander@droneslab.com>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.vtol_defaults
-
-
-param set-default FW_THR_CRUISE 65
-param set-default FW_RR_FF 0.6
-
-param set-default MIS_YAW_TMT 10
-
-param set-default MC_ROLL_P 7
-param set-default MC_ROLLRATE_I 0.1
-param set-default MC_ROLLRATE_D 0.004
-param set-default MC_PITCH_P 7
-param set-default MC_PITCHRATE_I 0.1
-param set-default MC_PITCHRATE_D 0.004
-param set-default MC_YAW_P 3.5
-param set-default MC_YAWRATE_P 0.6
-param set-default MC_YAWRATE_I 0.04
-param set-default MC_YAWRATE_MAX 40
-
-param set-default MPC_ACC_HOR_MAX 2
-param set-default MPC_Z_VEL_MAX_DN 1.5
-param set-default MPC_LAND_SPEED 0.8
-param set-default MPC_YAWRAUTO_MAX 40
-
-param set-default PWM_AUX_DIS5 950
-param set-default PWM_AUX_REV1 1
-param set-default PWM_AUX_REV2 1
-
-param set-default VT_ARSP_TRANS 15
-param set-default VT_ARSP_BLEND 8
-param set-default VT_F_TRANS_THR 0.75
-param set-default VT_IDLE_PWM_MC 1080
-param set-default VT_MOT_ID 1234
-param set-default VT_FW_MOT_OFFID 1234
-param set-default VT_TYPE 2
-
-set MIXER quad_x
-set MIXER_AUX vtol_AAERT
-
-set PWM_OUT 1234
diff --git a/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger b/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger
deleted file mode 100644
index 312ffd3e5b..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/13009_vtol_spt_ranger
+++ /dev/null
@@ -1,83 +0,0 @@
-#!/bin/sh
-#
-# @name Sparkle Tech Ranger VTOL
-#
-# @type Standard VTOL
-# @class VTOL
-#
-# @maintainer Andreas Antener <andreas@uaventure.com>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-# @board holybro_kakutef7 exclude
-#
-
-. ${R}etc/init.d/rc.vtol_defaults
-
-param set-default FW_AIRSPD_MAX   22
-param set-default FW_AIRSPD_MIN   14
-param set-default FW_AIRSPD_TRIM  16
-param set-default FW_L1_PERIOD    25
-param set-default FW_PR_P 0.060
-param set-default FW_P_RMAX_NEG   40
-param set-default FW_P_RMAX_POS   40
-param set-default FW_RR_FF    0.4
-param set-default FW_RR_P 0.04
-param set-default FW_R_RMAX   40
-
-param set-default MC_PITCHRATE_D 0.004
-param set-default MC_PITCHRATE_I 0
-param set-default MC_PITCHRATE_MAX 60
-param set-default MC_PITCHRATE_P 0.21
-param set-default MC_PITCH_P 4
-param set-default MC_ROLLRATE_D 0.004
-param set-default MC_ROLLRATE_I 0.002
-param set-default MC_ROLLRATE_MAX 60
-param set-default MC_ROLLRATE_P 0.24
-param set-default MC_ROLL_P 4
-param set-default MC_YAWRATE_I 0.02
-param set-default MC_YAWRATE_MAX 40
-param set-default MC_YAWRATE_P 0.18
-
-param set-default MIS_TAKEOFF_ALT 2.5
-param set-default MIS_YAW_TMT 20
-
-param set-default MPC_ACC_HOR_MAX 1
-param set-default MPC_HOLD_MAX_XY 0.5
-param set-default MPC_HOLD_MAX_Z  0.5
-param set-default MPC_LAND_SPEED  1
-param set-default MPC_MANTHR_MIN  0.05
-param set-default MPC_MAN_Y_MAX   120
-param set-default MPC_THR_MIN 0.07
-param set-default MPC_TILTMAX_AIR 35
-param set-default MPC_TILTMAX_LND 20
-param set-default MPC_TKO_SPEED   1
-param set-default MPC_XY_P    0.3
-param set-default MPC_XY_VEL_MAX  3
-param set-default MPC_XY_VEL_P_ACC 1
-param set-default MPC_Z_P 0.5
-param set-default MPC_Z_VEL_P_ACC 2
-param set-default MPC_YAWRAUTO_MAX 40
-
-param set-default NAV_ACC_RAD 3
-
-param set-default PWM_AUX_REV1    1
-param set-default PWM_AUX_REV2    1
-param set-default PWM_AUX_REV3    1
-param set-default PWM_AUX_REV4    1
-
-param set-default PWM_AUX_DIS5 950
-
-param set-default VT_ARSP_TRANS   15
-param set-default VT_F_TRANS_THR    0.6
-param set-default VT_IDLE_PWM_MC 1180
-param set-default VT_MOT_ID 1234
-param set-default VT_FW_MOT_OFFID 1234
-param set-default VT_TRANS_MIN_TM 5
-param set-default VT_TRANS_TIMEOUT    30
-param set-default VT_TYPE 2
-
-set MIXER quad_x
-set MIXER_AUX vtol_AAERT
-
-set PWM_OUT 1234
diff --git a/ROMFS/px4fmu_common/init.d/airframes/13012_convergence b/ROMFS/px4fmu_common/init.d/airframes/13012_convergence
index 7cc6fc4d85..79b3dda403 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/13012_convergence
+++ b/ROMFS/px4fmu_common/init.d/airframes/13012_convergence
@@ -34,8 +34,6 @@ param set-default FW_ACRO_X_MAX   270
 param set-default FW_ACRO_Y_MAX   270
 param set-default FW_ACRO_Z_MAX   180
 param set-default FW_PSP_OFF  5
-param set-default FW_P_LIM_MAX    30
-param set-default FW_P_LIM_MIN    -30
 param set-default FW_RR_FF    0.33
 param set-default FW_RR_P 0.11
 
@@ -74,4 +72,4 @@ then
 	set PWM_OUT 1234
 else
 	set PWM_OUT 3456
-fi	
+fi
diff --git a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad
index 394227a33f..ea53dc3d65 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad
+++ b/ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad
@@ -53,9 +53,8 @@ param set-default FW_T_CLMB_MAX 3
 param set-default FW_T_SINK_MAX 3
 param set-default FW_T_SINK_MIN 1
 param set-default FW_T_VERT_ACC 6
-param set-default FW_THR_CRUISE 0.70
+param set-default FW_THR_TRIM 0.70
 param set-default FW_THR_SLEW_MAX 1
-param set-default FW_MAN_P_MAX 30
 param set-default FW_P_LIM_MAX 15
 param set-default FW_P_LIM_MIN -25
 param set-default FW_P_RMAX_NEG 45
@@ -90,7 +89,6 @@ param set-default MPC_ACC_HOR_MAX 2
 param set-default MPC_LAND_SPEED 1.2
 param set-default MPC_TILTMAX_LND 35
 param set-default MPC_Z_VEL_MAX_UP 1.5
-param set-default MPC_Z_VEL_MAX_DN 1.5
 param set-default MPC_HOLD_MAX_XY 0.5
 param set-default MPC_HOLD_MAX_Z 0.5
 param set-default MPC_TKO_RAMP_T 0.8
diff --git a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark
index 5d1be9b55f..2a0131e452 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark
+++ b/ROMFS/px4fmu_common/init.d/airframes/13014_vtol_babyshark
@@ -31,7 +31,6 @@ param set-default FW_AIRSPD_MAX 30
 param set-default FW_AIRSPD_MIN 19
 param set-default FW_AIRSPD_TRIM 23
 param set-default FW_L1_R_SLEW_MAX 40
-param set-default FW_MAN_P_MAX 30
 param set-default FW_PSP_OFF 3
 param set-default FW_P_LIM_MAX 18
 param set-default FW_P_LIM_MIN -25
@@ -39,7 +38,7 @@ param set-default FW_RLL_TO_YAW_FF 0.1
 param set-default FW_RR_P 0.08
 param set-default FW_R_LIM 45
 param set-default FW_R_RMAX 50
-param set-default FW_THR_CRUISE 0.65
+param set-default FW_THR_TRIM 0.65
 param set-default FW_THR_MIN 0.3
 param set-default FW_THR_SLEW_MAX 0.6
 param set-default FW_T_HRATE_FF 0
@@ -71,7 +70,6 @@ param set-default MPC_VEL_MANUAL 3
 param set-default MPC_XY_CRUISE 3
 param set-default MPC_XY_VEL_MAX 3.5
 param set-default MPC_YAWRAUTO_MAX 40
-param set-default MPC_Z_VEL_MAX_DN 1.5
 param set-default MPC_Z_VEL_MAX_UP 2
 
 param set-default NAV_ACC_RAD 3
diff --git a/ROMFS/px4fmu_common/init.d/airframes/13100_generic_vtol_tiltrotor b/ROMFS/px4fmu_common/init.d/airframes/13100_generic_vtol_tiltrotor
new file mode 100644
index 0000000000..a457a7ce76
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/airframes/13100_generic_vtol_tiltrotor
@@ -0,0 +1,44 @@
+#!/bin/sh
+#
+# @name Generic Tiltrotor VTOL
+#
+# @type VTOL Tiltrotor
+# @class VTOL
+#
+# @board px4_fmu-v2 exclude
+# @board bitcraze_crazyflie exclude
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+param set-default SYS_CTRL_ALLOC 1
+
+param set-default CA_AIRFRAME 3
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 1
+param set-default CA_ROTOR0_PY 1
+param set-default CA_ROTOR0_TILT 2
+param set-default CA_ROTOR1_PX -1
+param set-default CA_ROTOR1_PY -1
+param set-default CA_ROTOR2_PX 1
+param set-default CA_ROTOR2_PY -1
+param set-default CA_ROTOR2_TILT 1
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -1
+param set-default CA_ROTOR3_PY 1
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS2_TYPE 7
+param set-default CA_SV_CS2_TRQ_P 0.5
+param set-default CA_SV_CS2_TRQ_Y 0.5
+param set-default CA_SV_CS3_TYPE 8
+param set-default CA_SV_CS3_TRQ_P 0.5
+param set-default CA_SV_CS3_TRQ_Y -0.5
+param set-default CA_SV_TL_COUNT 2
+
+param set-default MAV_TYPE 21
+param set-default VT_TYPE 1
diff --git a/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter b/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter
index d235153980..888e530050 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter
+++ b/ROMFS/px4fmu_common/init.d/airframes/13200_generic_vtol_tailsitter
@@ -1,17 +1,10 @@
 #!/bin/sh
 #
-# @name Generic Tailsitter
+# @name Generic VTOL Tailsitter
 #
-# @type VTOL Duo Tailsitter
+# @type VTOL Tailsitter
 # @class VTOL
 #
-# @output MAIN1 motor right
-# @output MAIN2 motor left
-# @output MAIN5 elevon right
-# @output MAIN6 elevon left
-#
-# @maintainer Roman Bapst <roman@px4.io>
-#
 # @board px4_fmu-v2 exclude
 # @board bitcraze_crazyflie exclude
 # @board holybro_kakutef7 exclude
@@ -19,18 +12,13 @@
 
 . ${R}etc/init.d/rc.vtol_defaults
 
-param set-default MAV_TYPE 19
-
-param set-default VT_ELEV_MC_LOCK 0
-param set-default VT_MOT_COUNT 2
-param set-default VT_TYPE 0
-
+param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 4
 param set-default CA_ROTOR_COUNT 2
 param set-default CA_ROTOR0_KM -0.05
-param set-default CA_ROTOR0_PY 0.2
-param set-default CA_ROTOR1_KM -0.05
-param set-default CA_ROTOR1_PY -0.2
+param set-default CA_ROTOR0_PY -0.2
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR1_PY 0.2
 param set-default CA_SV_CS_COUNT 2
 param set-default CA_SV_CS0_TRQ_P 0.5
 param set-default CA_SV_CS0_TRQ_Y 0.5
@@ -39,6 +27,6 @@ param set-default CA_SV_CS1_TRQ_P 0.5
 param set-default CA_SV_CS1_TRQ_Y -0.5
 param set-default CA_SV_CS1_TYPE 6
 
-set MIXER vtol_tailsitter_duo
-
-set PWM_OUT 1234
+param set-default MAV_TYPE 19
+param set-default VT_TYPE 0
+param set-default VT_ELEV_MC_LOCK 0
diff --git a/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter b/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter
index ac1c5475c1..de21a88216 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter
+++ b/ROMFS/px4fmu_common/init.d/airframes/16001_helicopter
@@ -1,54 +1,26 @@
 #!/bin/sh
 #
-# @name Blade 130X
+# @name Generic Helicopter (Tail ESC)
 #
 # @type Helicopter
 # @class Copter
 #
-# @maintainer Bart Slinger <bartslinger@gmail.com>
-#
-# @output MAIN1 main motor
-# @output MAIN2 front swashplate servo
-# @output MAIN3 right swashplate servo
-# @output MAIN4 left swashplate servo
-# @output MAIN5 tail-rotor servo
-#
 # @board px4_fmu-v2 exclude
 # @board bitcraze_crazyflie exclude
 #
 
-. ${R}etc/init.d/rc.mc_defaults
+. ${R}etc/init.d/rc.heli_defaults
 
-# Configure as helicopter (number 4 defined in commander_helper.cpp)
-param set-default MAV_TYPE 4
 
-set MIXER blade130
-
-set PWM_OUT none
-
-param set-default ATT_BIAS_MAX 0
-
-param set-default CBRK_IO_SAFETY 22027
-
-param set-default MC_ROLL_P 5
+# 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
 param set-default MC_ROLLRATE_I 0
 param set-default MC_ROLLRATE_D 0
-param set-default MC_ROLLRATE_FF 0.15
+param set-default MC_ROLLRATE_FF 0.1
 param set-default MC_PITCHRATE_P 0
 param set-default MC_PITCHRATE_I 0
 param set-default MC_PITCHRATE_D 0
-param set-default MC_PITCHRATE_FF 0.15
-param set-default MC_YAW_P 3
-param set-default MC_YAWRATE_P 0.1
-param set-default MC_YAWRATE_I 0
-param set-default MC_ROLLRATE_MAX 720
-param set-default MC_PITCHRATE_MAX 720
-param set-default MC_YAWRATE_MAX 400
-param set-default MC_ACRO_R_MAX 360
-param set-default MC_ACRO_P_MAX 360
+param set-default MC_PITCHRATE_FF 0.1
 
-param set-default MPC_THR_MIN 0.06
-param set-default MPC_MANTHR_MIN 0.06
-
-param set-default PWM_MAIN_MIN 1075
+param set-default CA_AIRFRAME 10
diff --git a/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2 b/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2
deleted file mode 100644
index 8c847cbb44..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/17002_TF-AutoG2
+++ /dev/null
@@ -1,51 +0,0 @@
-#!/bin/sh
-#
-# @name ThunderFly Auto-G2
-#
-# @type Autogyro
-# @class Autogyro
-#
-# @output MAIN1 rotor_head_L
-# @output MAIN2 rotor_head_R
-# @output MAIN3 elevator
-# @output MAIN4 rudder
-# @output MAIN5 rudder (second, optional)
-# @output MAIN6 throttle
-# @output MAIN7 wheel
-#
-# @output AUX1 feed-through of RC AUX1 channel for prerotator (optional)
-# @output AUX2 feed-through of RC AUX2 channel for release device (optional)
-#
-# @url https://github.com/ThunderFly-aerospace/Auto-G2/
-# @maintainer ThunderFly s.r.o., Roman Dvorak <dvorakroman@thunderfly.cz>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.fw_defaults
-
-
-param set-default BAT1_CAPACITY 2500
-param set-default BAT1_N_CELLS 3
-
-param set-default PWM_AUX_RATE 50
-param set-default PWM_MAIN_RATE 50
-
-param set-default SENS_BOARD_ROT 8
-
-param set-default FW_AIRSPD_MAX 20
-param set-default FW_AIRSPD_MIN 7
-param set-default FW_AIRSPD_TRIM 13
-param set-default FW_THR_CRUISE 0.8
-
-param set-default FW_MAN_P_MAX 25
-param set-default FW_MAN_R_MAX 25
-param set-default FW_PR_I 0.02
-param set-default FW_R_LIM 40
-param set-default FW_P_LIM_MAX 25
-param set-default FW_P_LIM_MIN -5
-param set-default FW_P_RMAX_NEG 20
-
-set MIXER TF-AutoG2
-set MIXER_AUX pass
diff --git a/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2 b/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2
deleted file mode 100644
index e677228cd8..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/17003_TF-G2
+++ /dev/null
@@ -1,46 +0,0 @@
-#!/bin/sh
-#
-# @name ThunderFly TF-G2
-#
-# @type Autogyro
-# @class Autogyro
-#
-# @output MAIN2 rotor_head_L
-# @output MAIN3 rotor_head_R
-# @output MAIN4 rudder
-# @output MAIN5 throttle
-#
-#
-# @url https://github.com/ThunderFly-aerospace/TF-G2/
-# @maintainer ThunderFly s.r.o., Roman Dvorak <dvorakroman@thunderfly.cz>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.fw_defaults
-
-
-param set-default BAT1_CAPACITY 3300
-param set-default BAT1_N_CELLS 3
-
-param set-default PWM_AUX_RATE 50
-param set-default PWM_MAIN_RATE 50
-
-param set-default SENS_BOARD_ROT 4
-
-param set-default FW_AIRSPD_MAX 20
-param set-default FW_AIRSPD_MIN 7
-param set-default FW_AIRSPD_TRIM 13
-param set-default FW_THR_CRUISE 0.8
-
-param set-default FW_MAN_P_MAX 25
-param set-default FW_MAN_R_MAX 25
-param set-default FW_PR_I 0.02
-param set-default FW_R_LIM 40
-param set-default FW_P_LIM_MAX 25
-param set-default FW_P_LIM_MIN -5
-param set-default FW_P_RMAX_NEG 20
-
-set MIXER TF-G2
-set MIXER_AUX pass
diff --git a/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane b/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane
index 99e53f618b..9ed8f7f69b 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane
+++ b/ROMFS/px4fmu_common/init.d/airframes/2100_standard_plane
@@ -1,28 +1,16 @@
 #!/bin/sh
 #
-# @name Standard Plane
+# @name Generic Standard Plane
 #
 # @type Standard Plane
 # @class Plane
 #
-# @output MAIN1 aileron
-# @output MAIN2 elevator
-# @output MAIN3 throttle
-# @output MAIN4 rudder
-# @output MAIN5 flaps
-# @output MAIN6 gear
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-#
-# @maintainer Lorenz Meier <lorenz@px4.io>
-#
 # @board bitcraze_crazyflie exclude
 #
 
 . ${R}etc/init.d/rc.fw_defaults
 
+param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 1
 param set-default CA_ROTOR_COUNT 1
 param set-default CA_ROTOR0_PX 0.3
@@ -34,11 +22,3 @@ param set-default CA_SV_CS2_TRQ_P 1.0
 param set-default CA_SV_CS2_TYPE 3
 param set-default CA_SV_CS3_TRQ_Y 1.0
 param set-default CA_SV_CS3_TYPE 4
-
-param set-default PWM_AUX_RATE 50
-param set-default PWM_MAIN_RATE 50
-
-set MIXER AETRFG
-
-# Rate must be set by group (see pwm info).
-# Throttle is in the same group as servos.
diff --git a/ROMFS/px4fmu_common/init.d/airframes/2105_maja b/ROMFS/px4fmu_common/init.d/airframes/2105_maja
deleted file mode 100644
index 026a0c3043..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/2105_maja
+++ /dev/null
@@ -1,45 +0,0 @@
-#!/bin/sh
-#
-# @name Bormatec Maja
-#
-# @type Standard Plane
-# @class Plane
-#
-# @output MAIN1 aileron
-# @output MAIN2 aileron
-# @output MAIN3 elevator
-# @output MAIN4 rudder
-# @output MAIN5 throttle
-# @output MAIN6 wheel
-# @output MAIN7 flaps
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-#
-# @maintainer Andreas Antener <andreas@uaventure.com>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.fw_defaults
-
-param set-default FW_MAN_P_MAX 55
-param set-default FW_MAN_R_MAX 55
-param set-default FW_R_LIM 55
-
-param set-default FW_WR_FF 0.2
-param set-default FW_WR_I 0.2
-param set-default FW_WR_IMAX 0.8
-param set-default FW_WR_P 1
-param set-default FW_W_RMAX 0
-
-# set disarmed value for the ESC
-param set-default PWM_MAIN_DISARM 1000
-
-
-set MIXER AAERTWF
-
-# use PWM parameters for throttle channel
-set PWM_OUT 5
diff --git a/ROMFS/px4fmu_common/init.d/airframes/2200_mini_talon b/ROMFS/px4fmu_common/init.d/airframes/2200_mini_talon
deleted file mode 100644
index 8fbe544c5b..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/2200_mini_talon
+++ /dev/null
@@ -1,48 +0,0 @@
-#!/bin/sh
-#
-# @name X-UAV Mini Talon
-#
-# @type Plane V-Tail
-# @class Plane
-#
-# @output MAIN1 aileron right
-# @output MAIN2 aileron left
-# @output MAIN3 v-tail right
-# @output MAIN4 v-tail left
-# @output MAIN5 throttle
-# @output MAIN6 wheel
-# @output MAIN7 flaps right
-# @output MAIN8 flaps left
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-#
-# @maintainer Friedrich Beckmann <friedrich.beckmann@hs-augsburg.de>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.fw_defaults
-
-param set-default FW_MAN_P_MAX 55
-param set-default FW_MAN_R_MAX 55
-param set-default FW_R_LIM 55
-
-param set-default FW_WR_FF 0.2
-param set-default FW_WR_I 0.2
-param set-default FW_WR_IMAX 0.8
-param set-default FW_WR_P 1
-param set-default FW_W_RMAX 0
-
-# set disarmed value for the ESC
-param set-default PWM_MAIN_DISARM 1000
-
-# The Mini Talon does not have a wheel and
-# no flaps. I leave them here because the mixer
-# computes also wheel and flap controls.
-set MIXER AAVVTWFF_vtail
-
-# use PWM parameters for throttle channel
-set PWM_OUT 5
diff --git a/ROMFS/px4fmu_common/init.d/airframes/3000_generic_wing b/ROMFS/px4fmu_common/init.d/airframes/3000_generic_wing
index 4b5ee877d6..005a596103 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/3000_generic_wing
+++ b/ROMFS/px4fmu_common/init.d/airframes/3000_generic_wing
@@ -5,19 +5,20 @@
 # @type Flying Wing
 # @class Plane
 #
-# @output MAIN1 left aileron
-# @output MAIN2 right aileron
-# @output MAIN4 throttle
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-#
-# @maintainer
-#
 # @board bitcraze_crazyflie exclude
 #
 
 . ${R}etc/init.d/rc.fw_defaults
 
-set MIXER fw_generic_wing
+param set-default SYS_CTRL_ALLOC 1
+param set-default CA_AIRFRAME 1
+
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS0_TRQ_P 0.5
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TYPE 6
+param set-default CA_SV_CS1_TRQ_P 0.5
+param set-default CA_SV_CS1_TRQ_R 0.5
diff --git a/ROMFS/px4fmu_common/init.d/airframes/3031_phantom b/ROMFS/px4fmu_common/init.d/airframes/3031_phantom
deleted file mode 100644
index b31b2230ce..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/3031_phantom
+++ /dev/null
@@ -1,49 +0,0 @@
-#!/bin/sh
-#
-# @name Phantom FPV Flying Wing
-#
-# @url https://docs.px4.io/master/en/frames_plane/wing_wing_z84.html
-#
-# @type Flying Wing
-# @class Plane
-#
-# @output MAIN1 left aileron
-# @output MAIN2 right aileron
-# @output MAIN4 throttle
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-#
-# @maintainer Simon Wilks <simon@uaventure.com>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.fw_defaults
-
-param set-default FW_AIRSPD_MIN 13
-param set-default FW_AIRSPD_MAX 25
-param set-default FW_R_TC 0.3
-param set-default FW_P_TC 0.3
-param set-default FW_L1_PERIOD 15
-param set-default FW_PR_FF 0.2
-param set-default FW_PR_IMAX 0.2
-param set-default FW_PR_P 0.03
-param set-default FW_P_LIM_MAX 50
-param set-default FW_P_LIM_MIN -50
-param set-default FW_RR_P 0.08
-param set-default FW_R_LIM 50
-param set-default FW_R_RMAX 50
-
-param set-default PWM_MAIN_DISARM 1000
-
-# Bottom of bay and nominal zero-pitch attitude differ
-# the payload bay is pitched up about 7 degrees
-param set-default SENS_BOARD_Y_OFF 7
-
-set MIXER fw_generic_wing
-
-# Provide ESC a constant 1000 us pulse
-set PWM_OUT 4
diff --git a/ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5 b/ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5
deleted file mode 100644
index f09bba392b..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/3032_skywalker_x5
+++ /dev/null
@@ -1,40 +0,0 @@
-#!/bin/sh
-#
-# @name Skywalker X5 Flying Wing
-#
-# @type Flying Wing
-# @class Plane
-#
-# @output MAIN1 left aileron
-# @output MAIN2 right aileron
-# @output MAIN4 throttle
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-#
-# @maintainer Julian Oes <julian@px4.io>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.fw_defaults
-
-param set-default FW_AIRSPD_MIN 15
-param set-default FW_AIRSPD_TRIM 20
-param set-default FW_AIRSPD_MAX 40
-param set-default FW_R_TC 0.3
-param set-default FW_P_TC 0.3
-param set-default FW_L1_DAMPING 0.74
-param set-default FW_L1_PERIOD 16
-param set-default FW_LND_ANG 15
-param set-default FW_LND_FLALT 5
-param set-default FW_LND_HVIRT 13
-param set-default FW_LND_TLALT 5
-param set-default FW_THR_LND_MAX 0
-param set-default FW_PR_FF 0.35
-param set-default FW_RR_FF 0.6
-param set-default FW_RR_P 0.04
-
-set MIXER fw_generic_wing
diff --git a/ROMFS/px4fmu_common/init.d/airframes/3033_wingwing b/ROMFS/px4fmu_common/init.d/airframes/3033_wingwing
deleted file mode 100644
index d9a18853a2..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/3033_wingwing
+++ /dev/null
@@ -1,48 +0,0 @@
-#!/bin/sh
-#
-# @name Wing Wing (aka Z-84) Flying Wing
-#
-# @url https://docs.px4.io/master/en/frames_plane/wing_wing_z84.html
-#
-# @type Flying Wing
-# @class Plane
-#
-# @output MAIN1 left aileron
-# @output MAIN2 right aileron
-# @output MAIN4 throttle
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-#
-# @maintainer Lorenz Meier <lorenz@px4.io>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.fw_defaults
-
-param set-default BAT1_N_CELLS 2
-param set-default FW_AIRSPD_MAX 15
-param set-default FW_AIRSPD_TRIM 13
-param set-default FW_R_TC 0.3
-param set-default FW_P_TC 0.3
-param set-default FW_L1_DAMPING 0.74
-param set-default FW_L1_PERIOD 16
-param set-default FW_LND_ANG 15
-param set-default FW_LND_FLALT 5
-param set-default FW_LND_HVIRT 13
-param set-default FW_LND_TLALT 5
-param set-default FW_THR_LND_MAX 0
-param set-default FW_PR_FF 0.35
-param set-default FW_RR_FF 0.6
-param set-default FW_RR_P 0.04
-
-param set-default PWM_MAIN_DISARM 1000
-
-# Set mixer.
-set MIXER fw_generic_wing
-
-# Provide ESC a constant 1000 us pulse.
-set PWM_OUT 4
diff --git a/ROMFS/px4fmu_common/init.d/airframes/3036_pigeon b/ROMFS/px4fmu_common/init.d/airframes/3036_pigeon
deleted file mode 100644
index b889f38db9..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/3036_pigeon
+++ /dev/null
@@ -1,49 +0,0 @@
-#!/bin/sh
-#
-# @name Sparkle Tech Pigeon
-#
-# @url http://www.sparkletech.hk/
-#
-# @type Flying Wing
-# @class Plane
-#
-# @output MAIN1 left aileron
-# @output MAIN2 right aileron
-# @output MAIN4 throttle
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-#
-# @maintainer Simon Wilks <simon@uaventure.com>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.fw_defaults
-
-param set-default FW_AIRSPD_MIN 15
-param set-default FW_AIRSPD_TRIM 20
-param set-default FW_AIRSPD_MAX 27
-param set-default FW_ATT_TC 0.3
-param set-default FW_PR_FF 0.35
-param set-default FW_PR_IMAX 0.2
-param set-default FW_PR_P 0.05
-param set-default FW_P_TC 0.3
-param set-default FW_RR_FF 0.3
-param set-default FW_RR_P 0.03
-param set-default FW_R_LIM 40
-param set-default FW_R_RMAX 50
-param set-default FW_R_TC 0.3
-
-param set-default PWM_MAIN_DISARM 1000
-
-# Bottom of bay and nominal zero-pitch attitude differ
-# the payload bay is pitched up about 7 degrees
-param set-default SENS_BOARD_Y_OFF 11.9
-
-set MIXER fw_generic_wing
-
-# Provide ESC a constant 1000 us pulse
-set PWM_OUT 4
diff --git a/ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha
deleted file mode 100644
index cc74225a12..0000000000
--- a/ROMFS/px4fmu_common/init.d/airframes/3100_tbs_caipirinha
+++ /dev/null
@@ -1,61 +0,0 @@
-#!/bin/sh
-#
-# @name TBS Caipirinha
-#
-# @type Flying Wing
-# @class Plane
-#
-# @output MAIN1 left aileron
-# @output MAIN2 right aileron
-# @output MAIN4 throttle
-#
-# @output AUX1 feed-through of RC AUX1 channel
-# @output AUX2 feed-through of RC AUX2 channel
-# @output AUX3 feed-through of RC AUX3 channel
-#
-# @maintainer Lorenz Meier <lorenz@px4.io>
-#
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#
-
-. ${R}etc/init.d/rc.fw_defaults
-
-param set-default FW_AIRSPD_MAX 25
-param set-default FW_AIRSPD_MIN 12.5
-param set-default FW_AIRSPD_TRIM 16.5
-param set-default FW_L1_PERIOD 15
-param set-default FW_LND_ANG 15
-param set-default FW_LND_FLALT 8
-param set-default FW_LND_HVIRT 13
-param set-default FW_LND_TLALT 10
-param set-default FW_THR_LND_MAX 0
-param set-default FW_P_LIM_MAX 20
-param set-default FW_P_LIM_MIN -30
-param set-default FW_R_LIM 45
-param set-default FW_PR_FF 0.45
-param set-default FW_PR_P 0.005
-param set-default FW_RR_FF 0.45
-param set-default FW_RR_P 0.013
-param set-default FW_P_RMAX_NEG 70
-param set-default FW_P_RMAX_POS 70
-param set-default FW_R_RMAX 70
-param set-default FW_THR_CRUISE 0.55
-
-param set-default LNDFW_AIRSPD_MAX 6
-param set-default LNDFW_XYACC_MAX 4
-param set-default LNDFW_VEL_XY_MAX 3
-param set-default LNDFW_VEL_Z_MAX 5
-
-param set-default MIS_TAKEOFF_ALT 50
-
-param set-default NAV_LOITER_RAD 30
-
-param set-default PWM_MAIN_REV1 1
-param set-default PWM_MAIN_REV2 1
-param set-default PWM_MAIN_MIN 900
-param set-default PWM_MAIN_MAX 2100
-
-set MIXER fw_generic_wing
-
-set PWM_OUT 1234
diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision
index cf15f0cbca..68338392bc 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision
+++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision
@@ -69,7 +69,6 @@ param set-default CP_DIST 6
 param set-default MPC_ACC_DOWN_MAX 5
 param set-default MPC_ACC_HOR_MAX 10
 param set-default MPC_ACC_UP_MAX 4
-param set-default MPC_MANTHR_MIN 0
 param set-default MPC_MAN_Y_MAX 120
 param set-default MPC_TILTMAX_AIR 45
 param set-default MPC_THR_HOVER 0.3
diff --git a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250 b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250
index dfd2bacd61..8970e6e7a5 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250
+++ b/ROMFS/px4fmu_common/init.d/airframes/4050_generic_250
@@ -26,7 +26,6 @@ param set-default MC_ROLLRATE_MAX 1600
 param set-default MC_PITCHRATE_MAX 1600
 param set-default MC_YAWRATE_MAX 1000
 
-param set-default MPC_MANTHR_MIN 0
 param set-default MPC_MAN_TILT_MAX 60
 
 # use thrust curve factor (instead of TPA)
diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
index d99b2d8d25..e82803e361 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
+++ b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
@@ -44,19 +44,12 @@ px4_add_romfs_files(
 
 	# [2000, 2999] Standard planes"
 	2100_standard_plane
-	2105_maja
 	2106_albatross
-	2200_mini_talon
 
 	2507_cloudship
 
 	# [3000, 3999] Flying wing"
 	3000_generic_wing
-	3031_phantom
-	3032_skywalker_x5
-	3033_wingwing
-	3036_pigeon
-	3100_tbs_caipirinha
 
 	# [4000, 4999] Quadrotor x"
 	4001_quad_x
@@ -67,8 +60,6 @@ px4_add_romfs_files(
 	4016_holybro_px4vision
 	4017_nxp_hovergames
 	4019_x500_v2
-	4030_3dr_solo
-	4031_3dr_quad
 	4040_reaper
 	4041_beta75x
 	4050_generic_250
@@ -77,11 +68,8 @@ px4_add_romfs_files(
 	4053_holybro_kopis2
 	4061_atl_mantis_edu
 	4071_ifo
-	4072_draco
 	4073_ifo-s
 	4080_zmr250
-	4090_nanomind
-	4100_tiltquadrotor
 	4500_clover4
 	4900_crazyflie
 	4901_crazyflie21
@@ -102,10 +90,6 @@ px4_add_romfs_files(
 	# [9000, 9999] Octorotor +"
 	9001_octo_+
 
-	# [10000, 10999] Quadrotor Wide arm / H frame"
-	10015_tbs_discovery
-	10016_3dr_iris
-	10018_tbs_endurance
 
 	# [11000, 11999] Hexa Cox
 	11001_hexa_cox
@@ -115,44 +99,31 @@ px4_add_romfs_files(
 
 	# [13000, 13999] VTOL
 	13000_generic_vtol_standard
-	13001_caipirinha_vtol
-	13002_firefly6
 	13003_quad_tailsitter
 	13004_quad+_tailsitter
 	13005_vtol_AAERT_quad
 	13006_vtol_standard_delta
 	13007_vtol_AAVVT_quad
-	13008_QuadRanger
-	13009_vtol_spt_ranger
-	13012_convergence
+	13100_generic_vtol_tiltrotor
 	13013_deltaquad
 	13014_vtol_babyshark
 	13030_generic_vtol_quad_tiltrotor
-	13050_generic_vtol_octo
 	13200_generic_vtol_tailsitter
 
 	# [14000, 14999] Tri Y
-	14001_tri_y_yaw+
-	14002_tri_y_yaw-
 
-	15001_coax_heli
 
 	16001_helicopter
 
 	# [17000, 17999] Autogyro
-	17002_TF-AutoG2
-	17003_TF-G2
+
 
 	# [18000, 18999] High-altitude balloons
 	18001_TF-B1
 
 	# [22000, 22999] Reserve for custom models
 
-	24001_dodeca_cox
-
 	50000_generic_ground_vehicle
-	50003_aion_robotics_r1_rover
-	50004_nxpcup_car_dfrobot_gpx
 
 	# [60000, 61000] (Unmanned) Underwater Robots
 	60000_uuv_generic
diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_defaults b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
index 6cae25306e..cf91ebcdac 100644
--- a/ROMFS/px4fmu_common/init.d/rc.fw_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.fw_defaults
@@ -14,7 +14,7 @@ param set-default MAV_TYPE 1
 # Default parameters for fixed wing UAVs.
 #
 param set-default COM_POS_FS_DELAY 5
-param set-default COM_POS_FS_EPH 15
+param set-default COM_POS_FS_EPH 50
 param set-default COM_POS_FS_EPV 30
 param set-default COM_VEL_FS_EVH 5
 # Disable preflight disarm to not interfere with external launching
diff --git a/ROMFS/px4fmu_common/init.d/rc.heli_defaults b/ROMFS/px4fmu_common/init.d/rc.heli_defaults
new file mode 100644
index 0000000000..42d2aca268
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/rc.heli_defaults
@@ -0,0 +1,17 @@
+#!/bin/sh
+#
+# Helicopter default parameters.
+#
+# NOTE: Script variables are declared/initialized/unset in the rcS script.
+#
+
+# Inherit from mc
+. ${R}etc/init.d/rc.mc_defaults
+
+param set-default MAV_TYPE 4
+
+param set-default COM_PREARM_MODE 2
+param set-default COM_SPOOLUP_TIME 10
+
+# No need for minimum collective pitch (or airmode) to keep torque authority
+param set-default MPC_MANTHR_MIN 0
diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface
index fee6ad252c..09e92d679a 100644
--- a/ROMFS/px4fmu_common/init.d/rc.interface
+++ b/ROMFS/px4fmu_common/init.d/rc.interface
@@ -32,14 +32,17 @@ fi
 
 #
 # If mount (gimbal) control is enabled and output mode is AUX, set the aux
-# mixer to mount (override the airframe-specific MIXER_AUX setting).
+# mixer to mount if no other aux mixer has been set.
 #
 if param greater -s MNT_MODE_IN -1
 then
-        if param compare -s MNT_MODE_OUT 0
-        then
-                set MIXER_AUX mount
-        fi
+	if param compare -s MNT_MODE_OUT 0
+	then
+		if [ $MIXER_AUX = none ]
+		then
+			set MIXER_AUX mount
+		fi
+	fi
 fi
 
 #
diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors
index 2694eb3c64..378d075519 100644
--- a/ROMFS/px4fmu_common/init.d/rc.sensors
+++ b/ROMFS/px4fmu_common/init.d/rc.sensors
@@ -129,11 +129,7 @@ fi
 # Sensirion SDP3X differential pressure sensor external I2C
 if param compare -s SENS_EN_SDP3X 1
 then
-	if ! sdp3x start -X
-	then
-		# try another common address
-		sdp3x start -X -a 0x22
-	fi
+	sdp3x start -X -k
 fi
 
 # TE MS4515 differential pressure sensor external I2C
diff --git a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
index 5440ee5a70..7b1e5eec6a 100644
--- a/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
+++ b/ROMFS/px4fmu_common/init.d/rc.vtol_defaults
@@ -10,6 +10,8 @@ set VEHICLE_TYPE vtol
 # MAV_TYPE_VTOL_FIXEDROTOR 22
 param set-default MAV_TYPE 22
 
+param set-default COM_POS_FS_EPH 50
+
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_YAW_TMT 10
 
@@ -21,16 +23,20 @@ param set-default HTE_VXY_THR 2.0
 param set-default MIS_DIST_WPS 5000
 
 param set-default MPC_ACC_HOR_MAX 2
-param set-default MPC_TKO_SPEED 1
-param set-default MPC_VEL_MANUAL 3
-param set-default MPC_XY_CRUISE 3
+param set-default MPC_VEL_MANUAL 5
+param set-default MPC_XY_CRUISE 5
 param set-default MPC_XY_ERR_MAX 5
-param set-default MPC_XY_VEL_MAX 4
-param set-default MPC_Z_VEL_MAX_DN 1.5
+param set-default MPC_XY_VEL_MAX 8
 param set-default MPC_JERK_MAX 4.5
 param set-default MPC_YAW_MODE 4
 
-param set-default NAV_ACC_RAD 3
+# reduce aggressiveness around roll and yaw axis,
+# as VTOLs usually have high intertia and lot af drag due to wings
+param set-default MC_ROLL_P 5
+param set-default MC_ROLLRATE_MAX 120
+param set-default MC_YAW_P 2
+param set-default MC_YAWRATE_MAX 120
+param set-default MPC_MAN_Y_MAX 90
 
 param set-default PWM_AUX_RATE 50
 param set-default PWM_MAIN_RATE 400
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index c0b9ce7f76..77900b3751 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -107,6 +107,16 @@ then
 		fi
 	fi
 
+	# Check for an update
+	if [ -e /fs/microsd/new ]
+	then
+		echo "Updating external autostart files"
+		# FIXME: add NuttX support for recursive rm, then we can remove the
+		# directory (for now this is done during updating via MAVLink FTP)
+		#rm -r $SDCARD_EXT_PATH
+		mv /fs/microsd/new $SDCARD_EXT_PATH
+	fi
+
 	set PARAM_FILE /fs/microsd/params
 fi
 
@@ -206,6 +216,26 @@ else
 	fi
 	unset BOARD_RC_DEFAULTS
 
+	#
+	# Set parameters and env variables for selected SYS_AUTOSTART.
+	#
+	set AUTOSTART_PATH etc/init.d/rc.autostart
+	if ! param compare SYS_AUTOSTART 0
+	then
+		if param greater SYS_AUTOSTART 1000000
+		then
+			# Use external startup file
+			if [ $SDCARD_AVAILABLE = yes ]
+			then
+				set AUTOSTART_PATH etc/init.d/rc.autostart_ext
+			else
+				echo "ERROR [init] SD card not mounted - trying to load airframe from ROMFS"
+			fi
+		fi
+		. ${R}$AUTOSTART_PATH
+	fi
+	unset AUTOSTART_PATH
+
 	#
 	# Start the tone_alarm driver.
 	# Needs to be started after the parameters are loaded (for CBRK_BUZZER).
@@ -243,26 +273,6 @@ else
 	rgbled start -X -q
 	rgbled_ncp5623c start -X -q
 
-	#
-	# Set parameters and env variables for selected AUTOSTART.
-	#
-	set AUTOSTART_PATH etc/init.d/rc.autostart
-	if ! param compare SYS_AUTOSTART 0
-	then
-		if param greater SYS_AUTOSTART 1000000
-		then
-			# Use external startup file
-			if [ $SDCARD_AVAILABLE = yes ]
-			then
-				set AUTOSTART_PATH etc/init.d/rc.autostart_ext
-			else
-				echo "ERROR [init] SD card not mounted - trying to load airframe from ROMFS"
-			fi
-		fi
-		. ${R}$AUTOSTART_PATH
-	fi
-	unset AUTOSTART_PATH
-
 	#
 	# Override parameters from user configuration file.
 	#
@@ -303,7 +313,7 @@ else
 	then
 		# Check for the mini using build with px4io fw file
 		# but not a px4IO
-		if ver hwtypecmp V540 V560
+		if ver hwtypecmp V5004000 V5006000
 		then
 			param set SYS_USE_IO 0
 		else
@@ -399,6 +409,21 @@ else
 		commander start
 	fi
 
+	# PPS capture driver (before pwm_out)
+	if param greater -s PPS_CAP_ENABLE 0
+	then
+		pps_capture start
+	fi
+
+	# Camera capture driver (before pwm_out)
+	if param greater -s CAM_CAP_FBACK 0
+	then
+		if camera_capture start
+		then
+			camera_capture on
+		fi
+	fi
+
 	#
 	# Configure vehicle type specific parameters.
 	# Note: rc.vehicle_setup is the entry point for rc.interface,
@@ -441,21 +466,6 @@ else
 		rc_input start $RC_INPUT_ARGS
 	fi
 
-	# PPS capture driver (before pwm_out)
-	if param greater -s PPS_CAP_ENABLE 0
-	then
-		pps_capture start
-	fi
-
-	# Camera capture driver (before pwm_out)
-	if param greater -s CAM_CAP_FBACK 0
-	then
-		if camera_capture start
-		then
-			camera_capture on
-		fi
-	fi
-
 	#
 	# Play the startup tune (if not disabled or there is an error)
 	#
@@ -505,6 +515,12 @@ else
 		px4flow start -X &
 	fi
 
+	# Payload deliverer module if gripper is enabled
+	if param compare -s PD_GRIPPER_EN 1
+	then
+		payload_deliverer start
+	fi
+
 	#
 	# Optional board supplied extras: rc.board_extras
 	#
diff --git a/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt b/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt
index 60f6d12ff2..080215c979 100644
--- a/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt
+++ b/ROMFS/px4fmu_common/mixers-sitl/CMakeLists.txt
@@ -39,5 +39,6 @@ px4_add_romfs_files(
 	quad_x_vtol.main.mix
 	standard_vtol_sitl.main.mix
 	tiltrotor_sitl.main.mix
+	tiltrotor_tri_sitl.main.mix
 	uuv_x_sitl.main.mix
 )
diff --git a/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_tri_sitl.main.mix b/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_tri_sitl.main.mix
new file mode 100644
index 0000000000..5ba8071383
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers-sitl/tiltrotor_tri_sitl.main.mix
@@ -0,0 +1,38 @@
+Mixer for quad tiltrotor (x motor configuration)
+================================================
+
+This file defines a single mixer for a tiltrotor (SITL gazebo) with motors in X configuration. The plane has two ailerons and one elevator.
+
+R: 3y 10000 10000 10000 0
+
+# tilt servo motor 1 (right)
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 8      0  20000 -10000 -10000  10000
+S: 0 2 -10000 -10000       0 -10000  10000
+
+# tilt servo motor 2 (left)
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 8      0  20000 -10000 -10000  10000
+S: 0 2  10000  10000      0  -10000  10000
+
+# tilt servo motor 3 (rear)
+-------------
+M: 1
+S: 1 8      0  20000  -10000 -10000  10000
+
+# mixer for the left aileron
+M: 1
+O:      10000  10000   0 -10000  10000
+S: 1 0  -10000 -10000    0 -10000  10000
+
+# mixer for the right aileron
+M: 1
+O:      10000  10000   0 -10000  10000
+S: 1 0  10000  10000    0 -10000  10000
+
+# mixer for the elevator
+M: 1
+O:       10000   10000   0 -10000  10000
+S: 1 1   10000   10000    0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/CCPM.main.mix b/ROMFS/px4fmu_common/mixers/CCPM.main.mix
deleted file mode 100644
index 311a57285c..0000000000
--- a/ROMFS/px4fmu_common/mixers/CCPM.main.mix
+++ /dev/null
@@ -1,48 +0,0 @@
-Helicopter 120 degree Cyclic-Collective-Pitch Mixing (CCPM) for PX4FMU
-==================================================
-
-
-Output 0 - Rear Servo Mixer
-----------------
-
-Rear Servo = Collective (Thrust - 3) + Elevator (Pitch - 1)
-
-M: 2
-S: 0 3  10000  10000      0 -10000  10000
-S: 0 1  10000  10000      0 -10000  10000
-
-
-Output 1 - Left Servo Mixer
------------------
-Left Servo = Collective (Thurst - 3) - 0.5 * Elevator (Pitch - 1) + 0.866 * Aileron (Roll - 0)
-
-M: 3
-S: 0 3  -10000  -10000      0 -10000  10000
-S: 0 1   -5000   -5000      0 -10000  10000
-S: 0 0    8660    8660      0 -10000  10000
-
-
-Output 2 - Right Servo Mixer
-----------------
-Right Servo = Collective (Thurst - 3) - 0.5 * Elevator (Pitch - 1) - 0.866 * Aileron (Roll - 0)
-
-
-M: 3
-S: 0 3  -10000  -10000      0 -10000  10000
-S: 0 1   -5000   -5000      0 -10000  10000
-S: 0 0   -8660   -8660      0 -10000  10000
-
-Output 3 - Tail Servo Mixer
-----------------
-Tail Servo = Yaw (control index = 2)
-
-M: 1
-S: 0 2  10000  10000      0 -10000  10000
-
-
-Output 4 - Motor speed mixer
------------------
-This would be the motor speed control output from governor power demand- not sure what index to use here?
-
-M: 1
-S: 0 4      0  20000 -10000 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/CMakeLists.txt b/ROMFS/px4fmu_common/mixers/CMakeLists.txt
index 00903468fc..4065ff1b41 100644
--- a/ROMFS/px4fmu_common/mixers/CMakeLists.txt
+++ b/ROMFS/px4fmu_common/mixers/CMakeLists.txt
@@ -38,16 +38,11 @@ px4_add_romfs_files(
 	AERT.main.mix
 	AETRFG.main.mix
 	babyshark.main.mix
-	blade130.main.mix
-	CCPM.main.mix
 	cloudship.main.mix
-	coax.main.mix
 	delta.main.mix
 	deltaquad.main.mix
 	dodeca_bottom_cox.aux.mix
 	dodeca_top_cox.main.mix
-	firefly6.aux.mix
-	firefly6.main.mix
 	fw_generic_wing.main.mix
 	generic_diff_rover.main.mix
 	hexa_cox.main.mix
@@ -74,7 +69,6 @@ px4_add_romfs_files(
 	rover_diff_and_servo.main.mix
 	rover_generic.main.mix
 	standard_vtol_hitl.main.mix
-	TF-AutoG2.main.mix
 	tilt_quad.aux.mix
 	tilt_quad.main.mix
 	tri_y_yaw+.main.mix
@@ -84,8 +78,6 @@ px4_add_romfs_files(
 	vtol_AAERT.aux.mix
 	vtol_AAVVT.aux.mix
 	vtol_TTTTAAER.aux.mix
-	vtol_convergence.main.mix
 	vtol_delta.aux.mix
 	vtol_tailsitter_duo.main.mix
-	vtol_tailsitter_duo_sat.main.mix
 )
diff --git a/ROMFS/px4fmu_common/mixers/TF-AutoG2.main.mix b/ROMFS/px4fmu_common/mixers/TF-AutoG2.main.mix
deleted file mode 100644
index 23c146904b..0000000000
--- a/ROMFS/px4fmu_common/mixers/TF-AutoG2.main.mix
+++ /dev/null
@@ -1,56 +0,0 @@
-Aileron/rudder/elevator/throttle mixer for PX4FMU
-==================================================
-# @board px4_fmu-v2 exclude
-
-Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
-(roll), 1 (pitch) and 3 (thrust).
-
-CH1, 2: rotor-head mixer
--------------
-Two scalers total (output, roll).
-
-M: 2
-O: 10000 10000 0 -10000 10000
-S: 0 0 -10000 -10000 0 -10000 10000
-S: 0 1  10000  10000      0 -10000  10000
-
-
-M: 2
-O: 10000 10000 0 -10000 10000
-S: 0 0 -10000 -10000 0 -10000 10000
-S: 0 1 -10000 -10000      0 -10000  10000
-
-
-CH3: Elevator, prerotator mixer
-------------
-Two scalers total (output, roll).
-
-M: 1
-O: 7500 7500 0 -10000 10000
-S: 3 6 10000 10000 0 -10000 10000
-
-
-CH4, 5: Rudder mixer
-------------
-Two scalers total (output, yaw).
-
-M: 1
-S: 0 2  10000  10000      0 -10000  10000
-
-M: 1
-S: 0 2  -10000  -10000      0 -10000  10000
-
-CH4: Motor speed mixer
------------------
-Two scalers total (output, thrust).
-
-M: 1
-S: 0 3      0  20000 -10000 -10000  10000
-
-
-CH3: Wheel mixer
-------------
-Two scalers total (output, yaw).
-
-M: 1
-S: 0 2  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/TF-G2.main.mix b/ROMFS/px4fmu_common/mixers/TF-G2.main.mix
deleted file mode 100644
index 37acba1b55..0000000000
--- a/ROMFS/px4fmu_common/mixers/TF-G2.main.mix
+++ /dev/null
@@ -1,40 +0,0 @@
-TF-G2 autogyro mixer
-==================================================
-# @board px4_fmu-v2 exclude
-
-Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
-(roll), 1 (pitch) and 3 (thrust).
-
-CH1: empty - fixing cuav nano jittering of 1st output. 
-Z:
-
-
-CH2, 3: rotor-head mixer
--------------
-Two scalers total (output, roll).
-
-M: 2
-O: 10000 10000 0 -10000 10000
-S: 0 0 -10000 -10000 0 -10000 10000
-S: 0 1  10000  10000      0 -10000  10000
-
-M: 2
-O: 10000 10000 0 -10000 10000
-S: 0 0 -10000 -10000 0 -10000 10000
-S: 0 1 -10000 -10000      0 -10000  10000
-
-
-CH4: Rudder mixer
-------------
-Two scalers total (output, yaw).
-
-M: 1
-S: 0 2  10000  10000      0 -10000  10000
-
-
-CH5: Motor speed mixer
------------------
-Two scalers total (output, thrust).
-
-M: 1
-S: 0 3      0  20000 -10000 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/blade130.main.mix b/ROMFS/px4fmu_common/mixers/blade130.main.mix
deleted file mode 100644
index 3cbf49f510..0000000000
--- a/ROMFS/px4fmu_common/mixers/blade130.main.mix
+++ /dev/null
@@ -1,15 +0,0 @@
-Helicopter Cyclic-Collective-Pitch Mixing (CCPM) for PX4FMU
-Blade 130x helicopter has longer servo arms left and right. The front servo arm is shortest and has normalized length 10000.
-==================================================
-
-H: 3
-T:      0   3000   6000   8000  10000
-P:    500   1500   2500   3500   4500
-# Swash plate servos:
-S:      0  10000  10000      0  -8000   8000
-S:    140  13054  10000      0  -8000   8000
-S:    220  13054  10000      0  -8000   8000
-
-# Tail servo:
-M: 1
-S: 0 2  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/coax.main.mix b/ROMFS/px4fmu_common/mixers/coax.main.mix
deleted file mode 100644
index 0c99dd3b0b..0000000000
--- a/ROMFS/px4fmu_common/mixers/coax.main.mix
+++ /dev/null
@@ -1,29 +0,0 @@
-Coaxial helicopter mixer
-- Two servomotors act on the swashplate (90 degree angle on the swashplate, decoupled effect on roll and pitch).
-- No collective pitch.
-- One motor per rotor.
-===========================
-
-Left swashplate servomotor, pitch axis
--------------
-M: 1
-S: 0 1  -10000  -10000      0 -10000  10000
-
-Right swashplate servomotor, roll axis
--------------
-M: 1
-S: 0 0  10000  10000      0 -10000  10000
-
-Upper rotor (CCW)
-Mixing between yaw and thrust
--------------
-M: 2
-S: 0 2  10000  10000      0 -10000  10000
-S: 0 3      0  20000 -10000 -10000  10000
-
-Lower rotor (CW)
-Mixing between yaw and thrust
--------------
-M: 2
-S: 0 2  -10000  -10000      0 -10000  10000
-S: 0 3      0  20000 -10000 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix
deleted file mode 100644
index 9007a23b96..0000000000
--- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix
+++ /dev/null
@@ -1,24 +0,0 @@
-# mixer for the FireFly6 tilt mechansim servo, elevons and landing gear
-=======================================================================
-
-# @board px4_fmu-v2 exclude
-
-Tilt mechanism servo mixer
----------------------------
-M: 1
-S: 1 8      0  20000 -10000 -10000  10000
-
-Elevon mixers
--------------
-M: 2
-S: 1 0   7500   7500      0 -10000  10000
-S: 1 1   8000   8000      0 -10000  10000
-
-M: 2
-S: 1 0    7500    7500    0 -10000  10000
-S: 1 1   -8000   -8000    0 -10000  10000
-
-Landing gear mixer
-------------------
-M: 1
-S: 3 6  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/firefly6.main.mix b/ROMFS/px4fmu_common/mixers/firefly6.main.mix
deleted file mode 100644
index 59148528c6..0000000000
--- a/ROMFS/px4fmu_common/mixers/firefly6.main.mix
+++ /dev/null
@@ -1,6 +0,0 @@
-# FireFly6 mixer for PX4FMU
-#
-#===========================
-# @board px4_fmu-v2 exclude
-
-R: 6c
diff --git a/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo_sat.main.mix b/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo_sat.main.mix
deleted file mode 100644
index 2a9e3197c1..0000000000
--- a/ROMFS/px4fmu_common/mixers/vtol_tailsitter_duo_sat.main.mix
+++ /dev/null
@@ -1,39 +0,0 @@
-Tailsitter duo mixer
-============================
-
-This file defines a mixer for a generic duo tailsitter VTOL (eg TBS Caipirinha tailsitter edition). This vehicle
-has two motors in total, one attached to each wing. It also has two elevons which
-are located in the slipstream of the propellers. This mixer generates 4 PWM outputs
-on the main PWM ouput port, two at 400Hz for the motors, and two at 50Hz for the
-elevon servos. Channels 1-4 are configured to run at 400Hz, while channels 5-8 run
-at the default rate of 50Hz. Note that channels 3 and 4 are assigned but not used.
-
-Motor mixer
-------------
-Channel 1 connects to the right (starboard) motor.
-Channel 2 connects to the left (port) motor.
-
-R: 2-
-
-Zero mixer (2x)
----------------
-Channels 3,4 are unused.
-
-Z:
-
-Z:
-
-Elevons mixer
---------------
-Channel 5 connects to the right (starboard) elevon.
-Channel 6 connects to the left (port) elevon.
-Here we saturate the elevons before their full range
-to avoid roll-pitch-yaw coupling during faster maneuvers
-
-M: 2
-S: 1 0   10000   10000    0 -6000  6000
-S: 1 1   10000   10000    0 -6000  6000
-
-M: 2
-S: 1 0   10000   10000    0 -6000  6000
-S: 1 1  -10000  -10000    0 -6000  6000
diff --git a/src/lib/landing_slope/CMakeLists.txt b/ROMFS/recovery/CMakeLists.txt
similarity index 93%
rename from src/lib/landing_slope/CMakeLists.txt
rename to ROMFS/recovery/CMakeLists.txt
index 55f5940b78..bb65f2439f 100644
--- a/src/lib/landing_slope/CMakeLists.txt
+++ b/ROMFS/recovery/CMakeLists.txt
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2020 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,4 @@
 #
 ############################################################################
 
-px4_add_library(landing_slope Landingslope.cpp)
+add_subdirectory(init.d)
diff --git a/ROMFS/recovery/init.d/CMakeLists.txt b/ROMFS/recovery/init.d/CMakeLists.txt
new file mode 100644
index 0000000000..32a84764ad
--- /dev/null
+++ b/ROMFS/recovery/init.d/CMakeLists.txt
@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2020 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_romfs_files(
+	rcS
+)
diff --git a/ROMFS/recovery/init.d/rcS b/ROMFS/recovery/init.d/rcS
new file mode 100644
index 0000000000..7b68877db5
--- /dev/null
+++ b/ROMFS/recovery/init.d/rcS
@@ -0,0 +1,75 @@
+#!/bin/sh
+# Un comment and use set +e to ignore and set -e to enable 'exit on error control'
+set +e
+# Un comment the line below to help debug scripts by printing a trace of the script commands
+#set -x
+# PX4FMU startup script.
+#
+# NOTE: environment variable references:
+#    If the dollar sign ('$') is followed by a left bracket ('{') then the
+#    variable name is terminated with the right bracket character ('}').
+#    Otherwise, the variable name goes to the end of the argument.
+#
+#
+# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
+#
+#------------------------------------------------------------------------------
+set R /
+
+#
+# Start CDC/ACM serial driver.
+#
+sercon
+
+#
+# Print full system version.
+#
+ver all
+
+#
+# Set the parameter file the board supports params on
+# MTD device.
+#
+if mft query -q -k MTD -s MTD_PARAMETERS -v /fs/mtd_params
+then
+	set PARAM_FILE /fs/mtd_params
+fi
+
+#
+# Load parameters.
+#
+# if the board has a storage for (factory) calibration data
+if mft query -q -k MTD -s MTD_CALDATA -v /fs/mtd_caldata
+then
+	param load /fs/mtd_caldata
+fi
+
+#
+# Load parameters.
+#
+param select $PARAM_FILE
+if ! param load
+then
+	param reset_all
+fi
+
+#
+# board mavlink: rc.mavlink
+#
+set BOARD_RC_MAVLINK ${R}etc/init.d/rc.board_mavlink
+if [ -f $BOARD_RC_MAVLINK ]
+then
+	echo "Board sensors: ${BOARD_RC_MAVLINK}"
+	. $BOARD_RC_MAVLINK
+fi
+unset BOARD_RC_MAVLINK
+
+unset R
+
+mavlink boot_complete
+
+echo ""
+echo "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
+echo "!!!!!!     This is the RECOVERY firmware! WARNs and ERRORs are expected!      !!!!!"
+echo "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
+echo ""
diff --git a/Tools/auterion/remote_update_fmu.sh b/Tools/auterion/remote_update_fmu.sh
new file mode 100755
index 0000000000..e98667d2ec
--- /dev/null
+++ b/Tools/auterion/remote_update_fmu.sh
@@ -0,0 +1,97 @@
+#!/bin/bash
+# Flash PX4 to a device running AOS in the local network
+if [ "$1" == "-h" ] || [ "$1" == "--help" ] || [ $# -lt 2 ]; then
+	echo "Usage: $0 -f <firmware.px4|.elf> [-c <configuration_dir>] -d <IP/Device> [-u <user>] [-p <ssh_port>]"
+	exit 1
+fi
+
+ssh_port=22
+ssh_user=root
+
+while getopts ":f:c:d:p:u:" opt; do
+	case ${opt} in
+		f )
+			if [ -n "$OPTARG" ]; then
+				firmware_file="$OPTARG"
+			else
+				echo "ERROR: -f requires a non-empty option argument."
+				exit 1
+			fi
+			;;
+		c )
+			if [ -f "$OPTARG/rc.autostart" ]; then
+				config_dir="$OPTARG"
+			else
+				echo "ERROR: -c configuration directory is empty or does not contain a valid rc.autostart"
+				exit 1
+			fi
+			;;
+		d )
+			if [ "$OPTARG" ]; then
+				device="$OPTARG"
+			else
+				echo "ERROR: -d requires a non-empty option argument."
+				exit 1
+			fi
+			;;
+		p )
+			if [[ "$OPTARG" =~ ^[0-9]+$ ]]; then
+				ssh_port="$OPTARG"
+			else
+				echo "ERROR: -p ssh_port must be a number."
+				exit 1
+			fi
+			;;
+		u )
+			if [ "$OPTARG" ]; then
+				ssh_user="$OPTARG"
+			else
+				echo "ERROR: -u requires a non-empty option argument."
+				exit 1
+			fi
+			;;
+	esac
+done
+
+target_dir=/shared_container_dir/fmu
+
+# create update-dev.tar
+target_file_name="update-dev.tar"
+tmp_dir="$(mktemp -d)"
+config_path=""
+firmware_path=""
+
+if [ -d "$config_dir" ]; then
+	cp -r "$config_dir" "$tmp_dir/config"
+	config_path=config
+fi
+
+if [ -f "$firmware_file" ]; then
+	extension="${firmware_file##*.}"
+	cp "$firmware_file" "$tmp_dir/firmware.$extension"
+	if [ "$extension" == "elf" ]; then
+		# ensure the file is stripped to reduce file size
+		arm-none-eabi-strip "$tmp_dir/firmware.$extension"
+	fi
+	firmware_path="firmware.$extension"
+fi
+
+if [ -z "$device" ]; then
+	echo "Error: missing device"
+	exit 1
+fi
+
+pushd "$tmp_dir" &>/dev/null
+
+if [ -z $firmware_path ] && [ -z $config_path ]; then
+	exit 1
+fi
+
+tar -C "$tmp_dir" --sort=name --owner=root:0 --group=root:0 --mtime='2019-01-01 00:00:00' -cvf $target_file_name $firmware_path $config_path
+scp -P $ssh_port "$target_file_name" $ssh_user@"$device":$target_dir
+popd &>/dev/null
+rm -rf "$tmp_dir"
+
+# grab status output
+cmd="tail --follow=name $target_dir/update_status 2>/dev/null || true"
+ssh -t -p $ssh_port $ssh_user@$device "$cmd"
diff --git a/Tools/auterion/upload_skynode.sh b/Tools/auterion/upload_skynode.sh
new file mode 100755
index 0000000000..23bf593166
--- /dev/null
+++ b/Tools/auterion/upload_skynode.sh
@@ -0,0 +1,36 @@
+#!/usr/bin/env bash
+
+DIR="$(dirname $(readlink -f $0))"
+PX4_BINARY_FILE="$1"
+DEFAULT_AUTOPILOT_HOST=10.41.0.1
+DEFAULT_AUTOPILOT_PORT=33333
+DEFAULT_AUTOPILOT_USER=auterion
+
+for i in "$@"
+do
+    case $i in
+        --default-ip=*)
+        DEFAULT_AUTOPILOT_HOST="${i#*=}"
+        ;;
+        --default-port=*)
+        DEFAULT_AUTOPILOT_PORT="${i#*=}"
+        ;;
+        --default-user=*)
+        DEFAULT_AUTOPILOT_USER="${i#*=}"
+        ;;
+        *)
+            # unknown option
+        ;;
+    esac
+done
+
+# allow these to be overridden
+[ -z "$AUTOPILOT_HOST" ] && AUTOPILOT_HOST=$DEFAULT_AUTOPILOT_HOST
+[ -z "$AUTOPILOT_PORT" ] && AUTOPILOT_PORT=$DEFAULT_AUTOPILOT_PORT
+[ -z "$AUTOPILOT_USER" ] && AUTOPILOT_USER=$DEFAULT_AUTOPILOT_USER
+
+echo "Uploading to $AUTOPILOT_HOST..."
+
+"$DIR"/remote_update_fmu.sh -f "$PX4_BINARY_FILE" -d "$AUTOPILOT_HOST" -p $AUTOPILOT_PORT -u $AUTOPILOT_USER
+
+exit 0
diff --git a/Tools/gazebo_sitl_multiple_run.sh b/Tools/gazebo_sitl_multiple_run.sh
index 292272e2bd..3deb3c8b25 100755
--- a/Tools/gazebo_sitl_multiple_run.sh
+++ b/Tools/gazebo_sitl_multiple_run.sh
@@ -6,10 +6,17 @@
 # For example gazebo can be run like this:
 #./Tools/gazebo_sitl_multiple_run.sh -n 10 -m iris
 
+SUPPORTED_MODELS=("iris" "plane" "standard_vtol" "rover" "r1_rover" "typhoon_h480", "shieldai_nova2", "skydio_x2d")
+
 function cleanup() {
 	pkill -x px4
-	pkill gzclient
 	pkill gzserver
+
+	if [[ -n "$HEADLESS" ]]; then
+		exit
+	else
+		pkill gzclient
+	fi
 }
 
 function spawn_model() {
@@ -20,10 +27,9 @@ function spawn_model() {
 	X=${X:=0.0}
 	Y=${Y:=$((3*${N}))}
 
-	SUPPORTED_MODELS=("iris" "plane" "standard_vtol" "rover" "r1_rover" "typhoon_h480")
 	if [[ " ${SUPPORTED_MODELS[*]} " != *"$MODEL"* ]];
 	then
-		echo "ERROR: Currently only vehicle model $MODEL is not supported!"
+		echo "ERROR: Currently vehicle model '$MODEL' is not supported!"
 		echo "       Supported Models: [${SUPPORTED_MODELS[@]}]"
 		trap "cleanup" SIGINT SIGTERM EXIT
 		exit 1
@@ -34,8 +40,19 @@ function spawn_model() {
 
 	pushd "$working_dir" &>/dev/null
 	echo "starting instance $N in $(pwd)"
+
+	if [[ -n "${PX4_VIDEO_HOST_IP}" ]]; then
+		export PX4_VIDEO_HOST_IP=${PX4_VIDEO_HOST_IP%.*}.$((7+$N))
+		echo "PX4_VIDEO_HOST_IP '$PX4_VIDEO_HOST_IP'"
+	fi
+
+	if [[ -n "${PX4_SIM_REMOTE_HOST}" ]]; then
+		export PX4_SIM_REMOTE_HOST=${PX4_SIM_REMOTE_HOST%.*}.$((7+$N))
+		echo "PX4_SIM_REMOTE_HOST '$PX4_SIM_REMOTE_HOST'"
+	fi
+
 	../bin/px4 -i $N -d "$build_path/etc" -w sitl_${MODEL}_${N} -s etc/init.d-posix/rcS >out.log 2>err.log &
-	python3 ${src_path}/Tools/sitl_gazebo/scripts/jinja_gen.py ${src_path}/Tools/sitl_gazebo/models/${MODEL}/${MODEL}.sdf.jinja ${src_path}/Tools/sitl_gazebo --mavlink_tcp_port $((4560+${N})) --mavlink_udp_port $((14560+${N})) --mavlink_id $((1+${N})) --gst_udp_port $((5600+${N})) --video_uri $((5600+${N})) --mavlink_cam_udp_port $((14530+${N})) --output-file /tmp/${MODEL}_${N}.sdf
+	python3 ${src_path}/Tools/sitl_gazebo/scripts/jinja_gen.py ${src_path}/Tools/sitl_gazebo/models/${MODEL}/${MODEL}.sdf.jinja ${src_path}/Tools/sitl_gazebo --mavlink_tcp_port $((4560+${N})) --mavlink_udp_port $((14560+${N})) --mavlink_id $((1+${N})) --gst_udp_host 172.5.0.$((7+${N})) --gst_udp_port $((5600)) --video_uri $((5600+${N})) --mavlink_cam_udp_port $((14530+${N})) --udp_onboard_gimbal_port_local $((13030+${N})) --output-file /tmp/${MODEL}_${N}.sdf --vehicle_id ${N}
 
 	echo "Spawning ${MODEL}_${N} at ${X} ${Y}"
 
@@ -47,12 +64,13 @@ function spawn_model() {
 
 if [ "$1" == "-h" ] || [ "$1" == "--help" ]
 then
-	echo "Usage: $0 [-n <num_vehicles>] [-m <vehicle_model>] [-w <world>] [-s <script>]"
+	echo "Usage: $0 [-n <num_vehicles>] [-m <vehicle_model>] [-w <world>] [-s <script>] [-c <custom_models>]"
 	echo "-s flag is used to script spawning vehicles e.g. $0 -s iris:3,plane:2"
+	echo "-c flag is used to allow custom models e.g. $0 -s \"my_plane my_plane_2\""
 	exit 1
 fi
 
-while getopts n:m:w:s:t:l: option
+while getopts n:m:w:s:t:c: option
 do
 	case "${option}"
 	in
@@ -61,7 +79,7 @@ do
 		w) WORLD=${OPTARG};;
 		s) SCRIPT=${OPTARG};;
 		t) TARGET=${OPTARG};;
-		l) LABEL=_${OPTARG};;
+		c) CUSTOM_MODELS=${OPTARG};;
 	esac
 done
 
@@ -71,6 +89,8 @@ target=${TARGET:=px4_sitl_default}
 vehicle_model=${VEHICLE_MODEL:="iris"}
 export PX4_SIM_MODEL=${vehicle_model}
 
+SUPPORTED_MODELS+=(${CUSTOM_MODELS})
+
 echo ${SCRIPT}
 SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
 src_path="$SCRIPT_DIR/.."
@@ -132,9 +152,19 @@ else
 			n=$(($n + 1))
 		done
 	done
-
 fi
-trap "cleanup" SIGINT SIGTERM EXIT
 
-echo "Starting gazebo client"
-gzclient
+if [[ -n "$HEADLESS" ]]; then
+	trap "cleanup" SIGINT SIGTERM
+
+	while :
+	do
+
+		  sleep 5
+	done
+else
+	trap "cleanup" SIGINT SIGTERM EXIT
+
+	echo "Starting gazebo client"
+	gzclient
+fi
diff --git a/Tools/px4moduledoc/srcparser.py b/Tools/px4moduledoc/srcparser.py
index 258ba0bfa4..a0df7aa43a 100644
--- a/Tools/px4moduledoc/srcparser.py
+++ b/Tools/px4moduledoc/srcparser.py
@@ -102,7 +102,7 @@ class ModuleDocumentation(object):
     def _handle_usage_param_int(self, args):
         assert(len(args) == 6) # option_char, default_val, min_val, max_val, description, is_optional
         option_char = self._get_option_char(args[0])
-        default_val = int(args[1], 0)
+        default_val = self._get_int(args[1])
         description = self._get_string(args[4])
         if self._is_bool_true(args[5]):
             self._usage_string += "     [-%s <val>]  %s\n" % (option_char, description)
@@ -214,6 +214,9 @@ class ModuleDocumentation(object):
             f = f[:-1]
         return float(f)
 
+    def _get_int(self, argument):
+        return int(eval(argument))
+
     def _is_string(self, argument):
         return len(argument) > 0 and argument[0] == '"'
 
@@ -307,6 +310,8 @@ class SourceParser(object):
             r'//.*?$|/\*.*?\*/|\'(?:\\.|[^\\\'])*\'|"(?:\\.|[^\\"])*"',
             re.DOTALL | re.MULTILINE)
 
+        self._define_pattern = re.compile(r'#define\s+(\w+?)[^\S\r\n]+(.+?)\s*?\n')
+
     def Parse(self, scope, contents):
         """
         Incrementally parse program contents and append all found documentations
@@ -316,6 +321,9 @@ class SourceParser(object):
         # remove comments from source
         contents = self._comment_remover(contents)
 
+        # replace preprocessor defines defined in file directly
+        contents = self._define_replacer(contents)
+
         extracted_function_calls = [] # list of tuples: (FUNC_NAME, list(ARGS))
 
         start_index = 0
@@ -379,6 +387,15 @@ class SourceParser(object):
                 return s
         return re.sub(self._comment_remove_pattern, replacer, text)
 
+    def _define_replacer(self, text):
+        """ check for C preprocesor #define in text and replace with argument"""
+        text = re.sub(r"\\\s*?\n"," ",text)
+        define_iter = self._define_pattern.finditer(text)
+        for define_pattern in define_iter:
+            text = re.sub(r"\b" +re.escape(str(define_pattern.groups()[0])) + r"\b", re.escape(str(define_pattern.groups()[1])), text)
+        return text
+
+
     def _do_consistency_check(self, contents, scope, module_doc):
         """
         check the documentation for consistency with the code (arguments to
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index b35e5bc372..bc60b20c21 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -570,7 +570,7 @@ class uploader(object):
         self.fw_maxsize = self.__getInfo(uploader.INFO_FLASH_SIZE)
 
     # upload the firmware
-    def upload(self, fw, force=False, boot_delay=None):
+    def upload(self, fw, force=False, boot_delay=None, boot_check=False):
         # Make sure we are doing the right thing
         start = time.time()
         if self.board_type != fw.property('board_id'):
@@ -603,9 +603,9 @@ class uploader(object):
                 print("sn: ", end='')
                 for byte in range(0, 12, 4):
                     x = self.__getSN(byte)
-                    x = x[::-1]  # reverse the bytes
                     self.sn = self.sn + x
-                    print(binascii.hexlify(x).decode('Latin-1'), end='')  # show user
+
+                print(binascii.hexlify(self.sn).decode('Latin-1'), end='')  # show user
                 print('')
                 print("chip: %08x" % self.__getCHIP())
 
@@ -667,6 +667,23 @@ class uploader(object):
 
         print("\nRebooting.", end='')
         self.__reboot()
+
+        if boot_check:
+            # check if application has booted
+            time.sleep(self.BOOT_CHECK_DELAY_s)
+            # this Sync is expected to fail, because jump to Px4 succeeded
+            btl_active = False
+            try:
+                # Test if bootloader answers
+                ret =self.__sync()
+                btl_active=True
+
+            except Exception as e :
+                btl_active = False
+
+            if btl_active:
+                raise Exception('Did not boot to PX4 Application, still in Bootloader')
+
         self.port.close()
         print(" Elapsed Time %3.3f\n" % (time.time() - start))
 
@@ -752,6 +769,7 @@ def main():
     parser.add_argument('--force', action='store_true', default=False, help='Override board type check, or silicon errata checks and continue loading')
     parser.add_argument('--boot-delay', type=int, default=None, help='minimum boot delay to store in flash')
     parser.add_argument('--use-protocol-splitter-format', action='store_true', help='use protocol splitter format for reboot')
+    parser.add_argument('--boot-check', action='store_true',default=False, help='Test if bootloader has exited, after boot. Throws an excpetion if bootloader does not jump to application')
     parser.add_argument('firmware', action="store", help="Firmware file to be uploaded")
     args = parser.parse_args()
 
diff --git a/Tools/setup_gazebo.bash b/Tools/setup_gazebo.bash
index f11cc26218..cb2b9de59a 100755
--- a/Tools/setup_gazebo.bash
+++ b/Tools/setup_gazebo.bash
@@ -17,8 +17,9 @@ BUILD_DIR=$2
 
 # setup Gazebo env and update package path
 export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:${BUILD_DIR}/build_gazebo
-export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:${SRC_DIR}/Tools/sitl_gazebo/models
+export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:${SRC_DIR}/Tools/sitl_gazebo/models:${SRC_DIR}/Tools/auterion/models
 export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:${BUILD_DIR}/build_gazebo
+export GAZEBO_MODEL_DATABASE_URI=http://simulation-models.tools.auterion.dev/
 
 echo -e "GAZEBO_PLUGIN_PATH $GAZEBO_PLUGIN_PATH"
 echo -e "GAZEBO_MODEL_PATH $GAZEBO_MODEL_PATH"
diff --git a/Tools/signtool.py b/Tools/signtool.py
new file mode 100644
index 0000000000..c11036ebd7
--- /dev/null
+++ b/Tools/signtool.py
@@ -0,0 +1,349 @@
+#!/usr/bin/env python3
+from os import EX_CANTCREAT
+import nacl.encoding
+import nacl.signing
+import nacl.hash
+import struct
+import zlib
+import json
+import base64
+from pathlib import Path
+from dataclasses import dataclass
+import cryptotools
+import argparse
+import os
+
+# Dictionary describing the possible TOC flags, they can be OR'ed together
+toc_flag_dict = {
+    'TOC_FLAG1_BOOT': 0x1,
+    'TOC_FLAG1_VTORS': 0x2,
+    'TOC_FLAG1_CHECK_SIGNATURE': 0x4,
+    'TOC_FLAG1_DECRYPT': 0x8,
+    'TOC_FLAG1_COPY': 0x10,
+    'TOC_FLAG1_RDCT': 0x80,
+}
+
+# Dataclasses describing the TOC data, used to parse the to and from binary
+
+
+@dataclass
+class TOC_start:
+    start_magic: str = None
+    version: int = None
+    STRUCT_STRUCTURE = "<4sI"  # type defininig string from the STRUCT package
+
+# Dataclass to describe the TOC for 2 entries, one describing the signature typen
+# and the second one describing the signature location.
+
+
+@dataclass
+class TOC_entry:
+    toc_position: int = 0
+    app_name: str = None
+    app_start: int = None
+    app_end: int = None
+    app_target: int = None
+    app_signature_idx: int = None
+    app_signature_key: int = None
+    app_encryption_key: int = None
+    app_flags1: int = None
+    app_reserved: int = None
+    sig_name: str = None
+    sig_start: int = None
+    sig_end: int = None
+    sig_target: int = None
+    sig_signature_idx: int = None
+    sig_signature_key: int = None
+    sig_encryption_key: int = None
+    sig_flags1: int = None
+    sig_reserved: int = None
+    # type defininig string from the STRUCT package
+    STRUCT_STRUCTURE = "<4sIIIBBBBI4sIIIBBBBI"
+
+
+def toc2bin(data):
+    '''
+    Takes as TOC data class and converts it to the binary representation.
+    Prepare a TOC_entry data class with values and hand it over to this function.
+
+    data: is a dataclass TOC_entry
+    retrun: a packed binary to add to the px4 bin file
+    '''
+    return struct.pack(data.STRUCT_STRUCTURE,
+                       data.app_name, data.app_start,
+                       data.app_end, data.app_target,
+                       data.app_signature_idx,
+                       data.app_signature_key,
+                       data.app_encryption_key,
+                       data.app_flags1,
+                       data.app_reserved,
+                       data.sig_name, data.sig_start,
+                       data.sig_end, data.sig_target,
+                       data.sig_signature_idx,
+                       data.sig_signature_key,
+                       data.sig_encryption_key,
+                       data.sig_flags1,
+                       data.sig_reserved)
+
+
+def bin2toc_start(bin):
+    '''
+    Takes binary data and unpacks a Toc_start header.
+
+    bin: binary data to parse for the TOC header.
+
+    return: a dataclass TOC_start
+    '''
+    data = TOC_start()
+    (data.start_magic, data.version) = struct.unpack(data.STRUCT_STRUCTURE, bin)
+    return data
+
+
+def bin2toc_entry(bin):
+    '''
+    Takes binary data and unpacks it into a TOC entry dataclass.
+    bin: Binary data to unpack the TOC entry from.
+
+    return: A dataclass of TOC_entry
+    '''
+
+    data = TOC_entry()
+    (data.app_name, data.app_start,
+     data.app_end, data.app_target,
+     data.app_signature_idx,
+     data.app_signature_key,
+     data.app_encryption_key,
+     data.app_flags1,
+     data.app_reserved,
+     data.sig_name, data.sig_start,
+     data.sig_end, data.sig_target,
+     data.sig_signature_idx,
+     data.sig_signature_key,
+     data.sig_encryption_key,
+     data.sig_flags1,
+     data.sig_reserved
+     ) = struct.unpack(data.STRUCT_STRUCTURE, bin)
+
+    return data
+
+
+def parse_toc(bin):
+    '''
+    Searches for the TOC in the binary data.
+    This function looks for a TOC within certain boundaries in the TOC. It
+    also checks to find a valid TOC_end.
+    Throws exceptions, if a toc is not found, wrong version or end is not found.
+
+    bin: Binary data to look for the TOC
+
+    return: dataclass with the TOC entry
+    '''
+
+    # This is a fixed address from the linker file, TOC is placed after.
+    BOOT_DELAY_ADDR = 0x200
+    TOC_LEN_MAX = 64
+    EXPECTED_TOC_VERSION = 1
+
+    toc_start_len = struct.calcsize(TOC_start().STRUCT_STRUCTURE)
+    toc_entry_len = struct.calcsize(TOC_entry().STRUCT_STRUCTURE)
+
+    start_indx = bin.find(b'TOC', BOOT_DELAY_ADDR,
+                          BOOT_DELAY_ADDR + TOC_LEN_MAX)
+    if start_indx <= 0:
+        raise Exception('TOC not found')
+
+    toc_start_header = bin2toc_start(
+        bin[start_indx:(start_indx+toc_start_len)])
+
+    print('TOC start found', toc_start_header,'@: ',hex(start_indx))
+
+    if toc_start_header.version != EXPECTED_TOC_VERSION:
+        raise Exception('Wrong TOC version!')
+
+    t = bin2toc_entry(
+        bin[start_indx+toc_start_len: (start_indx+toc_start_len+toc_entry_len)])
+    t.toc_position = start_indx+toc_start_len
+    print(t)
+
+    indx = bin.find(b'END', start_indx, start_indx + 512)
+    if indx <= 0:
+        toc_end = False
+        raise Exception('TOC end  not found')
+
+    return t
+
+
+def unpackPx4(file_path):
+    '''
+    Unpacks a .px4 file to get access to its binary data.
+
+    filepath: Path to a px4 file to extract.
+
+    return: A tuple of (binary data, json_data) of the file.
+
+    '''
+    # read the file
+    with open(file_path, "r") as f:
+        desc = json.load(f)
+    image = bytearray(zlib.decompress(base64.b64decode(desc['image'])))
+    return image, desc
+
+
+def packPx4(bin_image, json_data, file_path, pub_key):
+    '''
+    Packs a new .px4 file with given binary data and information from jason file.
+
+    bin_image: New binary data with signature to add to .px4 file
+    json_data: The json data from the previously parsed .px4.
+    file_path: File path to then new .px4.sec image.
+
+    return: Nothing
+    '''
+
+    head,tail=os.path.splitext(file_path)
+    with open(head +'_signed.px4', 'w') as f:
+        json_data['signed'] = 'Hash512_Ed25519'
+        json_data['pub_key'] = pub_key.decode('utf-8')
+        json_data['image_size'] = len(bin_image)
+        json_data['image'] = base64.b64encode(
+            zlib.compress(bin_image, 9)).decode('utf-8')
+        print('Pack new signed.px4 file with signature')
+        json.dump(json_data, f, indent=4)
+
+
+def ed25519_sign(private_key, signee_bin):
+    """
+    This function creates the signature. It takes the private key and the binary file
+    and returns the tuple (signature, public key)
+    """
+
+    signing_key = nacl.signing.SigningKey(
+        private_key, encoder=nacl.encoding.HexEncoder)
+
+    # Sign a message with the signing key
+    signed = signing_key.sign(signee_bin, encoder=nacl.encoding.RawEncoder)
+
+    # Obtain the verify key for a given signing key
+    verify_key = signing_key.verify_key
+
+    # Serialize the verify key to send it to a third party
+    verify_key_hex = verify_key.encode(encoder=nacl.encoding.HexEncoder)
+
+    return signed.signature, verify_key_hex
+
+
+def write_toc(toc_old, bin, signature_name, new_pub_key_index, new_flags):
+    '''
+    Writes a new TOC entry.
+    toc_old: The parsed data_class with the data of the old TOC
+    bin: The binary to insert the new TOC
+    signature_name: 4 char name of the newly added signature
+    new_pub_key_index: Key index of the public key belongs to the signature
+    new_flags: New Toc flags from toc_flag_dict dictionary.
+
+    return: New bin with modified TOC
+    '''
+
+    toc_new = toc_old
+    toc_new.app_signature_key = new_pub_key_index
+    toc_new.app_flags1 = new_flags
+    if len(signature_name) != 4:
+        raise Exception('Signature name has not the right length')
+    toc_new.app_name = bytes(signature_name, 'utf_8')
+    toc_new_bin = toc2bin(toc_new)
+
+    toc_entry_len = struct.calcsize(TOC_entry().STRUCT_STRUCTURE)
+    bin[toc_old.toc_position:toc_old.toc_position+toc_entry_len] = toc_new_bin
+
+    return bin
+
+
+def cli():
+    '''
+    Comand lined interface to the signtool.
+    See usage comand.
+
+    return: class with parsed arguments.
+    '''
+
+    parser = argparse.ArgumentParser(
+        description='Tool to extract and find crypto TOC from .px4 file. And append a signature from a given private key')
+    # defining arguments for parser object
+    parser.add_argument("--signee", type=str,
+                        metavar="file_path", default=None,
+                        help="Opens and reads the specified px4 file.")
+    parser.add_argument("--private_key", type=str,
+                        metavar="key string", default=None,
+                        help="Private key to sign the px4 image")
+    parser.add_argument("--key_index", type=int,
+                        metavar="Number", default=None,
+                        help="Index of the public key used to verify the binary")
+
+    parser.add_argument("--TOC_flags", type=str, choices=['TOC_FLAG1_BOOT', 'TOC_FLAG1_RDCT'],
+                        default=None,
+                        help="TOC flags to indicate signature")
+    args = parser.parse_args()
+    return args
+
+
+def sign(file_path, private_key, key_index, TOC_flags):
+    '''
+    Signs a binary file and updates TOC accordingly.
+    Reads a .px4 or a .bin at specified location and writes a new _signed.[px4|bin] file at the same location.
+
+    file_path: Path to a .px4 or .bin file to sign.
+    private_key: String of the private key used to sign the binary.
+    key_index: Index of the public key used to verify signature.
+    TOC_flags: New toc flags to be written.
+    '''
+    head,tail=os.path.splitext(file_path)
+    if(tail == '.px4'):
+        bin, json_data = unpackPx4(file_path)
+    elif(tail == '.bin'):
+        with open(file_path,mode='rb') as f:
+            bin =bytearray(f.read())
+    else:
+        raise Exception('Error: Unknown file type')
+
+
+    toc_old = parse_toc(bin)
+    new_bin = write_toc(toc_old, bin, 'MSTR', key_index,
+                        toc_flag_dict[TOC_flags])
+    app_len = toc_old.app_end - toc_old.app_start
+    print('Calculate new signature')
+    signature, pub_key = cryptotools.ed25519_sign(
+        private_key, bytes(new_bin[0:app_len]))
+    # Append signature to binary
+    print('Append signature to binary: ',signature.hex(),'\n','length: ',app_len )
+
+    sig_start=toc_old.sig_start - toc_old.app_start
+    sig_end = toc_old.sig_end - toc_old.app_start
+
+    if sig_start != app_len:
+        padding = bytearray(b'\xff')*(sig_start - app_len)
+        new_bin += padding
+
+    new_bin[toc_old.sig_start-toc_old.app_start:toc_old.sig_end -
+            toc_old.app_start] = signature
+
+    if(tail == '.px4'):
+        packPx4(new_bin, json_data, file_path,pub_key)
+    elif(tail == '.bin'):
+        head,tail=os.path.splitext(file_path)
+        with open(head +'_signed.bin',mode='wb') as f:
+            f.write(bin)
+    else:
+        raise Exception('Error: Unknown file type!')
+
+
+
+if(__name__ == "__main__"):
+    args = cli()
+    # test input to the sign function if not run from CLI
+    # sign('PX4-Autopilot/build/px4_fmu-v5x_default/px4_fmu-v5x_default.px4',
+    #      "8e969454c3f1ba924b4d436bc58b87d20c082ea99fb6a77b9e00f7b20dab0fd8",
+    #      0, 'TOC_FLAG1_BOOT')
+    # CLI test input:
+    # --signee /PX4-Autopilot/build/px4_fmu-v5x_default/px4_fmu-v5x_default.px4
+    #  --private_key 8e969454c3f1ba924b4d436bc58b87d20c082ea99fb6a77b9e00f7b20dab0fd8 --key_index 0 --TOC_flags TOC_FLAG1_BOOT
+    sign(args.signee, args.private_key, args.key_index, args.TOC_flags)
diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo
index 5610c3fb44..fd8637d32e 160000
--- a/Tools/sitl_gazebo
+++ b/Tools/sitl_gazebo
@@ -1 +1 @@
-Subproject commit 5610c3fb441a2f3babc8ad7a63c8c4ce3e40abfa
+Subproject commit fd8637d32e9b743733a1a2ee7a92e742854eaf42
diff --git a/boards/atl/mantis-edu/default.px4board b/boards/atl/mantis-edu/default.px4board
index 4116386115..789c8877a2 100644
--- a/boards/atl/mantis-edu/default.px4board
+++ b/boards/atl/mantis-edu/default.px4board
@@ -51,3 +51,5 @@ CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
 CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_FW_ATT_CONTROL=y
diff --git a/boards/atl/mantis-edu/src/board_config.h b/boards/atl/mantis-edu/src/board_config.h
index 2342446c13..8b8a99b12e 100644
--- a/boards/atl/mantis-edu/src/board_config.h
+++ b/boards/atl/mantis-edu/src/board_config.h
@@ -115,9 +115,7 @@
 #define GPIO_HW_REV_SENSE    /* PC3   */ ADC1_GPIO(13)
 #define GPIO_HW_VER_DRIVE    /* PG0   */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_VER_SENSE    /* PC2   */ ADC1_GPIO(12)
-#define HW_INFO_INIT         {'V','5','x', 'x',0}
-#define HW_INFO_INIT_VER     2
-#define HW_INFO_INIT_REV     3
+#define HW_INFO_INIT_PREFIX    "VAM"
 
 #define BOARD_TAP_ESC_MODE 2 // select closed-loop control mode for the esc
 // #define BOARD_USE_ESC_CURRENT_REPORT
diff --git a/boards/av/x-v1/src/CMakeLists.txt b/boards/av/x-v1/src/CMakeLists.txt
index 1eccb518cc..c8e0738015 100644
--- a/boards/av/x-v1/src/CMakeLists.txt
+++ b/boards/av/x-v1/src/CMakeLists.txt
@@ -41,6 +41,7 @@ add_library(drivers_board
 
 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
diff --git a/boards/bitcraze/crazyflie/src/mtd.cpp b/boards/bitcraze/crazyflie/src/mtd.cpp
index 2255e0d60a..2b00e231b2 100644
--- a/boards/bitcraze/crazyflie/src/mtd.cpp
+++ b/boards/bitcraze/crazyflie/src/mtd.cpp
@@ -72,7 +72,9 @@ static const px4_mft_entry_s mtd_mft = {
 
 static const px4_mft_s mft = {
 	.nmft = 1,
-	.mfts = &mtd_mft
+	.mfts = {
+		&mtd_mft
+	}
 };
 
 const px4_mft_s *board_get_manifest(void)
diff --git a/boards/cuav/nora/src/CMakeLists.txt b/boards/cuav/nora/src/CMakeLists.txt
index 2c040010bd..a218bdd4f3 100644
--- a/boards/cuav/nora/src/CMakeLists.txt
+++ b/boards/cuav/nora/src/CMakeLists.txt
@@ -56,6 +56,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led
diff --git a/boards/cuav/x7pro/src/CMakeLists.txt b/boards/cuav/x7pro/src/CMakeLists.txt
index 2c040010bd..a218bdd4f3 100644
--- a/boards/cuav/x7pro/src/CMakeLists.txt
+++ b/boards/cuav/x7pro/src/CMakeLists.txt
@@ -56,6 +56,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led
diff --git a/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin
index b91f26a198..3eab1b7bde 100755
Binary files a/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin and b/boards/cubepilot/cubeorange/extras/cubepilot_io-v2_default.bin differ
diff --git a/boards/cubepilot/cubeorange/src/CMakeLists.txt b/boards/cubepilot/cubeorange/src/CMakeLists.txt
index eacffd5702..c8e7699046 100644
--- a/boards/cubepilot/cubeorange/src/CMakeLists.txt
+++ b/boards/cubepilot/cubeorange/src/CMakeLists.txt
@@ -55,6 +55,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			drivers__led
 			nuttx_arch
diff --git a/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin
index b91f26a198..3eab1b7bde 100755
Binary files a/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin and b/boards/cubepilot/cubeyellow/extras/cubepilot_io-v2_default.bin differ
diff --git a/boards/cubepilot/cubeyellow/src/CMakeLists.txt b/boards/cubepilot/cubeyellow/src/CMakeLists.txt
index db075c83d3..ce7742dad7 100644
--- a/boards/cubepilot/cubeyellow/src/CMakeLists.txt
+++ b/boards/cubepilot/cubeyellow/src/CMakeLists.txt
@@ -42,6 +42,7 @@ add_library(drivers_board
 
 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
diff --git a/boards/holybro/can-gps-v1/src/board_config.h b/boards/holybro/can-gps-v1/src/board_config.h
index dcf5a9e3b5..f9acbad506 100644
--- a/boards/holybro/can-gps-v1/src/board_config.h
+++ b/boards/holybro/can-gps-v1/src/board_config.h
@@ -83,9 +83,9 @@
 #define GPIO_HW_VER_REV_DRIVE /* PB1 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN1)
 #define GPIO_HW_REV_SENSE     /* PA0 */ ADC1_GPIO(0)
 #define GPIO_HW_VER_SENSE     /* PA1 */ ADC1_GPIO(1)
-#define HW_INFO_INIT         {'C','A','N','G','P','S','x', 'x',0}
-#define HW_INFO_INIT_VER     6
-#define HW_INFO_INIT_REV     7
+#define HW_INFO_INIT_PREFIX         "CANGPS"
+
+#define CANGPS00   HW_VER_REV(0x0,0x0) // CANGPS
 
 #define FLASH_BASED_PARAMS
 
diff --git a/boards/holybro/can-gps-v1/src/manifest.c b/boards/holybro/can-gps-v1/src/manifest.c
index 1beb74589a..695cdcf108 100644
--- a/boards/holybro/can-gps-v1/src/manifest.c
+++ b/boards/holybro/can-gps-v1/src/manifest.c
@@ -104,7 +104,7 @@ static const px4_hw_mft_item_t hw_mft_list_v0000[] = {
 
 static px4_hw_mft_list_entry_t mft_lists[] = {
 // ver/rev
-	{0x0000, hw_mft_list_v0000,        arraySize(hw_mft_list_v0000)},
+	{CANGPS00, hw_mft_list_v0000,        arraySize(hw_mft_list_v0000)},
 };
 
 /************************************************************************************
@@ -127,7 +127,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
 
 	if (boards_manifest == px4_hw_mft_list_uninitialized) {
-		uint32_t ver_rev = board_get_hw_version() << 8;
+		uint32_t ver_rev = board_get_hw_version() << 16;
 		ver_rev |= board_get_hw_revision();
 
 		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
@@ -138,7 +138,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 		}
 
 		if (boards_manifest == px4_hw_mft_list_uninitialized) {
-			syslog(LOG_ERR, "[boot] Board %4"  PRIx32 " is not supported!\n", ver_rev);
+			syslog(LOG_ERR, "[boot] Board %08"  PRIx32 " is not supported!\n", ver_rev);
 		}
 	}
 
diff --git a/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin
index 36128d00f4..5140391401 100755
Binary files a/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin and b/boards/holybro/durandal-v1/extras/px4_io-v2_default.bin differ
diff --git a/boards/holybro/durandal-v1/init/rc.board_sensors b/boards/holybro/durandal-v1/init/rc.board_sensors
index 9397359b7d..60519aad1a 100644
--- a/boards/holybro/durandal-v1/init/rc.board_sensors
+++ b/boards/holybro/durandal-v1/init/rc.board_sensors
@@ -11,13 +11,13 @@ icm20689 -R 2 -s start
 bmi088 -A -R 2 -s start
 bmi088 -G -R 2 -s start
 
-if ver hwtypecmp  VD00
+if ver hwtypecmp  VD000000
 then
 	# Internal SPI BMI088
 	bmi088 -A -R 2 -s start
 	bmi088 -G -R 2 -s start
 fi
-if ver hwtypecmp  VD01
+if ver hwtypecmp  VD000001
 then
 	# Internal SPI ICM-20602
 	icm20602 -R 2 -s start
diff --git a/boards/holybro/durandal-v1/src/CMakeLists.txt b/boards/holybro/durandal-v1/src/CMakeLists.txt
index 8eec4e70c6..c1386c4181 100644
--- a/boards/holybro/durandal-v1/src/CMakeLists.txt
+++ b/boards/holybro/durandal-v1/src/CMakeLists.txt
@@ -59,6 +59,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led # drv_led_start
diff --git a/boards/holybro/durandal-v1/src/board_config.h b/boards/holybro/durandal-v1/src/board_config.h
index 55a3b52238..638d3ea43b 100644
--- a/boards/holybro/durandal-v1/src/board_config.h
+++ b/boards/holybro/durandal-v1/src/board_config.h
@@ -152,14 +152,12 @@
 #define GPIO_HW_REV_SENSE    /* PC3   */ GPIO_ADC12_INP13
 #define GPIO_HW_VER_DRIVE    /* PG0   */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_VER_SENSE    /* PC2   */ GPIO_ADC123_INP12
-#define HW_INFO_INIT         {'V','D','x', 'x',0}
-#define HW_INFO_INIT_VER     2
-#define HW_INFO_INIT_REV     3
+#define HW_INFO_INIT_PREFIX         "VD"
 
 #define BOARD_NUM_SPI_CFG_HW_VERSIONS 2
 
-#define VD00   HW_VER_REV(0x0,0x0) // Durandal,  Ver 0        Rev 0
-#define VD01   HW_VER_REV(0x0,0x1) // Durandal,  Ver 0        Rev 1
+#define VD00000000   HW_VER_REV(0x0,0x0) // Durandal,  Ver 0        Rev 0
+#define VD00000001   HW_VER_REV(0x0,0x1) // Durandal,  Ver 0        Rev 1
 
 /* CAN Silence
  *
diff --git a/boards/holybro/durandal-v1/src/init.c b/boards/holybro/durandal-v1/src/init.c
index 527a8680ae..f8dc55d72f 100644
--- a/boards/holybro/durandal-v1/src/init.c
+++ b/boards/holybro/durandal-v1/src/init.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2019, 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
@@ -206,7 +206,6 @@ stm32_boardinitialize(void)
  *
  ****************************************************************************/
 
-
 __EXPORT int board_app_initialize(uintptr_t arg)
 {
 	/* Power on Interfaces */
@@ -219,6 +218,13 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 
 	px4_platform_init();
 
+	// Use the default HW_VER_REV(0x0,0x0) for Ramtron
+
+	stm32_spiinitialize();
+
+	/* Configure the HW based on the manifest */
+
+	px4_platform_configure();
 
 	if (OK == board_determine_hw_info()) {
 		syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(),
@@ -228,7 +234,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 		syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n");
 	}
 
-	/* configure SPI interfaces (after we determined the HW version) */
+	/* Configure the actual SPI interfaces (after we determined the HW version) */
 
 	stm32_spiinitialize();
 
@@ -268,9 +274,5 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 
 #endif /* CONFIG_MMCSD */
 
-	/* Configure the HW based on the manifest */
-
-	px4_platform_configure();
-
 	return OK;
 }
diff --git a/boards/holybro/durandal-v1/src/manifest.c b/boards/holybro/durandal-v1/src/manifest.c
index 54ab596f50..12fa048c24 100644
--- a/boards/holybro/durandal-v1/src/manifest.c
+++ b/boards/holybro/durandal-v1/src/manifest.c
@@ -83,8 +83,8 @@ static const px4_hw_mft_item_t hw_mft_list_durandal[] = {
 
 static px4_hw_mft_list_entry_t mft_lists[] = {
 //  ver_rev
-	{VD00, hw_mft_list_durandal, arraySize(hw_mft_list_durandal)},
-	{VD01, hw_mft_list_durandal, arraySize(hw_mft_list_durandal)},
+	{VD00000000, hw_mft_list_durandal, arraySize(hw_mft_list_durandal)},
+	{VD00000001, hw_mft_list_durandal, arraySize(hw_mft_list_durandal)},
 };
 
 /************************************************************************************
@@ -107,7 +107,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
 
 	if (boards_manifest == px4_hw_mft_list_uninitialized) {
-		uint32_t ver_rev = board_get_hw_version() << 8;
+		uint32_t ver_rev = board_get_hw_version() << 16;
 		ver_rev |= board_get_hw_revision();
 
 		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
@@ -118,7 +118,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 		}
 
 		if (boards_manifest == px4_hw_mft_list_uninitialized) {
-			syslog(LOG_ERR, "[boot] Board %4"  PRIx32 " is not supported!\n", ver_rev);
+			syslog(LOG_ERR, "[boot] Board %08"  PRIx32 " is not supported!\n", ver_rev);
 		}
 	}
 
diff --git a/boards/holybro/kakutef7/src/CMakeLists.txt b/boards/holybro/kakutef7/src/CMakeLists.txt
index d4a02322dc..b9438152bd 100644
--- a/boards/holybro/kakutef7/src/CMakeLists.txt
+++ b/boards/holybro/kakutef7/src/CMakeLists.txt
@@ -42,6 +42,7 @@ add_library(drivers_board
 
 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
diff --git a/boards/holybro/kakuteh7/src/CMakeLists.txt b/boards/holybro/kakuteh7/src/CMakeLists.txt
index 26fc9de8fc..220642b2a5 100644
--- a/boards/holybro/kakuteh7/src/CMakeLists.txt
+++ b/boards/holybro/kakuteh7/src/CMakeLists.txt
@@ -56,6 +56,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led # drv_led_start
diff --git a/boards/holybro/pix32v5/extras/px4_io-v2_default.bin b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin
index 36128d00f4..5140391401 100755
Binary files a/boards/holybro/pix32v5/extras/px4_io-v2_default.bin and b/boards/holybro/pix32v5/extras/px4_io-v2_default.bin differ
diff --git a/boards/holybro/pix32v5/src/CMakeLists.txt b/boards/holybro/pix32v5/src/CMakeLists.txt
index 722847ed97..7866220565 100644
--- a/boards/holybro/pix32v5/src/CMakeLists.txt
+++ b/boards/holybro/pix32v5/src/CMakeLists.txt
@@ -47,6 +47,7 @@ add_dependencies(drivers_board arch_board_hw_info)
 target_link_libraries(drivers_board
 	PRIVATE
 		arch_board_hw_info
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
diff --git a/boards/holybro/pix32v5/src/board_config.h b/boards/holybro/pix32v5/src/board_config.h
index 61adf5d679..0c17be2100 100644
--- a/boards/holybro/pix32v5/src/board_config.h
+++ b/boards/holybro/pix32v5/src/board_config.h
@@ -185,9 +185,11 @@
 #define GPIO_HW_REV_SENSE    /* PC3   */ ADC1_GPIO(13)
 #define GPIO_HW_VER_DRIVE    /* PG0   */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_VER_SENSE    /* PC2   */ ADC1_GPIO(12)
-#define HW_INFO_INIT         {'V','5','x', 'x',0}
-#define HW_INFO_INIT_VER     2
-#define HW_INFO_INIT_REV     3
+#define HW_INFO_INIT_PREFIX         "VPIX32V5"
+
+#define VPIX32V500   HW_VER_REV(0x0,0x0) // PIX32V5           Rev 0
+#define VPIX32V540   HW_VER_REV(0x4,0x0) // HolyBro mini no can 2,3
+
 /* CAN Silence
  *
  * Silent mode control \ ESC Mux select
diff --git a/boards/holybro/pix32v5/src/manifest.c b/boards/holybro/pix32v5/src/manifest.c
index 2c5375a3c7..c9d9541eee 100644
--- a/boards/holybro/pix32v5/src/manifest.c
+++ b/boards/holybro/pix32v5/src/manifest.c
@@ -129,8 +129,8 @@ static const px4_hw_mft_item_t hw_mft_list_v0540[] = {
 };
 
 static px4_hw_mft_list_entry_t mft_lists[] = {
-	{0x0000, hw_mft_list_v0500,        arraySize(hw_mft_list_v0500)},
-	{0x0400, hw_mft_list_v0540,        arraySize(hw_mft_list_v0540)},  // HolyBro mini no can 2,3
+	{VPIX32V500, hw_mft_list_v0500,        arraySize(hw_mft_list_v0500)},
+	{VPIX32V540, hw_mft_list_v0540,        arraySize(hw_mft_list_v0540)},  // HolyBro mini no can 2,3
 };
 
 /************************************************************************************
@@ -153,7 +153,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
 
 	if (boards_manifest == px4_hw_mft_list_uninitialized) {
-		uint32_t ver_rev = board_get_hw_version() << 8;
+		uint32_t ver_rev = board_get_hw_version() << 16;
 		ver_rev |= board_get_hw_revision();
 
 		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
@@ -164,7 +164,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 		}
 
 		if (boards_manifest == px4_hw_mft_list_uninitialized) {
-			syslog(LOG_ERR, "[boot] Board %4"  PRIx32 " is not supported!\n", ver_rev);
+			syslog(LOG_ERR, "[boot] Board %08"  PRIx32 " is not supported!\n", ver_rev);
 		}
 	}
 
diff --git a/boards/matek/h743-mini/src/CMakeLists.txt b/boards/matek/h743-mini/src/CMakeLists.txt
index f677ccf9e0..c47215375d 100644
--- a/boards/matek/h743-mini/src/CMakeLists.txt
+++ b/boards/matek/h743-mini/src/CMakeLists.txt
@@ -57,6 +57,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led
diff --git a/boards/matek/h743-slim/src/CMakeLists.txt b/boards/matek/h743-slim/src/CMakeLists.txt
index f677ccf9e0..c47215375d 100644
--- a/boards/matek/h743-slim/src/CMakeLists.txt
+++ b/boards/matek/h743-slim/src/CMakeLists.txt
@@ -57,6 +57,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led
diff --git a/boards/matek/h743/src/CMakeLists.txt b/boards/matek/h743/src/CMakeLists.txt
index f677ccf9e0..c47215375d 100644
--- a/boards/matek/h743/src/CMakeLists.txt
+++ b/boards/matek/h743/src/CMakeLists.txt
@@ -57,6 +57,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led
diff --git a/boards/modalai/fc-v1/src/CMakeLists.txt b/boards/modalai/fc-v1/src/CMakeLists.txt
index 3ccb6124bf..c8d256bbbc 100644
--- a/boards/modalai/fc-v1/src/CMakeLists.txt
+++ b/boards/modalai/fc-v1/src/CMakeLists.txt
@@ -46,6 +46,7 @@ add_dependencies(drivers_board arch_board_hw_info)
 
 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		arch_board_hw_info
 		drivers__led # drv_led_start
diff --git a/boards/modalai/fc-v1/src/board_config.h b/boards/modalai/fc-v1/src/board_config.h
index 11e46d6586..ed81347bdb 100644
--- a/boards/modalai/fc-v1/src/board_config.h
+++ b/boards/modalai/fc-v1/src/board_config.h
@@ -142,9 +142,10 @@
 #define GPIO_HW_VER_REV_DRIVE  /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_REV_SENSE      /* PF5 */  ADC3_GPIO(15)
 #define GPIO_HW_VER_SENSE      /* PF4 */  ADC3_GPIO(14)
-#define HW_INFO_INIT           {'V','1','x', 'x',0}
-#define HW_INFO_INIT_VER       2 /* Offset in above string of the VER */
-#define HW_INFO_INIT_REV       3 /* Offset in above string of the REV */
+#define HW_INFO_INIT_PREFIX           "V1"
+#define V10006   HW_VER_REV(0x0,0x6) // V1006              Rev 6
+#define V10100   HW_VER_REV(0x1,0x0) // V1010              Rev 0
+
 
 /* PWM
  */
diff --git a/boards/modalai/fc-v1/src/manifest.c b/boards/modalai/fc-v1/src/manifest.c
index 7b2266bd1f..d63b19f5eb 100644
--- a/boards/modalai/fc-v1/src/manifest.c
+++ b/boards/modalai/fc-v1/src/manifest.c
@@ -90,8 +90,8 @@ static const px4_hw_mft_item_t hw_mft_list_fc0100[] = {
 };
 
 static px4_hw_mft_list_entry_t mft_lists[] = {
-	{0x0006, hw_mft_list_fc0006, arraySize(hw_mft_list_fc0006)},
-	{0x0100, hw_mft_list_fc0100, arraySize(hw_mft_list_fc0100)}
+	{V10006, hw_mft_list_fc0006, arraySize(hw_mft_list_fc0006)},
+	{V10100, hw_mft_list_fc0100, arraySize(hw_mft_list_fc0100)}
 };
 
 /************************************************************************************
@@ -114,7 +114,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
 
 	if (boards_manifest == px4_hw_mft_list_uninitialized) {
-		uint32_t ver_rev = board_get_hw_version() << 8;
+		uint32_t ver_rev = board_get_hw_version() << 16;
 		ver_rev |= board_get_hw_revision();
 
 		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
@@ -125,7 +125,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 		}
 
 		if (boards_manifest == px4_hw_mft_list_uninitialized) {
-			syslog(LOG_ERR, "[boot] Board %4"  PRIx32 " is not supported!\n", ver_rev);
+			syslog(LOG_ERR, "[boot] Board %08"  PRIx32 " is not supported!\n", ver_rev);
 		}
 	}
 
diff --git a/boards/modalai/fc-v2/src/CMakeLists.txt b/boards/modalai/fc-v2/src/CMakeLists.txt
index 7ff9cdf5f5..c84d6f291c 100644
--- a/boards/modalai/fc-v2/src/CMakeLists.txt
+++ b/boards/modalai/fc-v2/src/CMakeLists.txt
@@ -60,6 +60,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led # drv_led_start
diff --git a/boards/modalai/fc-v2/src/board_config.h b/boards/modalai/fc-v2/src/board_config.h
index 26f22e4b73..b0868261a9 100644
--- a/boards/modalai/fc-v2/src/board_config.h
+++ b/boards/modalai/fc-v2/src/board_config.h
@@ -182,9 +182,8 @@
 #define GPIO_HW_VER_REV_DRIVE  /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_REV_SENSE      /* PH4 */  GPIO_ADC3_INP15
 #define GPIO_HW_VER_SENSE      /* PH3 */  GPIO_ADC3_INP14
-#define HW_INFO_INIT           {'V','2','x', 'x',0}
-#define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
-#define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */
+#define HW_INFO_INIT_PREFIX           "V2"
+#define V20300   HW_VER_REV(0x3,0x0)
 
 /* PE6 is nARMED --> FCv2 this goes to TP13
  *  The GPIO will be set as input while not armed HW will have external HW Pull UP.
diff --git a/boards/modalai/fc-v2/src/manifest.c b/boards/modalai/fc-v2/src/manifest.c
index 4f01dd360f..485e5edd63 100644
--- a/boards/modalai/fc-v2/src/manifest.c
+++ b/boards/modalai/fc-v2/src/manifest.c
@@ -83,7 +83,7 @@ static const px4_hw_mft_item_t hw_mft_list_fc0200[] = {
 };
 
 static px4_hw_mft_list_entry_t mft_lists[] = {
-	{0x0300, hw_mft_list_fc0200, arraySize(hw_mft_list_fc0200)},
+	{V20300, hw_mft_list_fc0200, arraySize(hw_mft_list_fc0200)},
 };
 
 /************************************************************************************
@@ -106,7 +106,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
 
 	if (boards_manifest == px4_hw_mft_list_uninitialized) {
-		uint32_t ver_rev = board_get_hw_version() << 8;
+		uint32_t ver_rev = board_get_hw_version() << 16;
 		ver_rev |= board_get_hw_revision();
 
 		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
@@ -117,7 +117,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 		}
 
 		if (boards_manifest == px4_hw_mft_list_uninitialized) {
-			syslog(LOG_ERR, "[boot] Board %4"  PRIx32 " is not supported!\n", ver_rev);
+			syslog(LOG_ERR, "[boot] Board %08"  PRIx32 " is not supported!\n", ver_rev);
 		}
 	}
 
diff --git a/boards/modalai/fc-v2/src/mtd.cpp b/boards/modalai/fc-v2/src/mtd.cpp
index 765ce93e2d..644b96581d 100644
--- a/boards/modalai/fc-v2/src/mtd.cpp
+++ b/boards/modalai/fc-v2/src/mtd.cpp
@@ -71,7 +71,9 @@ static const px4_mft_entry_s mtd_mft = {
 
 static const px4_mft_s mft = {
 	.nmft = 1,
-	.mfts = &mtd_mft
+	.mfts = {
+		&mtd_mft
+	}
 };
 
 const px4_mft_s *board_get_manifest(void)
diff --git a/boards/mro/ctrl-zero-f7-oem/src/CMakeLists.txt b/boards/mro/ctrl-zero-f7-oem/src/CMakeLists.txt
index a533df7c69..faaf0f3f7b 100644
--- a/boards/mro/ctrl-zero-f7-oem/src/CMakeLists.txt
+++ b/boards/mro/ctrl-zero-f7-oem/src/CMakeLists.txt
@@ -43,6 +43,7 @@ add_library(drivers_board
 
 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
diff --git a/boards/mro/ctrl-zero-f7/src/CMakeLists.txt b/boards/mro/ctrl-zero-f7/src/CMakeLists.txt
index a533df7c69..faaf0f3f7b 100644
--- a/boards/mro/ctrl-zero-f7/src/CMakeLists.txt
+++ b/boards/mro/ctrl-zero-f7/src/CMakeLists.txt
@@ -43,6 +43,7 @@ add_library(drivers_board
 
 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
diff --git a/boards/mro/ctrl-zero-h7-oem/src/CMakeLists.txt b/boards/mro/ctrl-zero-h7-oem/src/CMakeLists.txt
index 822841a74a..6a7d2ce306 100644
--- a/boards/mro/ctrl-zero-h7-oem/src/CMakeLists.txt
+++ b/boards/mro/ctrl-zero-h7-oem/src/CMakeLists.txt
@@ -55,6 +55,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			drivers__led
 			nuttx_arch
diff --git a/boards/mro/ctrl-zero-h7/src/CMakeLists.txt b/boards/mro/ctrl-zero-h7/src/CMakeLists.txt
index 822841a74a..6a7d2ce306 100644
--- a/boards/mro/ctrl-zero-h7/src/CMakeLists.txt
+++ b/boards/mro/ctrl-zero-h7/src/CMakeLists.txt
@@ -55,6 +55,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			drivers__led
 			nuttx_arch
diff --git a/boards/mro/pixracerpro/src/CMakeLists.txt b/boards/mro/pixracerpro/src/CMakeLists.txt
index eacffd5702..c8e7699046 100644
--- a/boards/mro/pixracerpro/src/CMakeLists.txt
+++ b/boards/mro/pixracerpro/src/CMakeLists.txt
@@ -55,6 +55,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			drivers__led
 			nuttx_arch
diff --git a/boards/mro/x21-777/extras/px4_io-v2_default.bin b/boards/mro/x21-777/extras/px4_io-v2_default.bin
index 36128d00f4..5140391401 100755
Binary files a/boards/mro/x21-777/extras/px4_io-v2_default.bin and b/boards/mro/x21-777/extras/px4_io-v2_default.bin differ
diff --git a/boards/mro/x21-777/nuttx-config/nsh/defconfig b/boards/mro/x21-777/nuttx-config/nsh/defconfig
index 65072334e0..1ddbbe7600 100644
--- a/boards/mro/x21-777/nuttx-config/nsh/defconfig
+++ b/boards/mro/x21-777/nuttx-config/nsh/defconfig
@@ -198,9 +198,9 @@ CONFIG_SYSTEM_CDCACM=y
 CONFIG_SYSTEM_NSH=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_UART4_BAUD=57600
-CONFIG_UART4_RXBUFSIZE=600
-CONFIG_UART4_RXDMA=y
-CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART4_RXBUFSIZE=1200
+CONFIG_UART4_RXDMA=n
+CONFIG_UART4_TXBUFSIZE=1200
 CONFIG_UART7_BAUD=57600
 CONFIG_UART7_RXBUFSIZE=600
 CONFIG_UART7_SERIAL_CONSOLE=y
@@ -225,10 +225,10 @@ CONFIG_USART3_RXBUFSIZE=600
 CONFIG_USART3_RXDMA=y
 CONFIG_USART3_TXBUFSIZE=3000
 CONFIG_USART6_BAUD=57600
-CONFIG_USART6_RXBUFSIZE=600
-CONFIG_USART6_RXDMA=y
-CONFIG_USART6_TXBUFSIZE=1500
-CONFIG_USART6_TXDMA=y
+CONFIG_USART6_RXBUFSIZE=1200
+CONFIG_USART6_RXDMA=n
+CONFIG_USART6_TXBUFSIZE=1200
+CONFIG_USART6_TXDMA=n
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
diff --git a/boards/nxp/fmuk66-e/src/CMakeLists.txt b/boards/nxp/fmuk66-e/src/CMakeLists.txt
index 2c563c1f83..090e21615c 100644
--- a/boards/nxp/fmuk66-e/src/CMakeLists.txt
+++ b/boards/nxp/fmuk66-e/src/CMakeLists.txt
@@ -46,6 +46,7 @@ add_library(drivers_board
 
 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		nuttx_arch # sdio
 		nuttx_drivers # sdio
 		drivers__led # drv_led_start
diff --git a/boards/nxp/fmuk66-v3/src/CMakeLists.txt b/boards/nxp/fmuk66-v3/src/CMakeLists.txt
index c5c34e8613..06864f0b22 100644
--- a/boards/nxp/fmuk66-v3/src/CMakeLists.txt
+++ b/boards/nxp/fmuk66-v3/src/CMakeLists.txt
@@ -45,6 +45,7 @@ add_library(drivers_board
 
 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		nuttx_arch # sdio
 		nuttx_drivers # sdio
 		drivers__led # drv_led_start
diff --git a/boards/nxp/fmurt1062-v1/src/board_config.h b/boards/nxp/fmurt1062-v1/src/board_config.h
index 53accc0e69..9d14fb5da4 100644
--- a/boards/nxp/fmurt1062-v1/src/board_config.h
+++ b/boards/nxp/fmurt1062-v1/src/board_config.h
@@ -192,9 +192,10 @@
 #define GPIO_HW_VER_REV_DRIVE /* GPIO_AD_B0_01 GPIO1_IO01   */  (GPIO_PORT1 | GPIO_PIN1 | GPIO_OUTPUT | GPIO_OUTPUT_ONE | HW_IOMUX)
 #define GPIO_HW_REV_SENSE     /* GPIO_AD_B1_08 GPIO1 Pin 24 */  ADC1_GPIO(13, 24)
 #define GPIO_HW_VER_SENSE     /* GPIO_AD_B1_04 GPIO1 Pin 20 */  ADC1_GPIO(9,  20)
-#define HW_INFO_INIT          {'V','5','x', 'x',0}
-#define HW_INFO_INIT_VER      2
-#define HW_INFO_INIT_REV      3
+#define HW_INFO_INIT_PREFIX   "V5"
+#define V500   HW_VER_REV(0x0,0x0) // FMUV5,                    Rev 0
+#define V540   HW_VER_REV(0x4,0x0) // mini no can 2,3,          Rev 0
+
 /* CAN Silence
  *
  * Silent mode control \ ESC Mux select
diff --git a/boards/nxp/fmurt1062-v1/src/manifest.c b/boards/nxp/fmurt1062-v1/src/manifest.c
index 8c18182949..63cd4eebed 100644
--- a/boards/nxp/fmurt1062-v1/src/manifest.c
+++ b/boards/nxp/fmurt1062-v1/src/manifest.c
@@ -88,8 +88,8 @@ static const px4_hw_mft_item_t hw_mft_list_v0540[] = {
 };
 
 static px4_hw_mft_list_entry_t mft_lists[] = {
-	{0x0000, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)},
-	{0x0400, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)},
+	{V500, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)},
+	{V540, hw_mft_list_v0540, arraySize(hw_mft_list_v0540)},
 };
 
 /************************************************************************************
diff --git a/boards/nxp/ucans32k146/src/mtd.cpp b/boards/nxp/ucans32k146/src/mtd.cpp
index 3f18a4cc02..d16cf9f2a6 100644
--- a/boards/nxp/ucans32k146/src/mtd.cpp
+++ b/boards/nxp/ucans32k146/src/mtd.cpp
@@ -66,7 +66,9 @@ static const px4_mft_entry_s mtd_mft = {
 
 static const px4_mft_s mft = {
 	.nmft = 1,
-	.mfts = &mtd_mft
+	.mfts = {
+		&mtd_mft
+	}
 };
 
 const px4_mft_s *board_get_manifest(void)
diff --git a/boards/px4/fmu-v2/extras/px4_io-v2_default.bin b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin
index 36128d00f4..5140391401 100755
Binary files a/boards/px4/fmu-v2/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v2/extras/px4_io-v2_default.bin differ
diff --git a/boards/px4/fmu-v3/extras/px4_io-v2_default.bin b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin
index 36128d00f4..5140391401 100755
Binary files a/boards/px4/fmu-v3/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v3/extras/px4_io-v2_default.bin differ
diff --git a/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin
index 36128d00f4..5140391401 100755
Binary files a/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v4pro/extras/px4_io-v2_default.bin differ
diff --git a/boards/px4/fmu-v4pro/src/mtd.cpp b/boards/px4/fmu-v4pro/src/mtd.cpp
index 9c0556f45b..4bd93619db 100644
--- a/boards/px4/fmu-v4pro/src/mtd.cpp
+++ b/boards/px4/fmu-v4pro/src/mtd.cpp
@@ -65,7 +65,9 @@ static const px4_mft_entry_s mtd_mft = {
 
 static const px4_mft_s mft = {
 	.nmft = 1,
-	.mfts = &mtd_mft
+	.mfts = {
+		&mtd_mft
+	}
 };
 
 const px4_mft_s *board_get_manifest(void)
diff --git a/boards/px4/fmu-v5/cryptotest.px4board b/boards/px4/fmu-v5/cryptotest.px4board
index 274e5b45ca..8206180ac2 100644
--- a/boards/px4/fmu-v5/cryptotest.px4board
+++ b/boards/px4/fmu-v5/cryptotest.px4board
@@ -3,3 +3,5 @@ CONFIG_DRIVERS_SW_CRYPTO=y
 CONFIG_DRIVERS_STUB_KEYSTORE=y
 CONFIG_PUBLIC_KEY0="../../../Tools/test_keys/key0.pub"
 CONFIG_PUBLIC_KEY1="../../../Tools/test_keys/rsa2048.pub"
+CONFIG_COMMON_OPTICAL_FLOW=n
+CONFIG_DRIVERS_OSD=n
diff --git a/boards/px4/fmu-v5/cyphal.px4board b/boards/px4/fmu-v5/cyphal.px4board
index d8754b8f34..863569607b 100644
--- a/boards/px4/fmu-v5/cyphal.px4board
+++ b/boards/px4/fmu-v5/cyphal.px4board
@@ -1,5 +1,11 @@
 # CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE is not set
+CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
+CONFIG_COMMON_TELEMETRY=n
+CONFIG_DRIVERS_BATT_SMBUS=n
+CONFIG_DRIVERS_CAMERA_CAPTURE=n
+CONFIG_DRIVERS_CAMERA_TRIGGER=n
 CONFIG_DRIVERS_HEATER=n
+CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_OSD=n
 CONFIG_DRIVERS_UAVCAN=n
 CONFIG_CYPHAL_BMS_SUBSCRIBER=y
diff --git a/boards/px4/fmu-v5/debug.px4board b/boards/px4/fmu-v5/debug.px4board
index be936a6aeb..9acb60af16 100644
--- a/boards/px4/fmu-v5/debug.px4board
+++ b/boards/px4/fmu-v5/debug.px4board
@@ -48,5 +48,4 @@ CONFIG_DRIVERS_MAGNETOMETER_LIS3MDL=y
 CONFIG_DRIVERS_MAGNETOMETER_LSM303AGR=y
 CONFIG_DRIVERS_MAGNETOMETER_QMC5883L=y
 CONFIG_DRIVERS_MAGNETOMETER_RM3100=y
-CONFIG_DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L=y
 CONFIG_DRIVERS_TEST_PPM=y
diff --git a/boards/px4/fmu-v5/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin
index 36128d00f4..5140391401 100755
Binary files a/boards/px4/fmu-v5/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v5/extras/px4_io-v2_default.bin differ
diff --git a/boards/px4/fmu-v5/init/rc.board_defaults b/boards/px4/fmu-v5/init/rc.board_defaults
index 93bb336f8c..b88ad0245e 100644
--- a/boards/px4/fmu-v5/init/rc.board_defaults
+++ b/boards/px4/fmu-v5/init/rc.board_defaults
@@ -9,7 +9,7 @@ param set-default BAT2_V_DIV 18.1
 param set-default BAT1_A_PER_V 36.367515152
 param set-default BAT2_A_PER_V 36.367515152
 
-if ver hwtypecmp V550 V552 V560 V562
+if ver hwtypecmp V5005000 V5005002 V5006000 V5006002
 then
 	# CUAV V5+ (V550/V552) and V5nano (V560/V562) have 3 IMUs
 	# Multi-EKF (IMUs only)
diff --git a/boards/px4/fmu-v5/init/rc.board_sensors b/boards/px4/fmu-v5/init/rc.board_sensors
index e024e3225b..b3d53a4934 100644
--- a/boards/px4/fmu-v5/init/rc.board_sensors
+++ b/boards/px4/fmu-v5/init/rc.board_sensors
@@ -5,7 +5,7 @@
 
 board_adc start
 
-if ver hwtypecmp V552 V562
+if ver hwtypecmp V5005002 V5006002
 then
 	# Internal SPI bus ICM-42688-P
 	icm42688p -s -R 2 -q start
diff --git a/boards/px4/fmu-v5/protected.px4board b/boards/px4/fmu-v5/protected.px4board
index 5937929c45..2a8d159ea4 100644
--- a/boards/px4/fmu-v5/protected.px4board
+++ b/boards/px4/fmu-v5/protected.px4board
@@ -19,6 +19,7 @@ CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL=n
 CONFIG_MODULES_FW_POS_CONTROL_L1=n
+CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIH=n
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=n
diff --git a/boards/px4/fmu-v5/rtps.px4board b/boards/px4/fmu-v5/rtps.px4board
index fa4fe4f07a..8a5b51a2e5 100644
--- a/boards/px4/fmu-v5/rtps.px4board
+++ b/boards/px4/fmu-v5/rtps.px4board
@@ -2,3 +2,5 @@ CONFIG_DRIVERS_HEATER=n
 CONFIG_DRIVERS_OSD=n
 CONFIG_MODULES_MICRODDS_CLIENT=y
 CONFIG_MODULES_MICRORTPS_BRIDGE=y
+CONFIG_MODULES_MICRODDS_CLIENT=y
+CONFIG_DRIVERS_UAVCAN=n
diff --git a/boards/px4/fmu-v5/src/CMakeLists.txt b/boards/px4/fmu-v5/src/CMakeLists.txt
index f900cbf8be..5323380e37 100644
--- a/boards/px4/fmu-v5/src/CMakeLists.txt
+++ b/boards/px4/fmu-v5/src/CMakeLists.txt
@@ -52,6 +52,7 @@ add_dependencies(drivers_board nuttx_context)
 target_link_libraries(drivers_board
 	PRIVATE
 		arch_board_hw_info
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
diff --git a/boards/px4/fmu-v5/src/board_config.h b/boards/px4/fmu-v5/src/board_config.h
index 0d9bb9041b..b5140cffd3 100644
--- a/boards/px4/fmu-v5/src/board_config.h
+++ b/boards/px4/fmu-v5/src/board_config.h
@@ -188,9 +188,8 @@
 #define GPIO_HW_REV_SENSE    /* PC3   */ ADC1_GPIO(13)
 #define GPIO_HW_VER_DRIVE    /* PG0   */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_VER_SENSE    /* PC2   */ ADC1_GPIO(12)
-#define HW_INFO_INIT         {'V','5','x', 'x',0}
-#define HW_INFO_INIT_VER     2
-#define HW_INFO_INIT_REV     3
+
+#define HW_INFO_INIT_PREFIX         "V5"
 #define BOARD_NUM_SPI_CFG_HW_VERSIONS 3
 #define V500   HW_VER_REV(0x0,0x0) // FMUV5,                    Rev 0
 #define V515   HW_VER_REV(0x1,0x5) // CUAV V5,                  Rev 5
diff --git a/boards/px4/fmu-v5/src/manifest.c b/boards/px4/fmu-v5/src/manifest.c
index 3e131f9ada..bab37eceff 100644
--- a/boards/px4/fmu-v5/src/manifest.c
+++ b/boards/px4/fmu-v5/src/manifest.c
@@ -186,7 +186,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
 
 	if (boards_manifest == px4_hw_mft_list_uninitialized) {
-		uint32_t ver_rev = board_get_hw_version() << 8;
+		uint32_t ver_rev = board_get_hw_version() << 16;
 		ver_rev |= board_get_hw_revision();
 
 		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
@@ -197,7 +197,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 		}
 
 		if (boards_manifest == px4_hw_mft_list_uninitialized) {
-			syslog(LOG_ERR, "[boot] Board %4"  PRIx32 " is not supported!\n", ver_rev);
+			syslog(LOG_ERR, "[boot] Board %08"  PRIx32 " is not supported!\n", ver_rev);
 		}
 	}
 
diff --git a/boards/px4/fmu-v5/stackcheck.px4board b/boards/px4/fmu-v5/stackcheck.px4board
index 2ca91e87ca..51cfba88b1 100644
--- a/boards/px4/fmu-v5/stackcheck.px4board
+++ b/boards/px4/fmu-v5/stackcheck.px4board
@@ -11,6 +11,8 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=n
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20689=n
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n
 CONFIG_DRIVERS_IRLOCK=n
+CONFIG_DRIVERS_GPS=n
+CONFIG_COMMON_MAGNETOMETER=n
 CONFIG_DRIVERS_OSD=n
 CONFIG_DRIVERS_PCA9685=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
diff --git a/boards/px4/fmu-v5/test.px4board b/boards/px4/fmu-v5/test.px4board
index 983aaf05cf..190abdac3a 100644
--- a/boards/px4/fmu-v5/test.px4board
+++ b/boards/px4/fmu-v5/test.px4board
@@ -3,6 +3,7 @@ CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
+CONFIG_DRIVERS_UAVCAN=n
 CONFIG_DRIVERS_RPM=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
diff --git a/boards/px4/fmu-v5/uavcanv0periph.px4board b/boards/px4/fmu-v5/uavcanv0periph.px4board
index c9675d9a0b..97f5573cd2 100644
--- a/boards/px4/fmu-v5/uavcanv0periph.px4board
+++ b/boards/px4/fmu-v5/uavcanv0periph.px4board
@@ -2,10 +2,15 @@
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=n
 CONFIG_COMMON_HYGROMETERS=n
 CONFIG_COMMON_TELEMETRY=n
+CONFIG_DRIVERS_BATT_SMBUS=n
+CONFIG_DRIVERS_CAMERA_CAPTURE=n
+CONFIG_DRIVERS_CAMERA_TRIGGER=n
 CONFIG_DRIVERS_ADC_ADS1115=n
+CONFIG_DRIVERS_GPS=n
 CONFIG_DRIVERS_HEATER=n
 CONFIG_DRIVERS_IMU_ANALOG_DEVICES_ADIS16448=n
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=n
+CONFIG_COMMON_OPTICAL_FLOW=n
 CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_OSD=n
 CONFIG_DRIVERS_PCA9685=n
diff --git a/boards/px4/fmu-v5x/cmake/upload.cmake b/boards/px4/fmu-v5x/cmake/upload.cmake
new file mode 100644
index 0000000000..270078fe26
--- /dev/null
+++ b/boards/px4/fmu-v5x/cmake/upload.cmake
@@ -0,0 +1,49 @@
+############################################################################
+#
+#   Copyright (c) 2020 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(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.px4)
+
+add_custom_target(upload_usb
+	COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh ${PX4_FW_NAME} --default-ip=10.41.1.1
+	DEPENDS ${PX4_FW_NAME}
+	COMMENT "uploading PX4"
+	USES_TERMINAL
+)
+
+add_custom_target(upload_wifi
+	COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh ${PX4_FW_NAME}
+	DEPENDS ${PX4_FW_NAME}
+	COMMENT "uploading PX4"
+	USES_TERMINAL
+)
diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board
index 135cbc3e60..22ac3ccabd 100644
--- a/boards/px4/fmu-v5x/default.px4board
+++ b/boards/px4/fmu-v5x/default.px4board
@@ -34,6 +34,8 @@ CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 CONFIG_DRIVERS_POWER_MONITOR_INA228=y
 CONFIG_DRIVERS_POWER_MONITOR_INA238=y
+CONFIG_DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION=y
+CONFIG_DRIVERS_PPS_CAPTURE=y
 CONFIG_DRIVERS_PWM_INPUT=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_PWM_OUT_SIM=y
@@ -77,6 +79,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
+CONFIG_MODULES_PAYLOAD_DELIVERER=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
@@ -84,6 +87,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_ACTUATOR_ENDURANCE_TEST=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
@@ -95,7 +99,6 @@ CONFIG_SYSTEMCMDS_MFT=y
 CONFIG_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_MOTOR_TEST=y
 CONFIG_SYSTEMCMDS_MTD=y
-CONFIG_SYSTEMCMDS_NETMAN=y
 CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
@@ -113,4 +116,5 @@ CONFIG_SYSTEMCMDS_UORB=y
 CONFIG_SYSTEMCMDS_USB_CONNECTED=y
 CONFIG_SYSTEMCMDS_VER=y
 CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_EXAMPLES_EXT_UPDATE_CHECKER=y
 CONFIG_EXAMPLES_FAKE_GPS=y
diff --git a/boards/px4/fmu-v5x/extras/px4_fmu-v5x_bootloader.bin b/boards/px4/fmu-v5x/extras/px4_fmu-v5x_bootloader.bin
old mode 100644
new mode 100755
index 6a9cba4c8e..c44fc7367a
Binary files a/boards/px4/fmu-v5x/extras/px4_fmu-v5x_bootloader.bin and b/boards/px4/fmu-v5x/extras/px4_fmu-v5x_bootloader.bin differ
diff --git a/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin
index 36128d00f4..5140391401 100755
Binary files a/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v5x/extras/px4_io-v2_default.bin differ
diff --git a/boards/px4/fmu-v5x/init/rc.board_defaults b/boards/px4/fmu-v5x/init/rc.board_defaults
index a7d844a366..9d1d26e402 100644
--- a/boards/px4/fmu-v5x/init/rc.board_defaults
+++ b/boards/px4/fmu-v5x/init/rc.board_defaults
@@ -2,15 +2,26 @@
 #
 # board specific defaults
 #------------------------------------------------------------------------------
+#
+# External components flashing
+#
 
-# Mavlink ethernet (CFG 1000)
-param set-default MAV_2_CONFIG 1000
-param set-default MAV_2_BROADCAST 1
-param set-default MAV_2_MODE 0
-param set-default MAV_2_RADIO_CTL 0
-param set-default MAV_2_RATE 100000
-param set-default MAV_2_REMOTE_PRT 14550
-param set-default MAV_2_UDP_PRT 14550
+if param compare COM_EXT_COMP_EN 2
+then
+	echo "-> Starting FAKE flasher - SUCCESS case"
+	update_checker fake_success
+fi
+
+if param compare COM_EXT_COMP_EN 3
+then
+	echo "-> Starting FAKE flasher - FAIL case"
+	update_checker fake_fail
+fi
+
+# Probe for LW20/c by default
+param set-default SENS_EN_SF1XX 6
+# start a MAVLink instance on the Pixhawk Payload Bus
+param set-default MAV_2_CONFIG 401
 
 param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
diff --git a/boards/px4/fmu-v5x/init/rc.board_mavlink b/boards/px4/fmu-v5x/init/rc.board_mavlink
index c0dbc02403..e37e645f14 100644
--- a/boards/px4/fmu-v5x/init/rc.board_mavlink
+++ b/boards/px4/fmu-v5x/init/rc.board_mavlink
@@ -3,8 +3,23 @@
 # board specific MAVLink startup script.
 #------------------------------------------------------------------------------
 
-if ver hwtypecmp V5X90 V5X91 V5Xa0 V5Xa1 V5X80 V5X81
+# if skynode base board is detected start Mavlink on Telem2
+if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001
 then
-	# Start MAVLink on the UART connected to the mission computer
-	mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z
+	if param compare MAV_S_FORWARD 1
+	then
+		set S_FORWARD "-f"
+	else
+		set S_FORWARD ""
+	fi
+
+	mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard_low_bandwidth -x -z $S_FORWARD
+
+	microdds_client start -t udp -h 192.168.0.5 -p 15555 -l
+
+	# Ensure nothing else starts on TEL2 (ttyS4)
+	set PRT_TEL2_ 1
+
+	unset S_FORWARD
+	unset MAVLINK_STREAM_ONLY
 fi
diff --git a/boards/px4/fmu-v5x/init/rc.board_sensors b/boards/px4/fmu-v5x/init/rc.board_sensors
index 1dfa208384..46b5d33c90 100644
--- a/boards/px4/fmu-v5x/init/rc.board_sensors
+++ b/boards/px4/fmu-v5x/init/rc.board_sensors
@@ -5,13 +5,14 @@
 
 set HAVE_PM2 yes
 
-if ver hwtypecmp V5X50 V5X51 V5X52
+if ver hwtypecmp V5X005000 V5X005001 V5X005002
 then
 	set HAVE_PM2 no
 fi
 if param compare -s ADC_ADS1115_EN 1
 then
 	ads1115 start -X
+	board_adc start -n
 else
 	board_adc start
 fi
@@ -48,33 +49,11 @@ then
 	fi
 fi
 
-if ver hwtypecmp V5X90 V5X91 V5X92 V5Xa0 V5Xa1 V5Xa2
+if ver hwtypecmp V5X000000 V5X000001 V5X000002 V5X001000
 then
-	#SKYNODE base fmu board orientation
-
-	if ver hwtypecmp V5X90 V5X91 V5Xa0 V5Xa1
-	then
-		# Internal SPI BMI088
-		bmi088 -A -R 2 -s start
-		bmi088 -G -R 2 -s start
-	else
-		# Internal SPI bus ICM20649
-		icm20649 -s -R 4 start
-	fi
-
-	# Internal SPI bus ICM42688p
-	icm42688p -R 4 -s start
-
-	# Internal SPI bus ICM-20602 (hard-mounted)
-	icm20602 -R 8 -s start
-
-	# Internal magnetometer on I2c
-	bmm150 -I -R 6 start
-
-else
 	#FMUv5Xbase board orientation
 
-	if ver hwtypecmp V5X00 V5X01
+	if ver hwtypecmp V5X000000 V5X000001
 	then
 		# Internal SPI BMI088
 		bmi088 -A -R 4 -s start
@@ -90,9 +69,34 @@ else
 	# Internal SPI bus ICM-20602 (hard-mounted)
 	icm20602 -R 10 -s start
 
-	# Internal magnetometer on I2c
+	# Internal magnetometer on I2C
 	bmm150 -I start
 
+else
+	#SKYNODE base fmu board orientation
+
+	if ver hwtypecmp V5X009000 V5X009001 V5X00a000 V5X00a001 V5X008000 V5X008001 V5X010001
+	then
+		# Internal SPI BMI088
+		bmi088 -A -R 2 -s start
+		bmi088 -G -R 2 -s start
+	else
+		# Internal SPI bus ICM20649
+		icm20649 -s -R 4 start
+	fi
+
+	# Internal SPI bus ICM42688p
+	icm42688p -R 4 -s start
+
+	# Internal SPI bus ICM-20602 (hard-mounted)
+	icm20602 -R 8 -s start
+
+	# Internal magnetometer on I2C
+	bmm150 -I -R 6 start
+
+	# Auto start power monitors
+	pm_selector_auterion start
+
 fi
 
 # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
@@ -104,7 +108,7 @@ ist8310 -X -b 1 -R 10 start
 if param compare SENS_INT_BARO_EN 1
 then
 	bmp388 -I -a 0x77 start
-	if ver hwtypecmp V5X00 V5X90 V5Xa0
+	if ver hwtypecmp V5X000000 V5X001000 V5X008000 V5X009000 V5X00a000
 	then
 		bmp388 -I start
 	else
@@ -112,4 +116,4 @@ then
 	fi
 
 fi
-unset HAVE_PM2
\ No newline at end of file
+unset HAVE_PM2
diff --git a/boards/px4/fmu-v5x/manifest/mft_v0_0x0010.bin b/boards/px4/fmu-v5x/manifest/mft_v0_0x0010.bin
new file mode 100644
index 0000000000..8dd3e37ac8
Binary files /dev/null and b/boards/px4/fmu-v5x/manifest/mft_v0_0x0010.bin differ
diff --git a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig
index e36b4ef4e3..070350442f 100644
--- a/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig
+++ b/boards/px4/fmu-v5x/nuttx-config/nsh/defconfig
@@ -13,9 +13,9 @@
 # CONFIG_MMCSD_SPI is not set
 # CONFIG_NSH_DISABLEBG is not set
 # CONFIG_NSH_DISABLESCRIPT is not set
-# CONFIG_NSH_DISABLE_ARP is not set
 # CONFIG_NSH_DISABLE_DF is not set
 # CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
 # CONFIG_NSH_DISABLE_GET is not set
 # CONFIG_NSH_DISABLE_ITEF is not set
 # CONFIG_NSH_DISABLE_LOOPS is not set
@@ -63,7 +63,6 @@ CONFIG_DEV_GPIO=y
 CONFIG_DEV_PIPE_MAXSIZE=1024
 CONFIG_DEV_PIPE_SIZE=70
 CONFIG_DISABLE_MQUEUE=y
-CONFIG_ETH0_PHY_LAN8742A=y
 CONFIG_FAT_DMAMEMORY=y
 CONFIG_FAT_LCNAMES=y
 CONFIG_FAT_LFN=y
@@ -104,33 +103,6 @@ CONFIG_MTD=y
 CONFIG_MTD_BYTE_WRITE=y
 CONFIG_MTD_PARTITION=y
 CONFIG_MTD_RAMTRON=y
-CONFIG_NET=y
-CONFIG_NETDB_DNSCLIENT=y
-CONFIG_NETDB_DNSCLIENT_ENTRIES=8
-CONFIG_NETDB_DNSSERVER_NOADDR=y
-CONFIG_NETDEV_PHY_IOCTL=y
-CONFIG_NETINIT_DHCPC=y
-CONFIG_NETINIT_DNS=y
-CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
-CONFIG_NETINIT_DRIPADDR=0XC0A800FE
-CONFIG_NETINIT_MONITOR=y
-CONFIG_NETINIT_THREAD=y
-CONFIG_NETINIT_THREAD_PRIORITY=49
-CONFIG_NETUTILS_TELNETD=y
-CONFIG_NET_ARP_IPIN=y
-CONFIG_NET_ARP_SEND=y
-CONFIG_NET_BROADCAST=y
-CONFIG_NET_ETH_PKTSIZE=1518
-CONFIG_NET_ICMP=y
-CONFIG_NET_ICMP_SOCKET=y
-CONFIG_NET_SOLINGER=y
-CONFIG_NET_TCP=y
-CONFIG_NET_TCPBACKLOG=y
-CONFIG_NET_TCP_DELAYED_ACK=y
-CONFIG_NET_TCP_WRITE_BUFFERS=y
-CONFIG_NET_UDP=y
-CONFIG_NET_UDP_CHECKSUMS=y
-CONFIG_NET_UDP_WRITE_BUFFERS=y
 CONFIG_NSH_ARCHINIT=y
 CONFIG_NSH_ARGCAT=y
 CONFIG_NSH_BUILTIN_APPS=y
@@ -143,8 +115,6 @@ CONFIG_NSH_QUOTE=y
 CONFIG_NSH_ROMFSETC=y
 CONFIG_NSH_ROMFSSECTSIZE=128
 CONFIG_NSH_STRERROR=y
-CONFIG_NSH_TELNET=y
-CONFIG_NSH_TELNET_LOGIN=y
 CONFIG_NSH_VARS=y
 CONFIG_OTG_ID_GPIO_DISABLE=y
 CONFIG_PIPES=y
@@ -191,21 +161,15 @@ CONFIG_STM32F7_BKPSRAM=y
 CONFIG_STM32F7_DMA1=y
 CONFIG_STM32F7_DMA2=y
 CONFIG_STM32F7_DMACAPABLE=y
-CONFIG_STM32F7_ETHMAC=y
 CONFIG_STM32F7_FLOWCONTROL_BROKEN=y
 CONFIG_STM32F7_I2C1=y
 CONFIG_STM32F7_I2C2=y
 CONFIG_STM32F7_I2C3=y
 CONFIG_STM32F7_I2C4=y
 CONFIG_STM32F7_I2C_DYNTIMEO=y
-CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=10
+CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=50
+CONFIG_STM32F7_I2C_DYNTIMEO_USECPERBYTE=10000
 CONFIG_STM32F7_OTGFS=y
-CONFIG_STM32F7_PHYADDR=0
-CONFIG_STM32F7_PHYSR=31
-CONFIG_STM32F7_PHYSR_100MBPS=0x8
-CONFIG_STM32F7_PHYSR_FULLDUPLEX=0x10
-CONFIG_STM32F7_PHYSR_MODE=0x10
-CONFIG_STM32F7_PHYSR_SPEED=0x8
 CONFIG_STM32F7_PHY_POLLING=y
 CONFIG_STM32F7_PROGMEM=y
 CONFIG_STM32F7_PWR=y
@@ -250,7 +214,6 @@ CONFIG_STM32F7_WWDG=y
 CONFIG_SYSTEM_CDCACM=y
 CONFIG_SYSTEM_DHCPC_RENEW=y
 CONFIG_SYSTEM_NSH=y
-CONFIG_SYSTEM_PING=y
 CONFIG_TASK_NAME_SIZE=24
 CONFIG_UART4_BAUD=57600
 CONFIG_UART4_RXBUFSIZE=600
@@ -259,7 +222,7 @@ CONFIG_UART5_IFLOWCONTROL=y
 CONFIG_UART5_OFLOWCONTROL=y
 CONFIG_UART5_RXBUFSIZE=600
 CONFIG_UART5_RXDMA=y
-CONFIG_UART5_TXBUFSIZE=3000
+CONFIG_UART5_TXBUFSIZE=10000
 CONFIG_UART5_TXDMA=y
 CONFIG_UART7_BAUD=57600
 CONFIG_UART7_IFLOWCONTROL=y
@@ -290,5 +253,5 @@ CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
 CONFIG_USEC_PER_TICK=1000
-CONFIG_USERMAIN_STACKSIZE=2944
+CONFIG_USERMAIN_STACKSIZE=3094
 CONFIG_USER_ENTRYPOINT="nsh_main"
diff --git a/boards/px4/fmu-v5x/nuttx-config/rtps/defconfig b/boards/px4/fmu-v5x/nuttx-config/rtps/defconfig
new file mode 100644
index 0000000000..67e3ab173f
--- /dev/null
+++ b/boards/px4/fmu-v5x/nuttx-config/rtps/defconfig
@@ -0,0 +1,295 @@
+#
+# This file is autogenerated: PLEASE DO NOT EDIT IT.
+#
+# You can use "make menuconfig" to make any modifications to the installed .config file.
+# You can then do "make savedefconfig" to generate a new defconfig file that includes your
+# modifications.
+#
+# CONFIG_DISABLE_ENVIRON is not set
+# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
+# CONFIG_MMCSD_HAVE_CARDDETECT is not set
+# CONFIG_MMCSD_HAVE_WRITEPROTECT is not set
+# CONFIG_MMCSD_MMCSUPPORT is not set
+# CONFIG_MMCSD_SPI is not set
+# CONFIG_NSH_DISABLEBG is not set
+# CONFIG_NSH_DISABLESCRIPT is not set
+# CONFIG_NSH_DISABLE_ARP is not set
+# CONFIG_NSH_DISABLE_DF is not set
+# CONFIG_NSH_DISABLE_EXEC is not set
+# CONFIG_NSH_DISABLE_EXIT is not set
+# CONFIG_NSH_DISABLE_GET is not set
+# CONFIG_NSH_DISABLE_ITEF is not set
+# CONFIG_NSH_DISABLE_LOOPS is not set
+# CONFIG_NSH_DISABLE_MKFATFS is not set
+# CONFIG_NSH_DISABLE_SEMICOLON is not set
+# CONFIG_NSH_DISABLE_TIME is not set
+CONFIG_ARCH="arm"
+CONFIG_ARCH_BOARD_CUSTOM=y
+CONFIG_ARCH_BOARD_CUSTOM_DIR="../../../../boards/px4/fmu-v5x/nuttx-config"
+CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
+CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
+CONFIG_ARCH_CHIP="stm32f7"
+CONFIG_ARCH_CHIP_STM32F765II=y
+CONFIG_ARCH_CHIP_STM32F7=y
+CONFIG_ARCH_INTERRUPTSTACK=512
+CONFIG_ARCH_STACKDUMP=y
+CONFIG_ARMV7M_BASEPRI_WAR=y
+CONFIG_ARMV7M_DCACHE=y
+CONFIG_ARMV7M_DTCM=y
+CONFIG_ARMV7M_ICACHE=y
+CONFIG_ARMV7M_MEMCPY=y
+CONFIG_ARMV7M_USEBASEPRI=y
+CONFIG_ARM_MPU_EARLY_RESET=y
+CONFIG_BOARDCTL_RESET=y
+CONFIG_BOARD_CRASHDUMP=y
+CONFIG_BOARD_LOOPSPERMSEC=22114
+CONFIG_BOARD_RESET_ON_ASSERT=2
+CONFIG_BUILTIN=y
+CONFIG_C99_BOOL8=y
+CONFIG_CDCACM=y
+CONFIG_CDCACM_IFLOWCONTROL=y
+CONFIG_CDCACM_PRODUCTID=0x0033
+CONFIG_CDCACM_PRODUCTSTR="PX4 FMU v5X.x"
+CONFIG_CDCACM_RXBUFSIZE=600
+CONFIG_CDCACM_TXBUFSIZE=12000
+CONFIG_CDCACM_VENDORID=0x3185
+CONFIG_CDCACM_VENDORSTR="Auterion"
+CONFIG_CLOCK_MONOTONIC=y
+CONFIG_DEBUG_FULLOPT=y
+CONFIG_DEBUG_HARDFAULT_ALERT=y
+CONFIG_DEBUG_SYMBOLS=y
+CONFIG_DEFAULT_SMALL=y
+CONFIG_DEV_FIFO_SIZE=0
+CONFIG_DEV_GPIO=y
+CONFIG_DEV_PIPE_MAXSIZE=1024
+CONFIG_DEV_PIPE_SIZE=70
+CONFIG_ETH0_PHY_LAN8742A=y
+CONFIG_FAT_DMAMEMORY=y
+CONFIG_FAT_LCNAMES=y
+CONFIG_FAT_LFN=y
+CONFIG_FAT_LFN_ALIAS_HASH=y
+CONFIG_FDCLONE_STDIO=y
+CONFIG_FSUTILS_IPCFG=y
+CONFIG_FS_BINFS=y
+CONFIG_FS_CROMFS=y
+CONFIG_FS_FAT=y
+CONFIG_FS_FATTIME=y
+CONFIG_FS_PROCFS=y
+CONFIG_FS_PROCFS_INCLUDE_PROGMEM=y
+CONFIG_FS_PROCFS_MAX_TASKS=64
+CONFIG_FS_PROCFS_REGISTER=y
+CONFIG_FS_ROMFS=y
+CONFIG_GRAN=y
+CONFIG_GRAN_INTR=y
+CONFIG_HAVE_CXX=y
+CONFIG_HAVE_CXXINITIALIZE=y
+CONFIG_I2C=y
+CONFIG_I2C_RESET=y
+CONFIG_IDLETHREAD_STACKSIZE=750
+CONFIG_IOB_NBUFFERS=24
+CONFIG_IOB_THROTTLE=0
+CONFIG_IPCFG_BINARY=y
+CONFIG_IPCFG_CHARDEV=y
+CONFIG_IPCFG_PATH="/fs/mtd_net"
+CONFIG_LIBC_FLOATINGPOINT=y
+CONFIG_LIBC_LONG_LONG=y
+CONFIG_LIBC_STRERROR=y
+CONFIG_MEMSET_64BIT=y
+CONFIG_MEMSET_OPTSPEED=y
+CONFIG_MMCSD=y
+CONFIG_MMCSD_SDIO=y
+CONFIG_MMCSD_SDIOWAIT_WRCOMPLETE=y
+CONFIG_MM_REGIONS=3
+CONFIG_MTD=y
+CONFIG_MTD_BYTE_WRITE=y
+CONFIG_MTD_PARTITION=y
+CONFIG_MTD_RAMTRON=y
+CONFIG_NET=y
+CONFIG_NETDB_DNSCLIENT=y
+CONFIG_NETDB_DNSCLIENT_ENTRIES=8
+CONFIG_NETDB_DNSSERVER_NOADDR=y
+CONFIG_NETDEV_PHY_IOCTL=y
+CONFIG_NETINIT_DHCPC=y
+CONFIG_NETINIT_DNS=y
+CONFIG_NETINIT_DNSIPADDR=0XC0A800FE
+CONFIG_NETINIT_DRIPADDR=0XC0A800FE
+CONFIG_NETINIT_MONITOR=y
+CONFIG_NETINIT_THREAD=y
+CONFIG_NETINIT_THREAD_PRIORITY=49
+CONFIG_NETUTILS_TELNETD=y
+CONFIG_NET_ARP_IPIN=y
+CONFIG_NET_ARP_SEND=y
+CONFIG_NET_BROADCAST=y
+CONFIG_NET_ETH_PKTSIZE=1518
+CONFIG_NET_ICMP=y
+CONFIG_NET_ICMP_SOCKET=y
+CONFIG_NET_SOLINGER=y
+CONFIG_NET_TCP=y
+CONFIG_NET_TCPBACKLOG=y
+CONFIG_NET_TCP_DELAYED_ACK=y
+CONFIG_NET_TCP_WRITE_BUFFERS=y
+CONFIG_NET_UDP=y
+CONFIG_NET_UDP_CHECKSUMS=y
+CONFIG_NET_UDP_WRITE_BUFFERS=y
+CONFIG_NSH_ARCHINIT=y
+CONFIG_NSH_ARGCAT=y
+CONFIG_NSH_BUILTIN_APPS=y
+CONFIG_NSH_CMDPARMS=y
+CONFIG_NSH_CROMFSETC=y
+CONFIG_NSH_LINELEN=128
+CONFIG_NSH_MAXARGUMENTS=15
+CONFIG_NSH_NESTDEPTH=8
+CONFIG_NSH_QUOTE=y
+CONFIG_NSH_ROMFSETC=y
+CONFIG_NSH_ROMFSSECTSIZE=128
+CONFIG_NSH_STRERROR=y
+CONFIG_NSH_TELNET=y
+CONFIG_NSH_TELNET_LOGIN=y
+CONFIG_NSH_VARS=y
+CONFIG_OTG_ID_GPIO_DISABLE=y
+CONFIG_PIPES=y
+CONFIG_PREALLOC_TIMERS=50
+CONFIG_PRIORITY_INHERITANCE=y
+CONFIG_PTHREAD_MUTEX_ROBUST=y
+CONFIG_PTHREAD_STACK_MIN=512
+CONFIG_RAMTRON_SETSPEED=y
+CONFIG_RAM_SIZE=245760
+CONFIG_RAM_START=0x20010000
+CONFIG_RAW_BINARY=y
+CONFIG_READLINE_CMD_HISTORY=y
+CONFIG_READLINE_TABCOMPLETION=y
+CONFIG_RTC_DATETIME=y
+CONFIG_SCHED_ATEXIT=y
+CONFIG_SCHED_HPWORK=y
+CONFIG_SCHED_HPWORKPRIORITY=249
+CONFIG_SCHED_HPWORKSTACKSIZE=1280
+CONFIG_SCHED_INSTRUMENTATION=y
+CONFIG_SCHED_INSTRUMENTATION_EXTERNAL=y
+CONFIG_SCHED_LPWORK=y
+CONFIG_SCHED_LPWORKPRIORITY=50
+CONFIG_SCHED_LPWORKSTACKSIZE=1632
+CONFIG_SCHED_WAITPID=y
+CONFIG_SDCLONE_DISABLE=y
+CONFIG_SDMMC2_SDIO_PULLUP=y
+CONFIG_SEM_PREALLOCHOLDERS=32
+CONFIG_SERIAL_IFLOWCONTROL_WATERMARKS=y
+CONFIG_SERIAL_TERMIOS=y
+CONFIG_SIG_DEFAULT=y
+CONFIG_SIG_SIGALRM_ACTION=y
+CONFIG_SIG_SIGUSR1_ACTION=y
+CONFIG_SIG_SIGUSR2_ACTION=y
+CONFIG_SIG_SIGWORK=4
+CONFIG_STACK_COLORATION=y
+CONFIG_START_DAY=30
+CONFIG_START_MONTH=11
+CONFIG_STDIO_BUFFER_SIZE=256
+CONFIG_STM32F7_ADC1=y
+CONFIG_STM32F7_ADC3=y
+CONFIG_STM32F7_BBSRAM=y
+CONFIG_STM32F7_BBSRAM_FILES=5
+CONFIG_STM32F7_BKPSRAM=y
+CONFIG_STM32F7_DMA1=y
+CONFIG_STM32F7_DMA2=y
+CONFIG_STM32F7_DMACAPABLE=y
+CONFIG_STM32F7_ETHMAC=y
+CONFIG_STM32F7_FLOWCONTROL_BROKEN=y
+CONFIG_STM32F7_I2C1=y
+CONFIG_STM32F7_I2C2=y
+CONFIG_STM32F7_I2C3=y
+CONFIG_STM32F7_I2C4=y
+CONFIG_STM32F7_I2C_DYNTIMEO=y
+CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=50
+CONFIG_STM32F7_I2C_DYNTIMEO_USECPERBYTE=10000
+CONFIG_STM32F7_OTGFS=y
+CONFIG_STM32F7_PHYADDR=0
+CONFIG_STM32F7_PHYSR=31
+CONFIG_STM32F7_PHYSR_100MBPS=0x8
+CONFIG_STM32F7_PHYSR_FULLDUPLEX=0x10
+CONFIG_STM32F7_PHYSR_MODE=0x10
+CONFIG_STM32F7_PHYSR_SPEED=0x8
+CONFIG_STM32F7_PHY_POLLING=y
+CONFIG_STM32F7_PROGMEM=y
+CONFIG_STM32F7_PWR=y
+CONFIG_STM32F7_RTC=y
+CONFIG_STM32F7_RTC_AUTO_LSECLOCK_START_DRV_CAPABILITY=y
+CONFIG_STM32F7_RTC_MAGIC_REG=1
+CONFIG_STM32F7_SAVE_CRASHDUMP=y
+CONFIG_STM32F7_SDMMC2=y
+CONFIG_STM32F7_SDMMC_DMA=y
+CONFIG_STM32F7_SERIALBRK_BSDCOMPAT=y
+CONFIG_STM32F7_SERIAL_DISABLE_REORDERING=y
+CONFIG_STM32F7_SPI1=y
+CONFIG_STM32F7_SPI1_DMA=y
+CONFIG_STM32F7_SPI1_DMA_BUFFER=1024
+CONFIG_STM32F7_SPI2=y
+CONFIG_STM32F7_SPI2_DMA=y
+CONFIG_STM32F7_SPI2_DMA_BUFFER=4096
+CONFIG_STM32F7_SPI3=y
+CONFIG_STM32F7_SPI3_DMA=y
+CONFIG_STM32F7_SPI3_DMA_BUFFER=1024
+CONFIG_STM32F7_SPI5=y
+CONFIG_STM32F7_SPI6=y
+CONFIG_STM32F7_SPI_DMA=y
+CONFIG_STM32F7_SPI_DMATHRESHOLD=8
+CONFIG_STM32F7_TIM10=y
+CONFIG_STM32F7_TIM11=y
+CONFIG_STM32F7_TIM3=y
+CONFIG_STM32F7_TIM9=y
+CONFIG_STM32F7_UART4=y
+CONFIG_STM32F7_UART5=y
+CONFIG_STM32F7_UART7=y
+CONFIG_STM32F7_UART8=y
+CONFIG_STM32F7_USART1=y
+CONFIG_STM32F7_USART2=y
+CONFIG_STM32F7_USART3=y
+CONFIG_STM32F7_USART6=y
+CONFIG_STM32F7_USART_BREAKS=y
+CONFIG_STM32F7_USART_INVERT=y
+CONFIG_STM32F7_USART_SINGLEWIRE=y
+CONFIG_STM32F7_USART_SWAP=y
+CONFIG_STM32F7_WWDG=y
+CONFIG_SYSTEM_CDCACM=y
+CONFIG_SYSTEM_DHCPC_RENEW=y
+CONFIG_SYSTEM_NSH=y
+CONFIG_SYSTEM_PING=y
+CONFIG_TASK_NAME_SIZE=24
+CONFIG_UART4_BAUD=57600
+CONFIG_UART4_RXBUFSIZE=600
+CONFIG_UART4_TXBUFSIZE=1500
+CONFIG_UART5_IFLOWCONTROL=y
+CONFIG_UART5_OFLOWCONTROL=y
+CONFIG_UART5_RXBUFSIZE=600
+CONFIG_UART5_RXDMA=y
+CONFIG_UART5_TXBUFSIZE=10000
+CONFIG_UART5_TXDMA=y
+CONFIG_UART7_BAUD=57600
+CONFIG_UART7_IFLOWCONTROL=y
+CONFIG_UART7_OFLOWCONTROL=y
+CONFIG_UART7_RXBUFSIZE=600
+CONFIG_UART7_RXDMA=y
+CONFIG_UART7_TXBUFSIZE=3000
+CONFIG_UART8_BAUD=57600
+CONFIG_UART8_RXBUFSIZE=600
+CONFIG_UART8_TXBUFSIZE=1500
+CONFIG_USART1_BAUD=57600
+CONFIG_USART1_RXBUFSIZE=600
+CONFIG_USART1_TXBUFSIZE=1500
+CONFIG_USART2_BAUD=57600
+CONFIG_USART2_IFLOWCONTROL=y
+CONFIG_USART2_OFLOWCONTROL=y
+CONFIG_USART2_RXBUFSIZE=600
+CONFIG_USART2_TXBUFSIZE=3000
+CONFIG_USART3_BAUD=57600
+CONFIG_USART3_RXBUFSIZE=180
+CONFIG_USART3_SERIAL_CONSOLE=y
+CONFIG_USART3_TXBUFSIZE=1500
+CONFIG_USART6_BAUD=57600
+CONFIG_USART6_RXBUFSIZE=600
+CONFIG_USART6_RXDMA=y
+CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USBDEV=y
+CONFIG_USBDEV_BUSPOWERED=y
+CONFIG_USBDEV_MAXPOWER=500
+CONFIG_USEC_PER_TICK=1000
+CONFIG_USERMAIN_STACKSIZE=3094
+CONFIG_USER_ENTRYPOINT="nsh_main"
diff --git a/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld b/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld
index 8d5a78da6b..7303c61768 100644
--- a/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld
+++ b/boards/px4/fmu-v5x/nuttx-config/scripts/script.ld
@@ -90,6 +90,7 @@ ENTRY(_stext)
  */
 EXTERN(abort)
 EXTERN(_bootdelay_signature)
+EXTERN(_main_toc)
 EXTERN(board_get_manifest)
 
 SECTIONS
@@ -104,6 +105,7 @@ SECTIONS
 		_bootdelay_signature = ABSOLUTE(.);
 		FILL(0xffecc2925d7d05c5)
 		. += 8;
+		*(.main_toc)
 		*(.text .text.*)
 		*(.fixup)
 		*(.gnu.warning)
@@ -187,4 +189,12 @@ SECTIONS
 	} > ITCM_RAM AT > FLASH_AXIM
 
 	_framfuncs = LOADADDR(.ramfunc);
+
+    /* Start of the image signature. This
+     *  has to be in the end of the image
+     */
+	.signature : {
+		. = ALIGN(4);
+		_boot_signature = ABSOLUTE(.);
+	} > FLASH_AXIM
 }
diff --git a/boards/px4/fmu-v5x/recovery.px4board b/boards/px4/fmu-v5x/recovery.px4board
new file mode 100644
index 0000000000..96328ac170
--- /dev/null
+++ b/boards/px4/fmu-v5x/recovery.px4board
@@ -0,0 +1,48 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_ETHERNET=y
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
+CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7"
+CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
+CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
+CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
+CONFIG_BOARD_SERIAL_PPB="/dev/ttyS3"
+CONFIG_BOARD_ROMFSROOT="recovery"
+CONFIG_DRIVERS_ADC_BOARD_ADC=y
+CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_LAND_DETECTOR=y
+CONFIG_MODULES_MAVLINK=y
+CONFIG_MODULES_NAVIGATOR=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_ACTUATOR_ENDURANCE_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_DUMPFILE=y
+CONFIG_SYSTEMCMDS_ESC_CALIB=y
+CONFIG_SYSTEMCMDS_GPIO=y
+CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
+CONFIG_SYSTEMCMDS_I2CDETECT=y
+CONFIG_SYSTEMCMDS_LED_CONTROL=y
+CONFIG_SYSTEMCMDS_MFT=y
+CONFIG_SYSTEMCMDS_MIXER=y
+CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
+CONFIG_SYSTEMCMDS_MOTOR_TEST=y
+CONFIG_SYSTEMCMDS_MTD=y
+CONFIG_SYSTEMCMDS_NSHTERM=y
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_REFLECT=y
+CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
+CONFIG_SYSTEMCMDS_SERIAL_TEST=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_USB_CONNECTED=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_EXAMPLES_EXT_UPDATE_CHECKER=y
+CONFIG_EXAMPLES_FAKE_GPS=y
diff --git a/boards/px4/fmu-v5x/rtps.px4board b/boards/px4/fmu-v5x/rtps.px4board
index 2b54d3ec72..2b3b0320d7 100644
--- a/boards/px4/fmu-v5x/rtps.px4board
+++ b/boards/px4/fmu-v5x/rtps.px4board
@@ -8,5 +8,6 @@ CONFIG_SYSTEMCMDS_REFLECT=n
 CONFIG_SYSTEMCMDS_SD_BENCH=n
 CONFIG_SYSTEMCMDS_SD_STRESS=n
 CONFIG_SYSTEMCMDS_SERIAL_TEST=n
+CONFIG_DRIVERS_UAVCAN=n
 CONFIG_MODULES_MICRODDS_CLIENT=y
 CONFIG_MODULES_MICRORTPS_BRIDGE=y
diff --git a/boards/px4/fmu-v5x/src/CMakeLists.txt b/boards/px4/fmu-v5x/src/CMakeLists.txt
index 684b11736e..2566d8c1f3 100644
--- a/boards/px4/fmu-v5x/src/CMakeLists.txt
+++ b/boards/px4/fmu-v5x/src/CMakeLists.txt
@@ -42,6 +42,7 @@ add_library(drivers_board
 	spi.cpp
 	timer_config.cpp
 	usb.c
+	toc.c
 )
 
 add_dependencies(drivers_board platform_gpio_mcp23009)
@@ -57,3 +58,17 @@ target_link_libraries(drivers_board
 		px4_layer
 		platform_gpio_mcp23009
 )
+
+set(manifest_bin_name "mft_v0_0x0010.bin" CACHE INTERNAL "MFT" FORCE)
+set(manifest_bin "${PX4_BINARY_DIR}/romfs_extras/${manifest_bin_name}" CACHE FILEPATH "manifest binary path")
+
+if(EXISTS ${PX4_BOARD_DIR}/manifest/${manifest_bin_name})
+	make_directory(${PX4_BINARY_DIR}/romfs_extras/)
+	file(COPY ${PX4_BOARD_DIR}/manifest/${manifest_bin_name} DESTINATION ${PX4_BINARY_DIR}/romfs_extras/)
+	file(RELATIVE_PATH manifest_bin_relative ${PX4_SOURCE_DIR} ${manifest_bin})
+	add_custom_command(OUTPUT ${manifest_bin}
+		COMMAND ${CMAKE_COMMAND} -E make_directory ${PX4_BINARY_DIR}/etc/extras/
+		COMMAND ${CMAKE_COMMAND} -E copy_if_different ${manifest_bin} ${PX4_BINARY_DIR}/etc/extras/
+		)
+	add_custom_target(copy_manifest_bin DEPENDS ${manifest_bin})
+endif()
diff --git a/boards/px4/fmu-v5x/src/board_config.h b/boards/px4/fmu-v5x/src/board_config.h
index d1055b7692..9a0b3deeb9 100644
--- a/boards/px4/fmu-v5x/src/board_config.h
+++ b/boards/px4/fmu-v5x/src/board_config.h
@@ -111,6 +111,8 @@
  *
  * Note that these are unshifted addresses.
  */
+
+#define BOARD_MTD_NUM_EEPROM        2 /* MTD: base_eeprom, imu_eeprom*/
 #define PX4_I2C_OBDEV_SE050         0x48
 
 #define GPIO_I2C4_DRDY1_BMP388      /* PG5  */  (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTG|GPIO_PIN5)
@@ -182,9 +184,7 @@
 #define GPIO_HW_VER_REV_DRIVE  /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_REV_SENSE      /* PF5 */  ADC3_GPIO(15)
 #define GPIO_HW_VER_SENSE      /* PF4 */  ADC3_GPIO(14)
-#define HW_INFO_INIT           {'V','5','X','x', 'x',0}
-#define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
-#define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */
+#define HW_INFO_INIT_PREFIX           "V5X"
 #define BOARD_NUM_SPI_CFG_HW_VERSIONS 6
 // Base                   FMUM
 #define V5X00   HW_VER_REV(0x0,0x0) // FMUV5X,                         Rev 0
@@ -194,12 +194,16 @@
 #define V5X50   HW_VER_REV(0x5,0x0) // FMUV5X, HB Mini                 Rev 0
 #define V5X51   HW_VER_REV(0x5,0x1) // FMUV5X I2C2 BMP388, HB Mini     Rev 1
 #define V5X52   HW_VER_REV(0x5,0x2) // FMUV5X, HB Mini                 Rev 2
+#define V5X80   HW_VER_REV(0x8,0x0) // USB (Q),                        Rev 0
+#define V5X81   HW_VER_REV(0x8,0x1) // USB (Q) I2C2 BMP388,            Rev 1
+#define V5X82   HW_VER_REV(0x8,0x2) // USB (Q) I2C2 BMP388,            Rev 2
 #define V5X90   HW_VER_REV(0x9,0x0) // NO USB,                         Rev 0
 #define V5X91   HW_VER_REV(0x9,0x1) // NO USB I2C2 BMP388,             Rev 1
 #define V5X92   HW_VER_REV(0x9,0x2) // NO USB I2C2 BMP388,             Rev 2
 #define V5Xa0   HW_VER_REV(0xa,0x0) // NO USB (Q),                     Rev 0
 #define V5Xa1   HW_VER_REV(0xa,0x1) // NO USB (Q) I2C2 BMP388,         Rev 1
 #define V5Xa2   HW_VER_REV(0xa,0x2) // NO USB (Q) I2C2 BMP388,         Rev 2
+#define V5X101  HW_VER_REV(0x10,0x1) // NO USB (Q) I2C2 BMP388,        Rev 1
 
 #define UAVCAN_NUM_IFACES_RUNTIME 1
 
@@ -305,6 +309,7 @@
 #define INPUT_CAP1_TIMER                  5
 #define INPUT_CAP1_CHANNEL     /* T5C4 */ 4
 #define GPIO_INPUT_CAP1        /*  PI0 */ GPIO_TIM5_CH4IN
+#define PPS_CAPTURE_CHANNEL		7 // FMU_CH8-> PI0
 
 /* PWM input driver. Use FMU AUX5 pins attached to timer4 channel 2 */
 #define PWMIN_TIMER                       4
diff --git a/boards/px4/fmu-v5x/src/init.cpp b/boards/px4/fmu-v5x/src/init.cpp
index 1632e5891e..67f17286ef 100644
--- a/boards/px4/fmu-v5x/src/init.cpp
+++ b/boards/px4/fmu-v5x/src/init.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -54,9 +54,7 @@
 #include <errno.h>
 
 #include <nuttx/config.h>
-extern "C" {
 #include <nuttx/board.h>
-}
 #include <nuttx/spi/spi.h>
 #include <nuttx/sdio.h>
 #include <nuttx/mmcsd.h>
@@ -72,6 +70,7 @@ extern "C" {
 #include <systemlib/px4_macros.h>
 #include <px4_arch/io_timer.h>
 #include <px4_platform_common/init.h>
+#include <px4_platform_common/px4_manifest.h>
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_determine_hw_info.h>
 #include <px4_platform/board_dma_alloc.h>
@@ -221,6 +220,13 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 
 	px4_platform_init();
 
+	// Use the default HW_VER_REV(0x0,0x0) for Ramtron
+
+	stm32_spiinitialize();
+
+	/* Configure the HW based on the manifest */
+
+	px4_platform_configure();
 
 	if (OK == board_determine_hw_info()) {
 		syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(),
@@ -230,11 +236,13 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 		syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n");
 	}
 
+	/* Configure the Actual SPI interfaces (after we determined the HW version)  */
+
 	stm32_spiinitialize();
 
 	board_spi_reset(10, 0xffff);
 
-	/* configure the DMA allocator */
+	/* Configure the DMA allocator */
 
 	if (board_dma_alloc_init() < 0) {
 		syslog(LOG_ERR, "[boot] DMA alloc FAILED\n");
@@ -266,13 +274,9 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 
 #endif /* CONFIG_MMCSD */
 
-	/* Configure the HW based on the manifest */
-
-	px4_platform_configure();
-
 	int hw_version = board_get_hw_version();
 
-	if (hw_version == 0x9 || hw_version == 0xa) {
+	if (hw_version == 0x9 || hw_version == 0xa || hw_version == 0x8) {
 		static MCP23009 mcp23009{3, 0x25};
 
 		// No USB
@@ -283,7 +287,7 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 			//ret = mcp23009.init(0xf1, 0xf0, 0x0f);
 		}
 
-		if (hw_version == 0xa) {
+		if (hw_version == 0xa || hw_version == 0x8) {
 			// < P6
 			//ret = mcp23009.init(0xf0, 0xf0, 0x0f);
 			// >= P6
diff --git a/boards/px4/fmu-v5x/src/manifest.c b/boards/px4/fmu-v5x/src/manifest.c
index 91415af45e..9c76d796b1 100644
--- a/boards/px4/fmu-v5x/src/manifest.c
+++ b/boards/px4/fmu-v5x/src/manifest.c
@@ -166,12 +166,16 @@ static px4_hw_mft_list_entry_t mft_lists[] = {
 	{V5X50, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X,   HB Mini       Rev 0
 	{V5X51, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X,   HB Mini       Rev 1
 	{V5X52, hw_mft_list_v0550, arraySize(hw_mft_list_v0550)}, // FMUV5X,   HB Mini       Rev 2
+	{V5X80, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // USB (Q),                Rev 0
+	{V5X81, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // USB (Q) I2C2 BMP388,    Rev 1
+	{V5X82, hw_mft_list_v0500, arraySize(hw_mft_list_v0500)}, // USB (Q) I2C2 BMP388,    Rev 2
 	{V5X90, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB,                 Rev 0
 	{V5X91, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388,     Rev 1
 	{V5X92, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388,     Rev 2
 	{V5Xa0, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q),             Rev 0
 	{V5Xa1, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 1
 	{V5Xa2, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB (Q) I2C2 BMP388, Rev 2
+	{V5X101, hw_mft_list_v0509, arraySize(hw_mft_list_v0509)}, // NO USB I2C2 BMP388,     Rev 1
 };
 
 /************************************************************************************
@@ -194,7 +198,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
 
 	if (boards_manifest == px4_hw_mft_list_uninitialized) {
-		uint32_t ver_rev = board_get_hw_version() << 8;
+		uint32_t ver_rev = board_get_hw_version() << 16;
 		ver_rev |= board_get_hw_revision();
 
 		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
@@ -205,7 +209,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 		}
 
 		if (boards_manifest == px4_hw_mft_list_uninitialized) {
-			syslog(LOG_ERR, "[boot] Board %4"  PRIx32 " is not supported!\n", ver_rev);
+			syslog(LOG_ERR, "[boot] Board %08"  PRIx32 " is not supported!\n", ver_rev);
 		}
 	}
 
diff --git a/boards/px4/fmu-v5x/src/mtd.cpp b/boards/px4/fmu-v5x/src/mtd.cpp
index f0b518699a..fc1e97d2c8 100644
--- a/boards/px4/fmu-v5x/src/mtd.cpp
+++ b/boards/px4/fmu-v5x/src/mtd.cpp
@@ -71,8 +71,8 @@ static const px4_mtd_entry_t base_eeprom = {
 	.npart = 2,
 	.partd = {
 		{
-			.type = MTD_MFT,
-			.path = "/fs/mtd_mft",
+			.type = MTD_MFT_VER,
+			.path = "/fs/mtd_mft_ver",
 			.nblocks = 248
 		},
 		{
@@ -86,12 +86,17 @@ static const px4_mtd_entry_t base_eeprom = {
 
 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 2,
+	.npart = 3,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 248
+			.nblocks = 240
+		},
+		{
+			.type = MTD_MFT_REV,
+			.path = "/fs/mtd_mft_rev",
+			.nblocks = 8
 		},
 		{
 			.type = MTD_ID,
@@ -117,7 +122,9 @@ static const px4_mft_entry_s mtd_mft = {
 
 static const px4_mft_s mft = {
 	.nmft = 1,
-	.mfts = &mtd_mft
+	.mfts = {
+		&mtd_mft
+	}
 };
 
 const px4_mft_s *board_get_manifest(void)
diff --git a/boards/px4/fmu-v5x/src/toc.c b/boards/px4/fmu-v5x/src/toc.c
new file mode 100644
index 0000000000..20a2b22831
--- /dev/null
+++ b/boards/px4/fmu-v5x/src/toc.c
@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 Technology Innovation Institute. 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 <image_toc.h>
+
+/* (Maximum) size of the signature */
+#define SIGNATURE_SIZE 64
+
+/* Boot image starts at _vectors and ends at
+ * the beginning of signature
+*/
+
+extern uint32_t _vectors[];
+extern const int *_boot_signature;
+
+#define APP_ADDR_START _vectors
+#define APP_ADDR_END ((const void *)(&_boot_signature-1))
+
+/* Boot signature start and end are defined by the
+ * signature definition below
+*/
+#define MSTR_SIG_ADDR_START ((const void *)&_boot_signature)
+#define MSTR_SIG_ADDR_END ((const void *)((const uint8_t *)MSTR_SIG_ADDR_START+SIGNATURE_SIZE))
+
+/* The table of contents */
+
+IMAGE_MAIN_TOC(2) = {
+	{TOC_START_MAGIC, TOC_VERSION},
+	{
+		{"MSTR", APP_ADDR_START, APP_ADDR_END, 0, 1, 0, 0, TOC_FLAG1_BOOT | TOC_FLAG1_CHECK_SIGNATURE},
+		{"SIG1", MSTR_SIG_ADDR_START, MSTR_SIG_ADDR_END, 0, 0, 0, 0, 0},
+	},
+	TOC_END_MAGIC
+};
diff --git a/boards/px4/fmu-v5x/test.px4board b/boards/px4/fmu-v5x/test.px4board
index 5279a489dc..41919484d8 100644
--- a/boards/px4/fmu-v5x/test.px4board
+++ b/boards/px4/fmu-v5x/test.px4board
@@ -5,6 +5,7 @@ CONFIG_DRIVERS_PCA9685=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_DRIVERS_RPM=n
 CONFIG_EXAMPLES_FAKE_GPS=n
+CONFIG_DRIVERS_UAVCAN=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
 CONFIG_MODULES_GYRO_FFT=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
diff --git a/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin b/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin
index 36128d00f4..5140391401 100755
Binary files a/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v6c/extras/px4_io-v2_default.bin differ
diff --git a/boards/px4/fmu-v6c/src/board_config.h b/boards/px4/fmu-v6c/src/board_config.h
index d92fa394a0..4f81375289 100644
--- a/boards/px4/fmu-v6c/src/board_config.h
+++ b/boards/px4/fmu-v6c/src/board_config.h
@@ -139,9 +139,7 @@
 #define GPIO_HW_VER_REV_DRIVE  /* PE12 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN12)
 #define GPIO_HW_REV_SENSE      /* PC0 */  GPIO_ADC123_INP10
 #define GPIO_HW_VER_SENSE      /* PC1 */  GPIO_ADC123_INP11
-#define HW_INFO_INIT           {'V','6','C','x', 'x',0}
-#define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
-#define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */
+#define HW_INFO_INIT_PREFIX    "V6C"
 
 #define BOARD_NUM_SPI_CFG_HW_VERSIONS 2 // Rev 0, 10 Sensor sets
 //                 Base/FMUM
diff --git a/boards/px4/fmu-v6c/src/manifest.c b/boards/px4/fmu-v6c/src/manifest.c
index 04d2386163..90b1358e65 100644
--- a/boards/px4/fmu-v6c/src/manifest.c
+++ b/boards/px4/fmu-v6c/src/manifest.c
@@ -121,7 +121,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
 
 	if (boards_manifest == px4_hw_mft_list_uninitialized) {
-		uint32_t ver_rev = board_get_hw_version() << 8;
+		uint32_t ver_rev = board_get_hw_version() << 16;
 		ver_rev |= board_get_hw_revision();
 
 		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
@@ -132,7 +132,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 		}
 
 		if (boards_manifest == px4_hw_mft_list_uninitialized) {
-			syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev);
+			syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev);
 		}
 	}
 
diff --git a/boards/px4/fmu-v6c/src/mtd.cpp b/boards/px4/fmu-v6c/src/mtd.cpp
index 530c7a3bef..b315ff399d 100644
--- a/boards/px4/fmu-v6c/src/mtd.cpp
+++ b/boards/px4/fmu-v6c/src/mtd.cpp
@@ -65,12 +65,17 @@ static const px4_mtd_entry_t fmum_fram = {
 
 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 2,
+	.npart = 3,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 248
+			.nblocks = 240
+		},
+		{
+			.type = MTD_MFT_REV,
+			.path = "/fs/mtd_mft_rev",
+			.nblocks = 8
 		},
 		{
 			.type = MTD_ID,
@@ -95,7 +100,9 @@ static const px4_mft_entry_s mtd_mft = {
 
 static const px4_mft_s mft = {
 	.nmft = 1,
-	.mfts = &mtd_mft
+	.mfts = {
+		&mtd_mft
+	}
 };
 
 const px4_mft_s *board_get_manifest(void)
diff --git a/boards/px4/fmu-v6u/src/CMakeLists.txt b/boards/px4/fmu-v6u/src/CMakeLists.txt
index 7ff9cdf5f5..c84d6f291c 100644
--- a/boards/px4/fmu-v6u/src/CMakeLists.txt
+++ b/boards/px4/fmu-v6u/src/CMakeLists.txt
@@ -60,6 +60,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led # drv_led_start
diff --git a/boards/px4/fmu-v6u/src/board_config.h b/boards/px4/fmu-v6u/src/board_config.h
index 890986ea0e..7fbdc4d7ae 100644
--- a/boards/px4/fmu-v6u/src/board_config.h
+++ b/boards/px4/fmu-v6u/src/board_config.h
@@ -173,9 +173,8 @@
 #define GPIO_HW_VER_REV_DRIVE  /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_REV_SENSE      /* PH4 */  GPIO_ADC3_INP15
 #define GPIO_HW_VER_SENSE      /* PH3 */  GPIO_ADC3_INP14
-#define HW_INFO_INIT           {'V','6','U','x', 'x',0}
-#define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
-#define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */
+#define HW_INFO_INIT_PREFIX   "V6U"
+#define V6U00                 HW_VER_REV(0x0,0x0)
 
 /* HEATER
  * PWM in future
diff --git a/boards/px4/fmu-v6u/src/manifest.c b/boards/px4/fmu-v6u/src/manifest.c
index 761b14efa2..f0cd4d4f12 100644
--- a/boards/px4/fmu-v6u/src/manifest.c
+++ b/boards/px4/fmu-v6u/src/manifest.c
@@ -82,7 +82,7 @@ static const px4_hw_mft_item_t hw_mft_list_v0600[] = {
 };
 
 static px4_hw_mft_list_entry_t mft_lists[] = {
-	{0x0000, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
+	{V6U00, hw_mft_list_v0600, arraySize(hw_mft_list_v0600)},
 };
 
 /************************************************************************************
@@ -105,7 +105,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
 
 	if (boards_manifest == px4_hw_mft_list_uninitialized) {
-		uint32_t ver_rev = board_get_hw_version() << 8;
+		uint32_t ver_rev = board_get_hw_version() << 16;
 		ver_rev |= board_get_hw_revision();
 
 		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
@@ -116,7 +116,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 		}
 
 		if (boards_manifest == px4_hw_mft_list_uninitialized) {
-			syslog(LOG_ERR, "[boot] Board %4"  PRIx32 " is not supported!\n", ver_rev);
+			syslog(LOG_ERR, "[boot] Board %08"  PRIx32 " is not supported!\n", ver_rev);
 		}
 	}
 
diff --git a/boards/px4/fmu-v6u/src/mtd.cpp b/boards/px4/fmu-v6u/src/mtd.cpp
index 765ce93e2d..644b96581d 100644
--- a/boards/px4/fmu-v6u/src/mtd.cpp
+++ b/boards/px4/fmu-v6u/src/mtd.cpp
@@ -71,7 +71,9 @@ static const px4_mft_entry_s mtd_mft = {
 
 static const px4_mft_s mft = {
 	.nmft = 1,
-	.mfts = &mtd_mft
+	.mfts = {
+		&mtd_mft
+	}
 };
 
 const px4_mft_s *board_get_manifest(void)
diff --git a/boards/px4/fmu-v6x/cmake/upload.cmake b/boards/px4/fmu-v6x/cmake/upload.cmake
new file mode 100644
index 0000000000..05d80861e2
--- /dev/null
+++ b/boards/px4/fmu-v6x/cmake/upload.cmake
@@ -0,0 +1,49 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+
+set(PX4_FW_NAME ${PX4_BINARY_DIR}/${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_${PX4_BOARD_LABEL}.px4)
+
+add_custom_target(upload_usb
+	COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh ${PX4_FW_NAME} --default-ip=10.41.1.1
+	DEPENDS ${PX4_FW_NAME}
+	COMMENT "uploading PX4"
+	USES_TERMINAL
+)
+
+add_custom_target(upload_wifi
+	COMMAND ${PX4_SOURCE_DIR}/Tools/auterion/upload_skynode.sh ${PX4_FW_NAME}
+	DEPENDS ${PX4_FW_NAME}
+	COMMENT "uploading PX4"
+	USES_TERMINAL
+)
diff --git a/boards/px4/fmu-v6x/default.px4board b/boards/px4/fmu-v6x/default.px4board
index 3a61af2e07..e133dcc919 100644
--- a/boards/px4/fmu-v6x/default.px4board
+++ b/boards/px4/fmu-v6x/default.px4board
@@ -9,7 +9,6 @@ CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
 CONFIG_DRIVERS_BAROMETER_MS5611=y
-CONFIG_DRIVERS_BATT_SMBUS=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
@@ -34,7 +33,6 @@ CONFIG_DRIVERS_PWM_OUT_SIM=y
 CONFIG_DRIVERS_PX4IO=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_SAFETY_BUTTON=y
-CONFIG_COMMON_TELEMETRY=y
 CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_DRIVERS_UAVCAN=y
 CONFIG_BOARD_UAVCAN_TIMER_OVERRIDE=2
@@ -52,7 +50,6 @@ CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
-CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -80,7 +77,6 @@ CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_MFT=y
 CONFIG_SYSTEMCMDS_MIXER=y
-CONFIG_SYSTEMCMDS_MOTOR_TEST=y
 CONFIG_SYSTEMCMDS_MTD=y
 CONFIG_SYSTEMCMDS_NETMAN=y
 CONFIG_SYSTEMCMDS_NSHTERM=y
diff --git a/boards/px4/fmu-v6x/extras/px4_fmu-v6x_bootloader.bin b/boards/px4/fmu-v6x/extras/px4_fmu-v6x_bootloader.bin
index 0d95b43f8a..7954b66abb 100755
Binary files a/boards/px4/fmu-v6x/extras/px4_fmu-v6x_bootloader.bin and b/boards/px4/fmu-v6x/extras/px4_fmu-v6x_bootloader.bin differ
diff --git a/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin
index 36128d00f4..5140391401 100755
Binary files a/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin and b/boards/px4/fmu-v6x/extras/px4_io-v2_default.bin differ
diff --git a/boards/px4/fmu-v6x/init/rc.board_mavlink b/boards/px4/fmu-v6x/init/rc.board_mavlink
new file mode 100644
index 0000000000..8c9a0d8712
--- /dev/null
+++ b/boards/px4/fmu-v6x/init/rc.board_mavlink
@@ -0,0 +1,7 @@
+#!/bin/sh
+#
+# PX4 FMUv6X specific board MAVLink startup script.
+#------------------------------------------------------------------------------
+
+# Start MAVLink on the USB port
+mavlink start -d /dev/ttyS4 -b 3000000 -r 290000 -m onboard -x -z
diff --git a/boards/px4/fmu-v6x/init/rc.board_sensors b/boards/px4/fmu-v6x/init/rc.board_sensors
index 4facbe4c05..cdf1f67500 100644
--- a/boards/px4/fmu-v6x/init/rc.board_sensors
+++ b/boards/px4/fmu-v6x/init/rc.board_sensors
@@ -4,7 +4,7 @@
 #------------------------------------------------------------------------------
 set HAVE_PM2 yes
 
-if ver hwtypecmp V6X50 V6X51 V6X53 V6X54
+if ver hwtypecmp V6X005000 V6X005001 V6X005003 V6X005004
 then
 	set HAVE_PM2 no
 fi
@@ -47,30 +47,44 @@ then
 	fi
 fi
 
-if ver hwtypecmp V6X04 V6X14 V6X54
+if ver hwtypecmp V6X000000 V6X000004 V6X009000 V6X001004 V6X005004
 then
-	# Internal SPI bus ICM20649
-	icm20649 -s -R 6 start
+  # Skynode detected
+
+  # Internal SPI BMI088
+  bmi088 -A -R 2 -s start
+  bmi088 -G -R 2 -s start
+
+  # Internal SPI bus ICM42688p
+  icm42688p -R 4 -s start
+
+  # Internal SPI bus ICM-20649 (hard-mounted)
+  icm20649 -R 12 -s start
+
+  # Internal magnetometer on I2c
+  bmm150 -I -R 6 start
+
 else
-# Internal SPI BMI088
-	bmi088 -A -R 4 -s start
-	bmi088 -G -R 4 -s start
-fi
+  # v6x base detected
 
-# Internal SPI bus ICM42688p
-icm42688p -R 6 -s start
+  # Internal SPI BMI088
+  bmi088 -A -R 4 -s start
+  bmi088 -G -R 4 -s start
 
-if ver hwtypecmp V6X03 V6X13 V6X04 V6X14 V6X53 V6X54
+  # Internal SPI bus ICM42688p
+  icm42688p -R 6 -s start
+
+if ver hwtypecmp V6X000003 V6X001003 V6X000004 V6X001004 V6X005003 V6X005004
 then
 	# Internal SPI bus ICM-42670-P (hard-mounted)
 	icm42670p -R 10 -s start
 else
 	# Internal SPI bus ICM-20649 (hard-mounted)
 	icm20649 -R 14 -s start
-fi
 
-# Internal magnetometer on I2c
-bmm150 -I start
+  # Internal magnetometer on I2c
+  bmm150 -I start
+fi
 
 # External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
 ist8310 -X -b 1 -R 10 start
@@ -78,14 +92,16 @@ ist8310 -X -b 1 -R 10 start
 # Possible internal Baro
 bmp388 -I -a 0x77 start
 
-if ver hwtypecmp V6X00 V6X10
+if ver hwtypecmp V6X000000 V6X009000 V6X001000
 then
+  # Skynode detected
 	bmp388 -I start
 else
+  # v6x base detected
 	bmp388 -X start
 fi
 
 # Baro on I2C3
 ms5611 -X start
 
-unset HAVE_PM2
\ No newline at end of file
+unset HAVE_PM2
diff --git a/boards/px4/fmu-v6x/recovery.px4board b/boards/px4/fmu-v6x/recovery.px4board
new file mode 100644
index 0000000000..64d49bb733
--- /dev/null
+++ b/boards/px4/fmu-v6x/recovery.px4board
@@ -0,0 +1,47 @@
+CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
+CONFIG_BOARD_ARCHITECTURE="cortex-m7"
+CONFIG_BOARD_ETHERNET=y
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
+CONFIG_BOARD_SERIAL_GPS2="/dev/ttyS7"
+CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS6"
+CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS4"
+CONFIG_BOARD_SERIAL_TEL3="/dev/ttyS1"
+CONFIG_BOARD_ROMFSROOT="recovery"
+CONFIG_DRIVERS_ADC_BOARD_ADC=y
+CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_LAND_DETECTOR=y
+CONFIG_MODULES_MAVLINK=y
+CONFIG_MODULES_NAVIGATOR=y
+CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_ACTUATOR_ENDURANCE_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
+CONFIG_SYSTEMCMDS_DMESG=y
+CONFIG_SYSTEMCMDS_DUMPFILE=y
+CONFIG_SYSTEMCMDS_ESC_CALIB=y
+CONFIG_SYSTEMCMDS_GPIO=y
+CONFIG_SYSTEMCMDS_HARDFAULT_LOG=y
+CONFIG_SYSTEMCMDS_I2CDETECT=y
+CONFIG_SYSTEMCMDS_LED_CONTROL=y
+CONFIG_SYSTEMCMDS_MFT=y
+CONFIG_SYSTEMCMDS_MIXER=y
+CONFIG_SYSTEMCMDS_MOTOR_RAMP=y
+CONFIG_SYSTEMCMDS_MOTOR_TEST=y
+CONFIG_SYSTEMCMDS_MTD=y
+CONFIG_SYSTEMCMDS_NSHTERM=y
+CONFIG_SYSTEMCMDS_PARAM=y
+CONFIG_SYSTEMCMDS_PERF=y
+CONFIG_SYSTEMCMDS_REBOOT=y
+CONFIG_SYSTEMCMDS_REFLECT=y
+CONFIG_SYSTEMCMDS_SD_BENCH=y
+CONFIG_SYSTEMCMDS_SD_STRESS=y
+CONFIG_SYSTEMCMDS_SERIAL_TEST=y
+CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
+CONFIG_SYSTEMCMDS_TOP=y
+CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
+CONFIG_SYSTEMCMDS_TUNE_CONTROL=y
+CONFIG_SYSTEMCMDS_UORB=y
+CONFIG_SYSTEMCMDS_USB_CONNECTED=y
+CONFIG_SYSTEMCMDS_VER=y
+CONFIG_SYSTEMCMDS_WORK_QUEUE=y
+CONFIG_EXAMPLES_EXT_UPDATE_CHECKER=y
+CONFIG_EXAMPLES_FAKE_GPS=y
diff --git a/boards/px4/fmu-v6x/src/CMakeLists.txt b/boards/px4/fmu-v6x/src/CMakeLists.txt
index 7ff9cdf5f5..c84d6f291c 100644
--- a/boards/px4/fmu-v6x/src/CMakeLists.txt
+++ b/boards/px4/fmu-v6x/src/CMakeLists.txt
@@ -60,6 +60,7 @@ else()
 
 	target_link_libraries(drivers_board
 		PRIVATE
+			arch_io_pins
 			arch_spi
 			arch_board_hw_info
 			drivers__led # drv_led_start
diff --git a/boards/px4/fmu-v6x/src/board_config.h b/boards/px4/fmu-v6x/src/board_config.h
index f72f2c134c..8c21f3b0b2 100644
--- a/boards/px4/fmu-v6x/src/board_config.h
+++ b/boards/px4/fmu-v6x/src/board_config.h
@@ -133,6 +133,8 @@
  *
  * Note that these are unshifted addresses.
  */
+#define BOARD_MTD_NUM_EEPROM        2 /* MTD: base_eeprom, imu_eeprom*/
+
 #define PX4_I2C_OBDEV_SE050         0x48
 
 #define GPIO_I2C2_DRDY1_BMP388      /* PG5  */  (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTG|GPIO_PIN5)
@@ -211,9 +213,7 @@
 #define GPIO_HW_VER_REV_DRIVE  /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_REV_SENSE      /* PH4 */  GPIO_ADC3_INP15
 #define GPIO_HW_VER_SENSE      /* PH3 */  GPIO_ADC3_INP14
-#define HW_INFO_INIT           {'V','6','X','x', 'x',0}
-#define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
-#define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */
+#define HW_INFO_INIT_PREFIX    "V6X"
 
 #define BOARD_NUM_SPI_CFG_HW_VERSIONS 6 // Rev 0 and Rev 3,4 Sensor sets
 //                 Base/FMUM
@@ -228,6 +228,7 @@
 #define V6X51   HW_VER_REV(0x5,0x1) // FMUV6X,     BMI388 I2C2    HB Mini Rev 1
 #define V6X53   HW_VER_REV(0x5,0x3) // FMUV6X,     Sensor Set     HB Mini Rev 3
 #define V6X54   HW_VER_REV(0x5,0x4) // FMUV6X,     Sensor Set     HB Mini Rev 4
+#define V6X90   HW_VER_REV(0x9,0x0) //                         Rev 0
 
 #define UAVCAN_NUM_IFACES_RUNTIME  1
 
diff --git a/boards/px4/fmu-v6x/src/hw_config.h b/boards/px4/fmu-v6x/src/hw_config.h
index 57149b1df1..4ad1049fed 100644
--- a/boards/px4/fmu-v6x/src/hw_config.h
+++ b/boards/px4/fmu-v6x/src/hw_config.h
@@ -69,7 +69,7 @@
 
 //#define USE_VBUS_PULL_DOWN
 #define INTERFACE_USART                1
-#define INTERFACE_USART_CONFIG         "/dev/ttyS0,115200"
+#define INTERFACE_USART_CONFIG         "/dev/ttyS0,1500000"
 #define BOOT_DELAY_ADDRESS             0x000001a0
 #define BOARD_TYPE                     53
 #define _FLASH_KBYTES                  (*(uint32_t *)0x1FF1E880)
diff --git a/boards/px4/fmu-v6x/src/init.c b/boards/px4/fmu-v6x/src/init.c
index a96a406496..3dc9a10ee9 100644
--- a/boards/px4/fmu-v6x/src/init.c
+++ b/boards/px4/fmu-v6x/src/init.c
@@ -52,7 +52,6 @@
 #include <string.h>
 #include <debug.h>
 #include <errno.h>
-#include <syslog.h>
 
 #include <nuttx/config.h>
 #include <nuttx/board.h>
@@ -71,6 +70,7 @@
 #include <systemlib/px4_macros.h>
 #include <px4_arch/io_timer.h>
 #include <px4_platform_common/init.h>
+#include <px4_platform_common/px4_manifest.h>
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_determine_hw_info.h>
 #include <px4_platform/board_dma_alloc.h>
@@ -219,6 +219,13 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 
 	px4_platform_init();
 
+	// Use the default HW_VER_REV(0x0,0x0) for Ramtron
+
+	stm32_spiinitialize();
+
+	/* Configure the HW based on the manifest */
+
+	px4_platform_configure();
 
 	if (OK == board_determine_hw_info()) {
 		syslog(LOG_INFO, "[boot] Rev 0x%1x : Ver 0x%1x %s\n", board_get_hw_revision(), board_get_hw_version(),
@@ -228,11 +235,13 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 		syslog(LOG_ERR, "[boot] Failed to read HW revision and version\n");
 	}
 
+	/* Configure the Actual SPI interfaces (after we determined the HW version)  */
+
 	stm32_spiinitialize();
 
 	board_spi_reset(10, 0xffff);
 
-	/* configure the DMA allocator */
+	/* Configure the DMA allocator */
 
 	if (board_dma_alloc_init() < 0) {
 		syslog(LOG_ERR, "[boot] DMA alloc FAILED\n");
@@ -264,13 +273,10 @@ __EXPORT int board_app_initialize(uintptr_t arg)
 
 	if (ret != OK) {
 		led_on(LED_RED);
+		return ret;
 	}
 
 #endif /* CONFIG_MMCSD */
 
-	/* Configure the HW based on the manifest */
-
-	px4_platform_configure();
-
 	return OK;
 }
diff --git a/boards/px4/fmu-v6x/src/manifest.c b/boards/px4/fmu-v6x/src/manifest.c
index c5a3da2831..05a7db55f4 100644
--- a/boards/px4/fmu-v6x/src/manifest.c
+++ b/boards/px4/fmu-v6x/src/manifest.c
@@ -172,7 +172,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
 
 	if (boards_manifest == px4_hw_mft_list_uninitialized) {
-		uint32_t ver_rev = board_get_hw_version() << 8;
+		uint32_t ver_rev = board_get_hw_version() << 16;
 		ver_rev |= board_get_hw_revision();
 
 		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
@@ -183,7 +183,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 		}
 
 		if (boards_manifest == px4_hw_mft_list_uninitialized) {
-			syslog(LOG_ERR, "[boot] Board %4" PRIx32 " is not supported!\n", ver_rev);
+			syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev);
 		}
 	}
 
diff --git a/boards/px4/fmu-v6x/src/mtd.cpp b/boards/px4/fmu-v6x/src/mtd.cpp
index f0b518699a..fc1e97d2c8 100644
--- a/boards/px4/fmu-v6x/src/mtd.cpp
+++ b/boards/px4/fmu-v6x/src/mtd.cpp
@@ -71,8 +71,8 @@ static const px4_mtd_entry_t base_eeprom = {
 	.npart = 2,
 	.partd = {
 		{
-			.type = MTD_MFT,
-			.path = "/fs/mtd_mft",
+			.type = MTD_MFT_VER,
+			.path = "/fs/mtd_mft_ver",
 			.nblocks = 248
 		},
 		{
@@ -86,12 +86,17 @@ static const px4_mtd_entry_t base_eeprom = {
 
 static const px4_mtd_entry_t imu_eeprom = {
 	.device = &i2c4,
-	.npart = 2,
+	.npart = 3,
 	.partd = {
 		{
 			.type = MTD_CALDATA,
 			.path = "/fs/mtd_caldata",
-			.nblocks = 248
+			.nblocks = 240
+		},
+		{
+			.type = MTD_MFT_REV,
+			.path = "/fs/mtd_mft_rev",
+			.nblocks = 8
 		},
 		{
 			.type = MTD_ID,
@@ -117,7 +122,9 @@ static const px4_mft_entry_s mtd_mft = {
 
 static const px4_mft_s mft = {
 	.nmft = 1,
-	.mfts = &mtd_mft
+	.mfts = {
+		&mtd_mft
+	}
 };
 
 const px4_mft_s *board_get_manifest(void)
diff --git a/boards/px4/sitl/default.px4board b/boards/px4/sitl/default.px4board
index fc401133bc..bc3ab551f1 100644
--- a/boards/px4/sitl/default.px4board
+++ b/boards/px4/sitl/default.px4board
@@ -35,6 +35,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
+CONFIG_MODULES_PAYLOAD_DELIVERER=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_REPLAY=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
@@ -52,6 +53,7 @@ CONFIG_SYSTEMCMDS_FAILURE=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
 CONFIG_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_MOTOR_TEST=y
+CONFIG_SYSTEMCMDS_PAIRING_BUTTON_SIM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PERF=y
 CONFIG_SYSTEMCMDS_PWM=y
diff --git a/boards/px4/sitl/src/board_shutdown.cpp b/boards/px4/sitl/src/board_shutdown.cpp
index 464fdc83c1..eeb3e921b7 100644
--- a/boards/px4/sitl/src/board_shutdown.cpp
+++ b/boards/px4/sitl/src/board_shutdown.cpp
@@ -40,6 +40,7 @@
 #include <px4_platform_common/tasks.h>
 #include <board_config.h>
 #include <stdio.h>
+#include <stdlib.h>
 
 #if defined(BOARD_HAS_POWER_CONTROL)
 int board_register_power_state_notification_cb(power_button_state_notification_t cb)
diff --git a/boards/sky-drones/smartap-airlink/src/board_config.h b/boards/sky-drones/smartap-airlink/src/board_config.h
index 024915c055..e03f0d08f1 100644
--- a/boards/sky-drones/smartap-airlink/src/board_config.h
+++ b/boards/sky-drones/smartap-airlink/src/board_config.h
@@ -173,10 +173,8 @@
 #define GPIO_HW_VER_REV_DRIVE  /* PG0 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTG|GPIO_PIN0)
 #define GPIO_HW_REV_SENSE      /* PF5 */  ADC3_GPIO(15)
 #define GPIO_HW_VER_SENSE      /* PF4 */  ADC3_GPIO(14)
-#define HW_INFO_INIT           {'V','5','X','x', 'x',0}
-#define HW_INFO_INIT_VER       3 /* Offset in above string of the VER */
-#define HW_INFO_INIT_REV       4 /* Offset in above string of the REV */
-
+#define HW_INFO_INIT_PREFIX     "SDSA"
+#define SDSA0501                 HW_VER_REV(0x05,0x01)
 /* HEATER
  * PWM in future
  */
diff --git a/boards/sky-drones/smartap-airlink/src/manifest.c b/boards/sky-drones/smartap-airlink/src/manifest.c
index 2f7deac667..ce45fefffd 100644
--- a/boards/sky-drones/smartap-airlink/src/manifest.c
+++ b/boards/sky-drones/smartap-airlink/src/manifest.c
@@ -88,7 +88,7 @@ static const px4_hw_mft_item_t hw_mft_list_v0501[] = {
 
 
 static px4_hw_mft_list_entry_t mft_lists[] = {
-	{0x0501, hw_mft_list_v0501, arraySize(hw_mft_list_v0501)},
+	{SDSA0501, hw_mft_list_v0501, arraySize(hw_mft_list_v0501)},
 };
 
 
@@ -112,7 +112,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;
 
 	if (boards_manifest == px4_hw_mft_list_uninitialized) {
-		uint32_t ver_rev = board_get_hw_version() << 8;
+		uint32_t ver_rev = board_get_hw_version() << 16;
 		ver_rev |= board_get_hw_revision();
 
 		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
@@ -123,7 +123,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 		}
 
 		if (boards_manifest == px4_hw_mft_list_uninitialized) {
-			syslog(LOG_ERR, "[boot] Board %4"  PRIx32 " is not supported!\n", ver_rev);
+			syslog(LOG_ERR, "[boot] Board %08"  PRIx32 " is not supported!\n", ver_rev);
 		}
 	}
 
diff --git a/boards/sky-drones/smartap-airlink/src/mtd.cpp b/boards/sky-drones/smartap-airlink/src/mtd.cpp
index 64006c84e1..8b74a4617c 100644
--- a/boards/sky-drones/smartap-airlink/src/mtd.cpp
+++ b/boards/sky-drones/smartap-airlink/src/mtd.cpp
@@ -66,8 +66,8 @@ static const px4_mtd_entry_t base_eeprom = {
 	.npart = 2,
 	.partd = {
 		{
-			.type = MTD_MFT,
-			.path = "/fs/mtd_mft",
+			.type = MTD_MFT_VER,
+			.path = "/fs/mtd_mft_ver",
 			.nblocks = 248
 		},
 		{
@@ -95,7 +95,9 @@ static const px4_mft_entry_s mtd_mft = {
 
 static const px4_mft_s mft = {
 	.nmft = 1,
-	.mfts = &mtd_mft
+	.mfts = {
+		&mtd_mft
+	}
 };
 
 const px4_mft_s *board_get_manifest(void)
diff --git a/boards/spracing/h7extreme/src/CMakeLists.txt b/boards/spracing/h7extreme/src/CMakeLists.txt
index c118465143..db917c9401 100644
--- a/boards/spracing/h7extreme/src/CMakeLists.txt
+++ b/boards/spracing/h7extreme/src/CMakeLists.txt
@@ -46,6 +46,7 @@ add_library(drivers_board
 
 target_link_libraries(drivers_board
 	PRIVATE
+		arch_io_pins
 		arch_spi
 		drivers__led # drv_led_start
 		nuttx_arch # sdio
diff --git a/codecov.yml b/codecov.yml
new file mode 100644
index 0000000000..227be97cb8
--- /dev/null
+++ b/codecov.yml
@@ -0,0 +1,30 @@
+codecov:
+  require_ci_to_pass: yes
+
+coverage:
+  precision: 2
+  round: down
+  range: "70...100"
+
+  status:
+    project: yes
+    patch: yes
+    changes: no
+
+parsers:
+  gcov:
+    branch_detection:
+      conditional: yes
+      loop: yes
+      method: no
+      macro: no
+
+comment: false
+
+github_checks:
+    annotations: false
+
+ignore:
+  - "src/examples"
+  - "src/modules/attitude_estimator_q"
+  - "src/modules/local_position_estimator"
diff --git a/launch/single_vehicle_spawn.launch b/launch/single_vehicle_spawn.launch
index 9c690af23a..c072f8053e 100644
--- a/launch/single_vehicle_spawn.launch
+++ b/launch/single_vehicle_spawn.launch
@@ -17,14 +17,14 @@
     <env name="PX4_ESTIMATOR" value="$(arg est)" />
     <arg name="mavlink_udp_port" default="14560"/>
     <arg name="mavlink_tcp_port" default="4560"/>
+    <arg name="gst_udp_host" default="172.5.0.7"/>
     <arg name="gst_udp_port" default="5600"/>
     <arg name="video_uri" default="5600"/>
     <arg name="mavlink_cam_udp_port" default="14530"/>
-    <arg name="mavlink_id" value="$(eval 1 + arg('ID'))" />
     <!-- PX4 configs -->
     <arg name="interactive" default="true"/>
     <!-- generate sdf vehicle model -->
-    <arg name="cmd" default="$(find mavlink_sitl_gazebo)/scripts/jinja_gen.py --stdout --mavlink_id=$(arg mavlink_id) --mavlink_udp_port=$(arg mavlink_udp_port) --mavlink_tcp_port=$(arg mavlink_tcp_port) --gst_udp_port=$(arg gst_udp_port) --video_uri=$(arg video_uri) --mavlink_cam_udp_port=$(arg mavlink_cam_udp_port) $(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf.jinja $(find mavlink_sitl_gazebo)"/>
+    <arg name="cmd" default="$(find mavlink_sitl_gazebo)/scripts/jinja_gen.py --stdout --mavlink_id=$(eval 1 + arg('ID')) --mavlink_udp_port=$(arg mavlink_udp_port) --mavlink_tcp_port=$(arg mavlink_tcp_port) --gst_udp_host=$(arg gst_udp_host) --gst_udp_port=$(arg gst_udp_port) --video_uri=$(arg video_uri) --mavlink_cam_udp_port=$(arg mavlink_cam_udp_port) $(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf.jinja $(find mavlink_sitl_gazebo)"/>
     <param command="$(arg cmd)" name="sdf_$(arg vehicle)$(arg ID)"/>
     <!-- PX4 SITL -->
     <arg unless="$(arg interactive)" name="px4_command_arg1" value=""/>
diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 2064d91ba2..d9f1c3ba4c 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -61,6 +61,7 @@ set(msg_files
 	collision_report.msg
 	commander_state.msg
 	control_allocator_status.msg
+	custom_action_status.msg
 	cpuload.msg
 	differential_pressure.msg
 	distance_sensor.msg
@@ -81,7 +82,10 @@ set(msg_files
 	estimator_aid_source_2d.msg
 	estimator_aid_source_3d.msg
 	event.msg
+	figure_eight_status.msg
+	follow_target_status.msg
 	follow_target.msg
+	follow_target_estimator.msg
 	failure_detector_status.msg
 	generator_status.msg
 	geofence_result.msg
@@ -94,6 +98,7 @@ set(msg_files
 	gimbal_manager_status.msg
 	gps_dump.msg
 	gps_inject_data.msg
+	gripper.msg
 	heater_status.msg
 	home_position.msg
 	hover_thrust_estimate.msg
@@ -135,6 +140,7 @@ set(msg_files
 	pwm_input.msg
 	px4io_status.msg
 	radio_status.msg
+	radio_status_extensions.msg
 	rate_ctrl_status.msg
 	rc_channels.msg
 	rc_parameter_map.msg
@@ -185,6 +191,7 @@ set(msg_files
 	vehicle_attitude_setpoint.msg
 	vehicle_command.msg
 	vehicle_command_ack.msg
+	vehicle_command_cancel.msg
 	vehicle_constraints.msg
 	vehicle_control_mode.msg
 	vehicle_global_position.msg
diff --git a/msg/actuator_controls.msg b/msg/actuator_controls.msg
index ec8203c9ab..a5452986af 100644
--- a/msg/actuator_controls.msg
+++ b/msg/actuator_controls.msg
@@ -1,5 +1,5 @@
 uint64 timestamp			# time since system start (microseconds)
-uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROLS = 10
 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
 uint8 INDEX_ROLL = 0
 uint8 INDEX_PITCH = 1
@@ -12,6 +12,7 @@ uint8 INDEX_LANDING_GEAR = 7
 uint8 INDEX_GIMBAL_SHUTTER = 3
 uint8 INDEX_CAMERA_ZOOM = 4
 uint8 INDEX_COLLECTIVE_TILT = 8
+uint8 INDEX_WHEEL = 9
 
 uint8 GROUP_INDEX_ATTITUDE = 0
 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
@@ -20,7 +21,7 @@ uint8 GROUP_INDEX_MANUAL_PASSTHROUGH = 3
 uint8 GROUP_INDEX_PAYLOAD = 6
 
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
-float32[9] control
+float32[10] control
 
 # TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3
 # TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc
diff --git a/msg/airspeed_validated.msg b/msg/airspeed_validated.msg
index 06731cc410..7efee247d9 100644
--- a/msg/airspeed_validated.msg
+++ b/msg/airspeed_validated.msg
@@ -7,6 +7,9 @@ float32 true_airspeed_m_s			# true filtered airspeed in m/s (TAS), set to NAN if
 float32 calibrated_ground_minus_wind_m_s 	# CAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
 float32 true_ground_minus_wind_m_s 		# TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid
 
+float32 calibrated_airspeed_trim_m_s 		# CAS approximated from trim airspeed setting, set to NAN if invalid
+float32 true_airspeed_trim_m_s 			# TAS approximated from trim airspeed setting, set to NAN if invalid
+
 bool airspeed_sensor_measurement_valid 		# True if data from at least one airspeed sensor is declared valid.
 
 int8 selected_airspeed_index 			# 1-3: airspeed sensor index, 0: groundspeed-windspeed, -1: airspeed invalid
diff --git a/msg/battery_status.msg b/msg/battery_status.msg
index 376506d423..276390acf5 100644
--- a/msg/battery_status.msg
+++ b/msg/battery_status.msg
@@ -1,4 +1,7 @@
 uint64 timestamp			# time since system start (microseconds)
+
+uint8 CONNECTED_BATTERIES_MAX = 4 # The number of max supported batteries
+
 bool connected				# Whether or not a battery is connected, based on a voltage threshold
 float32 voltage_v			# Battery voltage in volts, 0 if unknown
 float32 voltage_filtered_v	# Battery voltage in volts, filtered, 0 if unknown
@@ -17,10 +20,12 @@ uint8 BATTERY_SOURCE_EXTERNAL = 1
 uint8 BATTERY_SOURCE_ESCS = 2
 uint8 source				# Battery source
 uint8 priority				# Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1
-uint16 capacity				# actual capacity of the battery
+uint16 capacity         		# actual capacity of the battery in mAh
+uint8 model 				# Battery model
+uint8[2] fw_version			# Firmware version,format MAJOR.MINOR where [0] = MAJOR
 uint16 cycle_count			# number of discharge cycles the battery has experienced
 uint16 average_time_to_empty	# predicted remaining battery capacity based on the average rate of discharge in min
-uint16 serial_number		# serial number of the battery pack
+char[16] serial_number      	# serial number of the battery pack in ASCII characters
 uint16 manufacture_date		# manufacture date, part of serial number of the battery pack. formated as: Day + Month×32 + (Year–1980)×512
 uint16 state_of_health		# state of health. FullChargeCapacity/DesignCapacity, 0-100%.
 uint16 max_error			# max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100%
diff --git a/msg/camera_capture.msg b/msg/camera_capture.msg
index f4491ffefa..141bb2eb69 100644
--- a/msg/camera_capture.msg
+++ b/msg/camera_capture.msg
@@ -5,5 +5,5 @@ float64 lat					# Latitude in degrees (WGS84)
 float64 lon					# Longitude in degrees (WGS84)
 float32 alt					# Altitude (AMSL)
 float32 ground_distance			# Altitude above ground (meters)
-float32[4] q					# Attitude of the camera, zero rotation is facing towards front of vehicle
+float32[4] q					# Attitude of the camera relative to NED earth-fixed frame when using a gimbal, otherwise vehicle attitude
 int8 result					# 1 for success, 0 for failure, -1 if camera does not provide feedback
diff --git a/msg/camera_trigger.msg b/msg/camera_trigger.msg
index 0da1dd109d..a4a41a04bf 100644
--- a/msg/camera_trigger.msg
+++ b/msg/camera_trigger.msg
@@ -6,4 +6,4 @@ bool feedback	# Trigger feedback from camera
 
 uint32 ORB_QUEUE_LENGTH = 2
 
-# TOPICS camera_trigger
\ No newline at end of file
+# TOPICS camera_trigger
diff --git a/msg/commander_state.msg b/msg/commander_state.msg
index ca51625819..a35e634e07 100644
--- a/msg/commander_state.msg
+++ b/msg/commander_state.msg
@@ -17,7 +17,8 @@ uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12
 uint8 MAIN_STATE_AUTO_PRECLAND      = 13
 uint8 MAIN_STATE_ORBIT              = 14
 uint8 MAIN_STATE_AUTO_VTOL_TAKEOFF  = 15
-uint8 MAIN_STATE_MAX                = 16
+uint8 MAIN_STATE_INIT               = 16
+uint8 MAIN_STATE_MAX                = 17
 
 uint8 main_state
 
diff --git a/msg/control_allocator_status.msg b/msg/control_allocator_status.msg
index 3fbbe844ef..2d7b088322 100644
--- a/msg/control_allocator_status.msg
+++ b/msg/control_allocator_status.msg
@@ -1,12 +1,10 @@
 uint64 timestamp                        # time since system start (microseconds)
 
 bool torque_setpoint_achieved           # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
-float32[3] allocated_torque             # Torque allocated to actuators. Equal to `vehicle_torque_setpoint_s::xyz` if the setpoint was achieved.
 float32[3] unallocated_torque           # Unallocated torque. Equal to 0 if the setpoint was achieved.
                                         # Computed as: unallocated_torque = torque_setpoint - allocated_torque
 
 bool thrust_setpoint_achieved           # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved.
-float32[3] allocated_thrust             # Thrust allocated to actuators. Equal to `vehicle_thrust_setpoint_s::xyz` if the setpoint was achieved.
 float32[3] unallocated_thrust           # Unallocated thrust. Equal to 0 if the setpoint was achieved.
                                         # Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust
 
diff --git a/msg/custom_action_status.msg b/msg/custom_action_status.msg
new file mode 100644
index 0000000000..8b0814d6fa
--- /dev/null
+++ b/msg/custom_action_status.msg
@@ -0,0 +1,16 @@
+uint64 timestamp                        # time since system start (microseconds)
+
+uint8 STATUS_ACTION_ACCEPTED     = 0
+uint8 STATUS_ACTION_STARTED      = 1
+uint8 STATUS_ACTION_SUCCEEDED    = 2
+uint8 STATUS_ACTION_CANCELLED    = 3
+uint8 STATUS_ACTION_FAILED       = 4
+uint8 STATUS_ACTION_TIMED_OUT    = 5
+
+int8 REACTION_IGNORE             = 0
+int8 REACTION_LOITER             = 1
+int8 REACTION_RTL                = 2
+
+int8 action_id                          # ID of current action. -1 for none.
+uint8 status                            # Current status of custom actions
+uint8 failure_reaction                  # Reaction performed because of failed custom action
diff --git a/msg/figure_eight_status.msg b/msg/figure_eight_status.msg
new file mode 100644
index 0000000000..e14d8f0d8a
--- /dev/null
+++ b/msg/figure_eight_status.msg
@@ -0,0 +1,8 @@
+uint64 timestamp # time since system start (microseconds)
+float32 major_radius 	# Major axis radius of the figure eight [m]. Positive values orbit clockwise, negative values orbit counter-clockwise.
+float32 minor_radius 	# Minor axis radius of the figure eight [m].
+float32 orientation 	# Orientation of the major axis of the figure eight [rad].
+uint8 frame      # The coordinate system of the fields: x, y, z.
+int32 x          # X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7.
+int32 y        	 # Y coordinate of center point. Coordinate system depends on frame field: local = y position in meters * 1e4, global = latitude in degrees * 1e7.
+float32 z        # Altitude of center point. Coordinate system depends on frame field.
diff --git a/msg/follow_target.msg b/msg/follow_target.msg
index f0afe3f4b8..e88c2460c8 100644
--- a/msg/follow_target.msg
+++ b/msg/follow_target.msg
@@ -1,8 +1,11 @@
-uint64 timestamp		# time since system start (microseconds)
-float64 lat			# target position (deg * 1e7)
-float64 lon			# target position (deg * 1e7) 
-float32 alt			# target position
-float32 vy			# target vel in y
-float32 vx			# target vel in x
-float32 vz			# target vel in z 
-uint8 est_cap # target reporting capabilities
+uint64 timestamp  # time since system start (microseconds)
+
+float64 lat       # target position (deg * 1e7)
+float64 lon       # target position (deg * 1e7)
+float32 alt       # target position
+
+float32 vy        # target vel in y
+float32 vx        # target vel in x
+float32 vz        # target vel in z
+
+uint8 est_cap     # target reporting capabilities
diff --git a/msg/follow_target_estimator.msg b/msg/follow_target_estimator.msg
new file mode 100644
index 0000000000..135434046b
--- /dev/null
+++ b/msg/follow_target_estimator.msg
@@ -0,0 +1,13 @@
+uint64 timestamp                     # time since system start (microseconds)
+uint64 last_filter_reset_timestamp   # time of last filter reset (microseconds)
+
+bool valid              # True if estimator states are okay to be used
+bool stale              # True if estimator stopped receiving follow_target messages for some time. The estimate can still be valid, though it might be inaccurate.
+
+float64 lat_est         # Estimated target latitude
+float64 lon_est         # Estimated target longitude
+float32 alt_est         # Estimated target altitude
+
+float32[3] pos_est      # Estimated target NED position (m)
+float32[3] vel_est      # Estimated target NED velocity (m/s)
+float32[3] acc_est      # Estimated target NED acceleration (m^2/s)
diff --git a/msg/follow_target_status.msg b/msg/follow_target_status.msg
new file mode 100644
index 0000000000..4765588816
--- /dev/null
+++ b/msg/follow_target_status.msg
@@ -0,0 +1,3 @@
+uint64 timestamp             # time since system start (microseconds)
+
+float32[3] pos_est_filtered  # Lowpass-filtered target position used for vehicle's setpoint generation
diff --git a/msg/gps_inject_data.msg b/msg/gps_inject_data.msg
index 1cdaca85ea..1179f3a0e0 100644
--- a/msg/gps_inject_data.msg
+++ b/msg/gps_inject_data.msg
@@ -2,7 +2,7 @@ uint64 timestamp		# time since system start (microseconds)
 
 uint32 device_id                # unique device ID for the sensor that does not change between power cycles
 
-uint8 len                       # length of data
+uint16 len                       # length of data
 uint8 flags                     # LSB: 1=fragmented
 uint8[300] data                 # data to write to GPS device (RTCM message)
 
diff --git a/msg/gripper.msg b/msg/gripper.msg
new file mode 100644
index 0000000000..b1a44b38ea
--- /dev/null
+++ b/msg/gripper.msg
@@ -0,0 +1,7 @@
+## Used to command an actuation in the gripper, which is mapped to a specific output in the mixer module
+
+uint64 timestamp
+
+int8 command		# Commanded state for the gripper
+int8 COMMAND_GRAB = 0
+int8 COMMAND_RELEASE = 1
diff --git a/msg/log_message.msg b/msg/log_message.msg
index 3ea1de23dd..ddfb796c67 100644
--- a/msg/log_message.msg
+++ b/msg/log_message.msg
@@ -6,3 +6,5 @@ uint8 severity # log level (same as in the linux kernel, starting with 0)
 char[127] text
 
 uint8 ORB_QUEUE_LENGTH = 4
+
+# TOPICS log_message log_message_incoming
diff --git a/msg/manual_control_switches.msg b/msg/manual_control_switches.msg
index d343a3ab52..4d1cbab232 100644
--- a/msg/manual_control_switches.msg
+++ b/msg/manual_control_switches.msg
@@ -29,4 +29,6 @@ uint8 transition_switch          # VTOL transition switch: _HOVER, FORWARD_FLIGH
 uint8 photo_switch               # Photo trigger switch
 uint8 video_switch               # Photo trigger switch
 
+uint8 engage_main_motor_switch   # Engage the main motor (for helicopters)
+
 uint32 switch_changes            # number of switch changes
diff --git a/msg/mission.msg b/msg/mission.msg
index 70fc68ccf2..b4f4d27187 100644
--- a/msg/mission.msg
+++ b/msg/mission.msg
@@ -3,3 +3,6 @@ uint8 dataman_id	# default 0, there are two offboard storage places in the datam
 
 uint16 count		# count of the missions stored in the dataman
 int32 current_seq	# default -1, start at the one changed latest
+
+bool valid			# true if mission passed feasibility checks
+bool clear_mission	# true if mission is cleared in dataman
\ No newline at end of file
diff --git a/msg/position_controller_landing_status.msg b/msg/position_controller_landing_status.msg
index 8f5ae9654b..249529b406 100644
--- a/msg/position_controller_landing_status.msg
+++ b/msg/position_controller_landing_status.msg
@@ -1,9 +1,16 @@
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp # [us] time since system start
+float32 lateral_touchdown_offset # [m] lateral touchdown position offset manually commanded during landing
+bool flaring # true if the aircraft is flaring
 
-float32 horizontal_slope_displacement
+# abort status is:
+# 0 if not aborted
+# >0 if aborted, with the singular abort criterion which triggered the landing abort enumerated by the following abort reasons
+uint8 abort_status
 
-float32 slope_angle_rad
-
-float32 flare_length
-
-bool abort_landing				# true if landing should be aborted
+# abort reasons
+# after the manual operator abort, corresponds to individual bits of param FW_LND_ABORT
+uint8 NOT_ABORTED = 0
+uint8 ABORTED_BY_OPERATOR = 1
+uint8 TERRAIN_NOT_FOUND = 2 # FW_LND_ABORT (1 << 0)
+uint8 TERRAIN_TIMEOUT = 3 # FW_LND_ABORT (1 << 1)
+uint8 UNKNOWN_ABORT_CRITERION = 4
diff --git a/msg/position_setpoint.msg b/msg/position_setpoint.msg
index a3a1668a46..2e74de2c67 100644
--- a/msg/position_setpoint.msg
+++ b/msg/position_setpoint.msg
@@ -10,6 +10,9 @@ uint8 SETPOINT_TYPE_LAND=4	# land setpoint, altitude must be ignored, descend un
 uint8 SETPOINT_TYPE_IDLE=5	# do nothing, switch off motors or keep at idle speed (MC)
 uint8 SETPOINT_TYPE_FOLLOW_TARGET=6  # setpoint in NED frame (x, y, z, vx, vy, vz) set by follow target
 
+uint8 LOITER_TYPE_ORBIT=0 	# Circular pattern
+uint8 LOITER_TYPE_FIGUREEIGHT=1 # Pattern resembling an 8
+
 uint8 VELOCITY_FRAME_LOCAL_NED = 1 # MAV_FRAME_LOCAL_NED
 uint8 VELOCITY_FRAME_BODY_NED = 8 # MAV_FRAME_BODY_NED
 
@@ -34,12 +37,16 @@ bool yawspeed_valid		# true if yawspeed setpoint valid
 
 int8 landing_gear		# landing gear: see definition of the states in landing_gear.msg
 
-float32 loiter_radius		# loiter radius (only for fixed wing), in m
-int8 loiter_direction		# loiter direction: 1 = CW, -1 = CCW
+float32 loiter_radius		# loiter major axis radius in m
+float32 loiter_minor_radius	# loiter minor axis radius (used for non-circular loiter shapes) in m
+bool loiter_direction_counter_clockwise # loiter direction is clockwise by default and can be changed using this field
+float32 loiter_orientation 	# Orientation of the major axis with respect to true north in rad [-pi,pi)
+uint8 	loiter_pattern		# loitern pattern to follow
 
 float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoint navigation
 
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
-float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint)
+bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
+float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint), only has an effect for rover
 
 bool disable_weather_vane   # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind
diff --git a/msg/pps_capture.msg b/msg/pps_capture.msg
index b9ec949a9c..c6fa2cb979 100644
--- a/msg/pps_capture.msg
+++ b/msg/pps_capture.msg
@@ -1,2 +1,3 @@
 uint64 timestamp			  # time since system start (microseconds) at PPS capture event
-uint64 rtc_timestamp		# Corrected GPS UTC timestamp at PPS capture event
\ No newline at end of file
+uint64 rtc_timestamp		# Corrected GPS UTC timestamp at PPS capture event
+uint8  pps_rate_exceeded_counter # Increments when PPS dt < 50ms
diff --git a/msg/radio_status_extensions.msg b/msg/radio_status_extensions.msg
new file mode 100644
index 0000000000..751cbe7202
--- /dev/null
+++ b/msg/radio_status_extensions.msg
@@ -0,0 +1,8 @@
+
+uint64 timestamp			# time since system start(microseconds)
+uint8 rssi				# [dB] received signal strength indicator
+uint8 snr				# [dB] signal to noise ratio
+uint8 mcs_index				# multipoint coding scheme index
+uint8 number_spatial_streams		# number of spatial streams
+uint8 queue_size			# queue size, number of packets in tx queue
+uint8 air_time				# percentage of the time the radio is transmitting
diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg
index e44812d70d..13f3cc0753 100644
--- a/msg/rc_channels.msg
+++ b/msg/rc_channels.msg
@@ -27,13 +27,15 @@ uint8 FUNCTION_FLTBTN_SLOT_3 = 23
 uint8 FUNCTION_FLTBTN_SLOT_4 = 24
 uint8 FUNCTION_FLTBTN_SLOT_5 = 25
 uint8 FUNCTION_FLTBTN_SLOT_6 = 26
+uint8 FUNCTION_ENGAGE_MAIN_MOTOR = 27
 
 uint8 FUNCTION_FLTBTN_SLOT_COUNT = 6
 
+
 uint64 timestamp_last_valid					# Timestamp of last valid RC signal
 float32[18] channels						# Scaled to -1..1 (throttle: 0..1)
 uint8 channel_count						# Number of valid channels
-int8[27] function						# Functions mapping
+int8[28] function						# Functions mapping
 uint8 rssi							# Receive signal strength index
 bool signal_lost						# Control signal lost, should be checked together with topic timeout
 uint32 frame_drop_count						# Number of dropped frames
diff --git a/msg/tecs_status.msg b/msg/tecs_status.msg
index 74c47e1a2c..ede2614939 100644
--- a/msg/tecs_status.msg
+++ b/msg/tecs_status.msg
@@ -6,6 +6,7 @@ uint8 TECS_MODE_LAND = 3
 uint8 TECS_MODE_LAND_THROTTLELIM = 4
 uint8 TECS_MODE_BAD_DESCENT = 5
 uint8 TECS_MODE_CLIMBOUT = 6
+uint8 TECS_MODE_ECO= 7
 
 
 float32 altitude_sp
@@ -40,5 +41,12 @@ float32 pitch_integ
 
 float32 throttle_sp
 float32 pitch_sp_rad
+float32 throttle_trim	# estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions
 
 uint8 mode
+
+uint8 flight_phase
+
+uint8 TECS_FLIGHT_PHASE_LEVEL 	= 0		# In level flight (no altitude setpoint change)
+uint8 TECS_FLIGHT_PHASE_CLIMB 	= 1		# Climb is triggered (altitude setpoint is increasing)
+uint8 TECS_FLIGHT_PHASE_DESCEND = 2		# Descend is triggered (altitude setpoint is descreasing)
diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg
index 74a08efe91..cd8f8d6844 100644
--- a/msg/telemetry_status.msg
+++ b/msg/telemetry_status.msg
@@ -45,6 +45,7 @@ bool heartbeat_type_gimbal                  # MAV_TYPE_GIMBAL
 bool heartbeat_type_adsb                    # MAV_TYPE_ADSB
 bool heartbeat_type_camera                  # MAV_TYPE_CAMERA
 bool heartbeat_type_parachute               # MAV_TYPE_PARACHUTE
+bool heartbeat_type_open_drone_id           # MAV_TYPE_ODID
 
 # Heartbeats per component
 bool heartbeat_component_telemetry_radio    # MAV_COMP_ID_TELEMETRY_RADIO
@@ -59,3 +60,4 @@ bool heartbeat_component_uart_bridge        # MAV_COMP_ID_UART_BRIDGE
 # Misc component health
 bool avoidance_system_healthy
 bool parachute_system_healthy
+bool open_drone_id_system_healthy
diff --git a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em
index 1ab5e56e0c..695594d1b0 100644
--- a/msg/templates/uorb_microcdr/microRTPS_client.cpp.em
+++ b/msg/templates/uorb_microcdr/microRTPS_client.cpp.em
@@ -197,7 +197,7 @@ static int launch_send_thread(pthread_t &sender_thread, struct SendThreadArgs &a
 	pthread_attr_setstacksize(&sender_thread_attr, PX4_STACK_ADJUSTED(2250));
 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&sender_thread_attr, &param);
-	param.sched_priority = SCHED_PRIORITY_DEFAULT;
+	param.sched_priority = SCHED_PRIORITY_DEFAULT - 5;
 	(void)pthread_attr_setschedparam(&sender_thread_attr, &param);
 	int rc = pthread_create(&sender_thread, &sender_thread_attr, &send, (void *)&args);
 	if (rc != 0) {
diff --git a/msg/test_motor.msg b/msg/test_motor.msg
index 2dc6174996..5f06c938c3 100644
--- a/msg/test_motor.msg
+++ b/msg/test_motor.msg
@@ -11,4 +11,4 @@ uint32 timeout_ms				# timeout in ms after which to exit test mode (if 0, do not
 
 uint8 driver_instance				# select output driver (for boards with multiple outputs, like IO+FMU)
 
-uint8 ORB_QUEUE_LENGTH = 4
+uint8 ORB_QUEUE_LENGTH = 8
diff --git a/msg/tools/urtps_bridge_topics.yaml b/msg/tools/urtps_bridge_topics.yaml
index 6f6a538a7c..0faa505140 100644
--- a/msg/tools/urtps_bridge_topics.yaml
+++ b/msg/tools/urtps_bridge_topics.yaml
@@ -86,5 +86,3 @@ rtps:
     receive: true
   - msg:     timesync_status
     send:    true
-  - msg:     sensor_combined
-    send:    true
diff --git a/msg/vehicle_command.msg b/msg/vehicle_command.msg
index ffe10ea9d0..56db5f9e8d 100644
--- a/msg/vehicle_command.msg
+++ b/msg/vehicle_command.msg
@@ -1,4 +1,7 @@
-uint64 timestamp						# time since system start (microseconds)
+# Vehicle Command uORB message. Used for commanding a mission / action / etc.
+# Follows the MAVLink COMMAND_INT / COMMAND_LONG definition
+
+uint64 timestamp					# time since system start (microseconds)
 
 uint16 VEHICLE_CMD_CUSTOM_0 = 0				# test command
 uint16 VEHICLE_CMD_CUSTOM_1 = 1				# test command
@@ -11,7 +14,8 @@ uint16 VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20		# Return to launch location |Empty
 uint16 VEHICLE_CMD_NAV_LAND = 21			# Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude|
 uint16 VEHICLE_CMD_NAV_TAKEOFF = 22			# Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|
 uint16 VEHICLE_CMD_NAV_PRECLAND = 23			# Attempt a precision landing
-uint16 VEHICLE_CMD_DO_ORBIT = 34            # Start orbiting on the circumference of a circle defined by the parameters. |Radius [m] |Velocity [m/s] |Yaw behaviour |Empty |Latitude/X |Longitude/Y |Altitude/Z |
+uint16 VEHICLE_CMD_DO_ORBIT = 34            		# Start orbiting on the circumference of a circle defined by the parameters. |Radius [m] |Velocity [m/s] |Yaw behaviour |Empty |Latitude/X |Longitude/Y |Altitude/Z |
+uint16 VEHICLE_CMD_DO_FIGUREEIGHT = 35        		# Start flying on the outline of a figure eight defined by the parameters. |Major Radius [m] |Minor Radius [m] |Velocity [m/s] |Orientation |Latitude/X |Longitude/Y |Altitude/Z |
 uint16 VEHICLE_CMD_NAV_ROI = 80				# Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
 uint16 VEHICLE_CMD_NAV_PATHPLANNING = 81		# Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal|
 uint16 VEHICLE_CMD_NAV_VTOL_TAKEOFF = 84		# Takeoff from ground / hand and transition to fixed wing |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude|
@@ -24,7 +28,7 @@ uint16 VEHICLE_CMD_CONDITION_DELAY = 112		# Delay mission state machine. |Delay
 uint16 VEHICLE_CMD_CONDITION_CHANGE_ALT = 113		# Ascend/descend at rate.  Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude|
 uint16 VEHICLE_CMD_CONDITION_DISTANCE = 114		# Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_CONDITION_YAW = 115			# Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty|
-uint16 VEHICLE_CMD_CONDITION_LAST = 159		# NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|
+uint16 VEHICLE_CMD_CONDITION_LAST = 159			# NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_CONDITION_GATE = 4501		# Wait until passing a threshold |2D coord mode: 0: Orthogonal to planned route | Altitude mode: 0: Ignore altitude| Empty| Empty| Lat| Lon| Alt|
 uint16 VEHICLE_CMD_DO_SET_MODE = 176			# Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_JUMP = 177			# Jump to the desired command in the mission list.  Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty|
@@ -41,11 +45,11 @@ uint16 VEHICLE_CMD_DO_LAND_START = 189			# Mission command to perform a landing.
 uint16 VEHICLE_CMD_DO_GO_AROUND = 191			# Mission command to safely abort an autonmous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_REPOSITION = 192
 uint16 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193
-uint16 VEHICLE_CMD_DO_SET_ROI_LOCATION = 195	# Sets the region of interest (ROI) to a location. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Latitude| Longitude| Altitude|
-uint16 VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET = 196 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| pitch offset from next waypoint| roll offset from next waypoint| yaw offset from next waypoint|
+uint16 VEHICLE_CMD_DO_SET_ROI_LOCATION = 195		# Sets the region of interest (ROI) to a location. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Latitude| Longitude| Altitude|
+uint16 VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET = 196 	# Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| pitch offset from next waypoint| roll offset from next waypoint| yaw offset from next waypoint|
 uint16 VEHICLE_CMD_DO_SET_ROI_NONE = 197		# Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_CONTROL_VIDEO = 200		# Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
-uint16 VEHICLE_CMD_DO_SET_ROI = 201				# Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
+uint16 VEHICLE_CMD_DO_SET_ROI = 201			# Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z|
 uint16 VEHICLE_CMD_DO_DIGICAM_CONTROL=203
 uint16 VEHICLE_CMD_DO_MOUNT_CONFIGURE=204		# Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_MOUNT_CONTROL=205			# Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value|
@@ -54,18 +58,20 @@ uint16 VEHICLE_CMD_DO_FENCE_ENABLE=207			# Mission command to enable the geofenc
 uint16 VEHICLE_CMD_DO_PARACHUTE=208			# Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_MOTOR_TEST=209			# motor test command |Instance (1, ...)| throttle type| throttle| timeout [s]| Motor count | Test order| Empty|
 uint16 VEHICLE_CMD_DO_INVERTED_FLIGHT=210		# Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty|
-uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL=214		# Mission command to set TRIG_INTERVAL for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty|
+uint16 VEHICLE_CMD_DO_GRIPPER = 211			# Command to operate a gripper
+uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL=214	# Mission command to set TRIG_INTERVAL for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220		# Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_GUIDED_MASTER=221			# set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_GUIDED_LIMITS=222			# set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue.  0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue.  0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_DO_LAST = 240			# NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241		# Trigger calibration. This command will be only accepted if in pre-flight mode. See mavlink spec MAV_CMD_PREFLIGHT_CALIBRATION
-uint16 PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION = 3		# param value for VEHICLE_CMD_PREFLIGHT_CALIBRATION to start temperature calibration
+uint16 PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION = 3# param value for VEHICLE_CMD_PREFLIGHT_CALIBRATION to start temperature calibration
 uint16 VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242	# Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units|
 uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243		# UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started
 uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245		# Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246	# Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty|
 uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260			# Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty|
+uint16 VEHICLE_CMD_PREFLIGHT_ACTUATOR_TEST = 247	# Command the vehicle to perform specific actuator test which include fixed wing control surfaces and tiltrotor tilt mechanism. | axis - #param1, 1 Roll, 2 Pitch, 3 Yaw, 4 Tilt | Direction - #param2 0: Command positive actuation, 1: Command negative actuation. |
 uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal
 
 uint16 VEHICLE_CMD_MISSION_START = 300			# start running a mission |first_item: the first mission item to run| last_item:  the last mission item to run (after this item is run, the mission ends)|
@@ -74,57 +80,51 @@ uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311		# Actuator configuration command|co
 uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400		# Arms / Disarms a component |1 to arm, 0 to disarm
 uint16 VEHICLE_CMD_INJECT_FAILURE = 420			# Inject artificial failure for testing purposes
 uint16 VEHICLE_CMD_START_RX_PAIR = 500			# Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX|
-uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512    # Request to send a single instance of the specified message
-uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530            # Set camera capture mode (photo, video, etc.)
-uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531                # Set camera zoom
+uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512    		# Request to send a single instance of the specified message
+uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530		# Set camera capture mode (photo, video, etc.)
+uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531		# Set camera zoom
 uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532
-uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw
-uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control
-uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000   # Start image capture sequence.
-uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003            # Enable or disable on-board camera triggering system
-uint16 VEHICLE_CMD_VIDEO_START_CAPTURE = 2500   # Start a video capture.
-uint16 VEHICLE_CMD_VIDEO_STOP_CAPTURE = 2501    # Stop the current video capture.
-uint16 VEHICLE_CMD_LOGGING_START = 2510		# start streaming ULog data
+uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 	# Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw
+uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 	# Gimbal configuration to set which sysid/compid is in primary and secondary control
+uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000   	# Start image capture sequence.
+uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003		# Enable or disable on-board camera triggering system
+uint16 VEHICLE_CMD_VIDEO_START_CAPTURE = 2500   	# Start a video capture.
+uint16 VEHICLE_CMD_VIDEO_STOP_CAPTURE = 2501    	# Stop the current video capture.
+uint16 VEHICLE_CMD_LOGGING_START = 2510			# start streaming ULog data
 uint16 VEHICLE_CMD_LOGGING_STOP = 2511			# stop streaming ULog data
-uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600	# control starting/stopping transmitting data over the high latency link
-uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000    # Command VTOL transition
-uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001    # Request arm authorization
+uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600		# control starting/stopping transmitting data over the high latency link
+uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000    	# Command VTOL transition
+uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001	# Request arm authorization
 uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001	# Prepare a payload deployment in the flight plan
 uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed payload deployment
 uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006            # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location.
 
+uint16 VEHICLE_CMD_NAV_WAYPOINT_USER_1 = 31000
+
+uint16 VEHICLE_CMD_DO_WINCH = 42600			# Command to operate winch.
 
 # PX4 vehicle commands (beyond 16 bit mavlink commands)
 uint32 VEHICLE_CMD_PX4_INTERNAL_START    = 65537        # start of PX4 internal only vehicle commands (> UINT16_MAX)
 uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000       # Sets the GPS co-ordinates of the vehicle local origin (0,0,0) position. |Empty|Empty|Empty|Empty|Latitude|Longitude|Altitude|
 
-
-uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
-uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |
-uint8 VEHICLE_CMD_RESULT_DENIED = 2			# Command PERMANENTLY DENIED |
-uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3		# Command UNKNOWN/UNSUPPORTED |
-uint8 VEHICLE_CMD_RESULT_FAILED = 4			# Command executed, but failed |
-uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5		# Command being executed |
-uint8 VEHICLE_CMD_RESULT_ENUM_END = 6			#
-
-uint8 VEHICLE_MOUNT_MODE_RETRACT = 0				# Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization |
-uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1				# Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. |
+uint8 VEHICLE_MOUNT_MODE_RETRACT = 0			# Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization |
+uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1			# Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. |
 uint8 VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2		# Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization |
-uint8 VEHICLE_MOUNT_MODE_RC_TARGETING = 3			# Load neutral position and start RC Roll,Pitch,Yaw control with stabilization |
-uint8 VEHICLE_MOUNT_MODE_GPS_POINT = 4				# Load neutral position and start to point to Lat,Lon,Alt |
-uint8 VEHICLE_MOUNT_MODE_ENUM_END = 5				#
+uint8 VEHICLE_MOUNT_MODE_RC_TARGETING = 3		# Load neutral position and start RC Roll,Pitch,Yaw control with stabilization |
+uint8 VEHICLE_MOUNT_MODE_GPS_POINT = 4			# Load neutral position and start to point to Lat,Lon,Alt |
+uint8 VEHICLE_MOUNT_MODE_ENUM_END = 5			#
 
-uint8 VEHICLE_ROI_NONE = 0                         # No region of interest |
-uint8 VEHICLE_ROI_WPNEXT = 1                       # Point toward next MISSION |
-uint8 VEHICLE_ROI_WPINDEX = 2                      # Point toward given MISSION |
-uint8 VEHICLE_ROI_LOCATION = 3                     # Point toward fixed location |
-uint8 VEHICLE_ROI_TARGET = 4                       # Point toward target
+uint8 VEHICLE_ROI_NONE = 0                         	# No region of interest |
+uint8 VEHICLE_ROI_WPNEXT = 1                       	# Point toward next MISSION |
+uint8 VEHICLE_ROI_WPINDEX = 2                     	# Point toward given MISSION |
+uint8 VEHICLE_ROI_LOCATION = 3                    	# Point toward fixed location |
+uint8 VEHICLE_ROI_TARGET = 4                       	# Point toward target
 uint8 VEHICLE_ROI_ENUM_END = 5
 
-uint8 VEHICLE_CAMERA_ZOOM_TYPE_STEP = 0            # Zoom one step increment
-uint8 VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS = 1      # Continuous zoom up/down until stopped
-uint8 VEHICLE_CAMERA_ZOOM_TYPE_RANGE = 2           # Zoom value as proportion of full camera range
-uint8 VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH = 3    # Zoom to a focal length
+uint8 VEHICLE_CAMERA_ZOOM_TYPE_STEP = 0            	# Zoom one step increment
+uint8 VEHICLE_CAMERA_ZOOM_TYPE_CONTINUOUS = 1      	# Continuous zoom up/down until stopped
+uint8 VEHICLE_CAMERA_ZOOM_TYPE_RANGE = 2           	# Zoom value as proportion of full camera range
+uint8 VEHICLE_CAMERA_ZOOM_TYPE_FOCAL_LENGTH = 3    	# Zoom to a focal length
 
 uint8 PARACHUTE_ACTION_DISABLE = 0
 uint8 PARACHUTE_ACTION_ENABLE = 1
@@ -165,6 +165,10 @@ uint8 SPEED_TYPE_DESCEND_SPEED = 3
 int8 ARMING_ACTION_DISARM = 0
 int8 ARMING_ACTION_ARM = 1
 
+# param2 in VEHICLE_CMD_DO_GRIPPER
+uint8 GRIPPER_ACTION_RELEASE = 0
+uint8 GRIPPER_ACTION_GRAB = 1
+
 uint8 ORB_QUEUE_LENGTH = 8
 
 float32 param1			# Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum.
diff --git a/msg/vehicle_command_ack.msg b/msg/vehicle_command_ack.msg
index 9f52a8d62b..3ce250d949 100644
--- a/msg/vehicle_command_ack.msg
+++ b/msg/vehicle_command_ack.msg
@@ -1,11 +1,19 @@
-uint64 timestamp		# time since system start (microseconds)
-uint8 VEHICLE_RESULT_ACCEPTED = 0
-uint8 VEHICLE_RESULT_TEMPORARILY_REJECTED = 1
-uint8 VEHICLE_RESULT_DENIED = 2
-uint8 VEHICLE_RESULT_UNSUPPORTED = 3
-uint8 VEHICLE_RESULT_FAILED = 4
-uint8 VEHICLE_RESULT_IN_PROGRESS = 5
+# Vehicle Command Ackonwledgement uORB message.
+# Used for acknowledging the vehicle command being received.
+# Follows the MAVLink COMMAND_ACK message definition
 
+uint64 timestamp		# time since system start (microseconds)
+
+# Result cases. This follows the MAVLink MAV_RESULT enum definition
+uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0			# Command ACCEPTED and EXECUTED |
+uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1	# Command TEMPORARY REJECTED/DENIED |
+uint8 VEHICLE_CMD_RESULT_DENIED = 2			# Command PERMANENTLY DENIED |
+uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3		# Command UNKNOWN/UNSUPPORTED |
+uint8 VEHICLE_CMD_RESULT_FAILED = 4			# Command executed, but failed |
+uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5		# Command being executed |
+uint8 VEHICLE_CMD_RESULT_CANCELLED = 6			# Command Canceled
+
+# Arming denied specific cases
 uint16 ARM_AUTH_DENIED_REASON_GENERIC = 0
 uint16 ARM_AUTH_DENIED_REASON_NONE = 1
 uint16 ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT = 2
@@ -13,12 +21,13 @@ uint16 ARM_AUTH_DENIED_REASON_TIMEOUT = 3
 uint16 ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4
 uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5
 
-uint8 ORB_QUEUE_LENGTH = 4
+uint8 ORB_QUEUE_LENGTH = 8
 
-uint32 command
-uint8 result
-bool from_external
-uint8 result_param1
-int32 result_param2
+uint32 command						# Command that is being acknowledged
+uint8 result						# Command result
+uint8 result_param1					# Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS
+int32 result_param2					# Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied.
 uint8 target_system
 uint8 target_component
+
+bool from_external					# Indicates if the command came from an external source
diff --git a/msg/vehicle_command_cancel.msg b/msg/vehicle_command_cancel.msg
new file mode 100644
index 0000000000..183e2822fa
--- /dev/null
+++ b/msg/vehicle_command_cancel.msg
@@ -0,0 +1,7 @@
+uint64 timestamp		# time since system start (microseconds)
+
+uint8 ORB_QUEUE_LENGTH = 3
+
+uint16 command
+uint8 target_system
+uint8 target_component
diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg
index e5c87169b5..85b32a5f63 100644
--- a/msg/vehicle_gps_position.msg
+++ b/msg/vehicle_gps_position.msg
@@ -18,8 +18,13 @@ float32 hdop			# Horizontal dilution of precision
 float32 vdop			# Vertical dilution of precision
 
 int32 noise_per_ms		# GPS noise per millisecond
-int32 jamming_indicator		# indicates jamming is occurring
+
+uint8 JAMMING_STATE_UNKNOWN  = 0
+uint8 JAMMING_STATE_OK       = 1
+uint8 JAMMING_STATE_WARNING  = 2
+uint8 JAMMING_STATE_CRITICAL = 3
 uint8 jamming_state		# indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical
+int32 jamming_indicator		# indicates jamming is occurring
 
 float32 vel_m_s			# GPS ground speed, (metres/sec)
 float32 vel_n_m_s		# GPS North velocity, (metres/sec)
diff --git a/msg/vehicle_land_detected.msg b/msg/vehicle_land_detected.msg
index a35a8414c3..fc0ca4a6d8 100644
--- a/msg/vehicle_land_detected.msg
+++ b/msg/vehicle_land_detected.msg
@@ -12,6 +12,7 @@ bool has_low_throttle
 
 bool vertical_movement
 bool horizontal_movement
+bool rotational_movement
 
 bool close_to_ground_or_skipped_check
 
diff --git a/msg/vehicle_local_position.msg b/msg/vehicle_local_position.msg
index 981fec4ba8..a638bbb61c 100644
--- a/msg/vehicle_local_position.msg
+++ b/msg/vehicle_local_position.msg
@@ -62,7 +62,9 @@ uint8 DIST_BOTTOM_SENSOR_FLOW = 2	# (1 << 1) a flow sensor is used to estimate d
 float32 eph				# Standard deviation of horizontal position error, (metres)
 float32 epv				# Standard deviation of vertical position error, (metres)
 float32 evh				# Standard deviation of horizontal velocity error, (metres/sec)
-float32 evv				# Standard deviation of horizontal velocity error, (metres/sec)
+float32 evv				# Standard deviation of vertical velocity error, (metres/sec)
+
+bool dead_reckoning                     # True if this position is estimated through dead-reckoning
 
 # estimator specified vehicle limits
 float32 vxy_max				# maximum horizontal speed - set to 0 when limiting not required (meters/sec)
diff --git a/msg/vehicle_rates_setpoint.msg b/msg/vehicle_rates_setpoint.msg
index ae7a3c7822..f30ae2fc8f 100644
--- a/msg/vehicle_rates_setpoint.msg
+++ b/msg/vehicle_rates_setpoint.msg
@@ -1,8 +1,9 @@
 uint64 timestamp	# time since system start (microseconds)
 
-float32 roll		# body angular rates in NED frame
-float32 pitch		# body angular rates in NED frame
-float32 yaw		# body angular rates in NED frame
+# body angular rates in NED frame
+float32 roll		# [rad/s] roll rate setpoint
+float32 pitch		# [rad/s] pitch rate setpoint
+float32 yaw		# [rad/s] yaw rate setpoint
 
 # For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand.
 # For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero.
diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg
index 902d07a478..4671ae3672 100644
--- a/msg/vehicle_status.msg
+++ b/msg/vehicle_status.msg
@@ -1,5 +1,11 @@
-uint64 timestamp				# time since system start (microseconds)
+# Encodes the system state of the vehicle published by commander
 
+uint64 timestamp # time since system start (microseconds)
+
+uint64 armed_time # Arming timestamp (microseconds)
+uint64 takeoff_time # Takeoff timestamp (microseconds)
+
+uint8 arming_state
 uint8 ARMING_STATE_INIT = 0
 uint8 ARMING_STATE_STANDBY = 1
 uint8 ARMING_STATE_ARMED = 2
@@ -8,93 +14,8 @@ uint8 ARMING_STATE_SHUTDOWN = 4
 uint8 ARMING_STATE_IN_AIR_RESTORE = 5
 uint8 ARMING_STATE_MAX = 6
 
-# FailureDetector status
-uint16 FAILURE_NONE = 0
-uint16 FAILURE_ROLL = 1              # (1 << 0)
-uint16 FAILURE_PITCH = 2             # (1 << 1)
-uint16 FAILURE_ALT = 4               # (1 << 2)
-uint16 FAILURE_EXT = 8               # (1 << 3)
-uint16 FAILURE_ARM_ESC = 16          # (1 << 4)
-uint16 FAILURE_BATTERY = 32          # (1 << 5)
-uint16 FAILURE_IMBALANCED_PROP = 64  # (1 << 6)
-uint16 FAILURE_MOTOR = 128           # (1 << 7)
-
-# HIL
-uint8 HIL_STATE_OFF = 0
-uint8 HIL_STATE_ON = 1
-
-# Navigation state, i.e. "what should vehicle do".
-uint8 NAVIGATION_STATE_MANUAL = 0		# Manual mode
-uint8 NAVIGATION_STATE_ALTCTL = 1		# Altitude control mode
-uint8 NAVIGATION_STATE_POSCTL = 2		# Position control mode
-uint8 NAVIGATION_STATE_AUTO_MISSION = 3		# Auto mission mode
-uint8 NAVIGATION_STATE_AUTO_LOITER = 4		# Auto loiter mode
-uint8 NAVIGATION_STATE_AUTO_RTL = 5		# Auto return to launch mode
-uint8 NAVIGATION_STATE_UNUSED3 = 8 	        # Free slot
-uint8 NAVIGATION_STATE_UNUSED = 9	        # Free slot
-uint8 NAVIGATION_STATE_ACRO = 10		# Acro mode
-uint8 NAVIGATION_STATE_UNUSED1 = 11		# Free slot
-uint8 NAVIGATION_STATE_DESCEND = 12		# Descend mode (no position control)
-uint8 NAVIGATION_STATE_TERMINATION = 13		# Termination mode
-uint8 NAVIGATION_STATE_OFFBOARD = 14
-uint8 NAVIGATION_STATE_STAB = 15		# Stabilized mode
-uint8 NAVIGATION_STATE_UNUSED2 = 16		# Free slot
-uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17	# Takeoff
-uint8 NAVIGATION_STATE_AUTO_LAND = 18		# Land
-uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19	# Auto Follow
-uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20	# Precision land with landing target
-uint8 NAVIGATION_STATE_ORBIT = 21       # Orbit in a circle
-uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22   # Takeoff, transition, establish loiter
-uint8 NAVIGATION_STATE_MAX = 23
-
-uint8 VEHICLE_TYPE_UNKNOWN = 0
-uint8 VEHICLE_TYPE_ROTARY_WING = 1
-uint8 VEHICLE_TYPE_FIXED_WING = 2
-uint8 VEHICLE_TYPE_ROVER = 3
-uint8 VEHICLE_TYPE_AIRSHIP = 4
-
-# state machine / state of vehicle.
-# Encodes the complete system state and is set by the commander app.
-
-uint8 nav_state				# set navigation state machine to specified value
-uint64 nav_state_timestamp              # time when current nav_state activated
-uint8 arming_state			# current arming state
-uint8 hil_state				# current hil state
-bool failsafe				# true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
-uint64 failsafe_timestamp               # time when failsafe was activated
-
-uint8 system_type			# system type, contains mavlink MAV_TYPE
-uint8 system_id			# system id, contains MAVLink's system ID field
-uint8 component_id			# subsystem / component id, contains MAVLink's component ID field
-
-uint8 vehicle_type          # Type of vehicle (rotary wing/fixed-wing/rover/airship, see above)
-                            # If the vehicle is a VTOL, then this value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter,
-                            # and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing
-
-bool is_vtol				# True if the system is VTOL capable
-bool is_vtol_tailsitter                 # True if the system performs a 90° pitch down rotation during transition from MC to FW
-bool in_transition_mode			# True if VTOL is doing a transition
-bool in_transition_to_fw		# True if VTOL is doing a transition from MC to FW
-
-bool rc_signal_lost				# true if RC reception lost
-
-bool data_link_lost				# datalink to GCS lost
-uint8 data_link_lost_counter			# counts unique data link lost events
-
-bool high_latency_data_link_lost 			# Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost
-
-bool mission_failure				# Set to true if mission could not continue/finish
-bool geofence_violated
-
-uint16 failure_detector_status			# Bitmask containing FailureDetector status [0, 0, 0, 0, 0, FAILURE_ALT, FAILURE_PITCH, FAILURE_ROLL]
-
-# see SYS_STATUS mavlink message for the following
-# lower 32 bits are for the base flags, higher 32 bits are or the extended flags
-uint64 onboard_control_sensors_present
-uint64 onboard_control_sensors_enabled
-uint64 onboard_control_sensors_health
-
-
+uint8 latest_arming_reason
+uint8 latest_disarming_reason
 uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0
 uint8 ARM_DISARM_REASON_RC_STICK = 1
 uint8 ARM_DISARM_REASON_RC_SWITCH = 2
@@ -110,12 +31,87 @@ uint8 ARM_DISARM_REASON_FAILURE_DETECTOR = 11
 uint8 ARM_DISARM_REASON_SHUTDOWN = 12
 uint8 ARM_DISARM_REASON_UNIT_TEST = 13
 
-uint8 latest_arming_reason
-uint8 latest_disarming_reason
+uint64 nav_state_timestamp # time when current nav_state activated
 
-uint64 armed_time       # Arming timestamp (microseconds)
+# Navigation state "what should vehicle do"
+uint8 nav_state
+uint8 NAVIGATION_STATE_MANUAL = 0		# Manual mode
+uint8 NAVIGATION_STATE_ALTCTL = 1		# Altitude control mode
+uint8 NAVIGATION_STATE_POSCTL = 2		# Position control mode
+uint8 NAVIGATION_STATE_AUTO_MISSION = 3		# Auto mission mode
+uint8 NAVIGATION_STATE_AUTO_LOITER = 4		# Auto loiter mode
+uint8 NAVIGATION_STATE_AUTO_RTL = 5		# Auto return to launch mode
+uint8 NAVIGATION_STATE_UNUSED3 = 8 	        # Free slot
+uint8 NAVIGATION_STATE_UNUSED = 9	        # Free slot
+uint8 NAVIGATION_STATE_ACRO = 10		# Acro mode
+uint8 NAVIGATION_STATE_UNUSED4 = 11		# Free slot
+uint8 NAVIGATION_STATE_DESCEND = 12		# Descend mode (no position control)
+uint8 NAVIGATION_STATE_TERMINATION = 13		# Termination mode
+uint8 NAVIGATION_STATE_OFFBOARD = 14
+uint8 NAVIGATION_STATE_STAB = 15		# Stabilized mode
+uint8 NAVIGATION_STATE_UNUSED2 = 16		# Free slot
+uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17	# Takeoff
+uint8 NAVIGATION_STATE_AUTO_LAND = 18		# Land
+uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19	# Auto Follow
+uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20	# Precision land with landing target
+uint8 NAVIGATION_STATE_ORBIT = 21       # Orbit in a circle
+uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22   # Takeoff, transition, establish loiter
+uint8 NAVIGATION_STATE_INIT = 23
+uint8 NAVIGATION_STATE_MAX = 24
 
-uint64 takeoff_time     # Takeoff timestamp (microseconds)
+# Bitmask of detected failures
+uint16 failure_detector_status
+uint16 FAILURE_NONE = 0
+uint16 FAILURE_ROLL = 1              # (1 << 0)
+uint16 FAILURE_PITCH = 2             # (1 << 1)
+uint16 FAILURE_ALT = 4               # (1 << 2)
+uint16 FAILURE_EXT = 8               # (1 << 3)
+uint16 FAILURE_ARM_ESC = 16          # (1 << 4)
+uint16 FAILURE_BATTERY = 32          # (1 << 5)
+uint16 FAILURE_IMBALANCED_PROP = 64  # (1 << 6)
+uint16 FAILURE_MOTOR = 128           # (1 << 7)
+
+uint8 hil_state
+uint8 HIL_STATE_OFF = 0
+uint8 HIL_STATE_ON = 1
+
+# If it's a VTOL, then the value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing
+uint8 vehicle_type
+uint8 VEHICLE_TYPE_UNKNOWN = 0
+uint8 VEHICLE_TYPE_ROTARY_WING = 1
+uint8 VEHICLE_TYPE_FIXED_WING = 2
+uint8 VEHICLE_TYPE_ROVER = 3
+uint8 VEHICLE_TYPE_AIRSHIP = 4
+
+bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...)
+uint64 failsafe_timestamp # time when failsafe was activated
+bool failsafe_but_user_took_over
+
+# Link loss
+bool rc_signal_lost              # true if RC reception lost
+bool data_link_lost              # datalink to GCS lost
+uint8 data_link_lost_counter     # counts unique data link lost events
+bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost
+
+# VTOL flags
+bool is_vtol             # True if the system is VTOL capable
+bool is_vtol_tailsitter  # True if the system performs a 90° pitch down rotation during transition from MC to FW
+bool in_transition_mode  # True if VTOL is doing a transition
+bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW
+
+bool mission_failure # Set to true if mission could not continue/finish
+bool geofence_violated
+
+# MAVLink identification
+uint8 system_type  # system type, contains mavlink MAV_TYPE
+uint8 system_id	   # system id, contains MAVLink's system ID field
+uint8 component_id # subsystem / component id, contains MAVLink's component ID field
+
+# see SYS_STATUS mavlink message for the following
+# lower 32 bits are for the base flags, higher 32 bits are or the extended flags
+uint64 onboard_control_sensors_present
+uint64 onboard_control_sensors_enabled
+uint64 onboard_control_sensors_health
 
 bool safety_button_available # Set to true if a safety button is connected
 bool safety_off # Set to true if safety is off
diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg
index 0d36707314..e043976c64 100644
--- a/msg/vehicle_status_flags.msg
+++ b/msg/vehicle_status_flags.msg
@@ -33,6 +33,9 @@ bool circuit_breaker_engaged_usb_check
 bool circuit_breaker_engaged_posfailure_check	# set to true when the position valid checks have been disabled
 bool circuit_breaker_vtol_fw_arming_check 	# set to true if for VTOLs arming in fixed-wing mode should be allowed
 
+bool max_flight_time_exceeded		# set to true if the flight time (time since last arming) is above limit
+bool max_wind_speed_exceeded		# set to true if the currently measured wind speed is above limit
+
 bool offboard_control_signal_lost
 
 bool rc_signal_found_once
@@ -45,5 +48,11 @@ bool sd_card_detected_once                        # set to true if the SD card w
 bool avoidance_system_required					  # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
 bool avoidance_system_valid                       # Status of the obstacle avoidance system
 
+bool onboard_logging_system_required              # Set to true if logging system is required for arming via the COM_ARM_WO_OBLOG parameter
+bool onboard_logging_system_valid                 # Status of the onboard logging system
+
 bool parachute_system_present
 bool parachute_system_healthy
+
+bool open_drone_id_system_present
+bool open_drone_id_system_healthy
\ No newline at end of file
diff --git a/platforms/common/CMakeLists.txt b/platforms/common/CMakeLists.txt
index 120a59fdfe..dba8ca65de 100644
--- a/platforms/common/CMakeLists.txt
+++ b/platforms/common/CMakeLists.txt
@@ -60,3 +60,5 @@ endif()
 
 add_subdirectory(px4_work_queue)
 add_subdirectory(work_queue)
+
+px4_add_unit_gtest(SRC board_identity_test.cpp LINKLIBS px4_platform)
diff --git a/platforms/common/board_identity.c b/platforms/common/board_identity.c
index 6ee0f59a59..483717b599 100644
--- a/platforms/common/board_identity.c
+++ b/platforms/common/board_identity.c
@@ -40,6 +40,12 @@
 #include <px4_platform_common/px4_config.h>
 #include <stdio.h>
 #include <string.h>
+
+/**
+ * For special cases, specific boards may need to override the UUID, instead of using the generic
+ * PX4 GUID (gettable via 'board_get_px4_guid_formated' function). In that case we define the cascaded
+ * UUID function getters to incorporate the overridden UUID into the GUID.
+ */
 #if defined(BOARD_OVERRIDE_UUID) || defined(BOARD_OVERRIDE_MFGUID) || defined(BOARD_OVERRIDE_PX4_GUID)
 static const uint16_t soc_arch_id = PX4_SOC_ARCH_ID;
 static const char board_uuid[17] = BOARD_OVERRIDE_UUID;
@@ -72,11 +78,11 @@ int board_get_uuid32_formated(char *format_buffer, int size,
 	int offset = 0;
 	int sep_size = seperator ? strlen(seperator) : 0;
 
-	for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
+	for (unsigned i = 0; (offset < size - 1) && (i < PX4_CPU_UUID_WORD32_LENGTH); i++) {
 		offset += snprintf(&format_buffer[offset], size - offset, format, uuid[i]);
 
-		if (sep_size && i < PX4_CPU_UUID_WORD32_LENGTH - 1) {
-			strcat(&format_buffer[offset], seperator);
+		if (sep_size && (offset < size - sep_size - 1) && (i < PX4_CPU_UUID_WORD32_LENGTH - 1)) {
+			strncat(&format_buffer[offset], seperator, size - offset);
 			offset += sep_size;
 		}
 	}
diff --git a/platforms/common/board_identity_test.cpp b/platforms/common/board_identity_test.cpp
new file mode 100644
index 0000000000..a028e9c29f
--- /dev/null
+++ b/platforms/common/board_identity_test.cpp
@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   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 <px4_platform_common/px4_config.h>
+#include <string.h>
+
+/**
+ * @file board_identity_test.cpp
+ * @author Junwoo Hwang <junwoo091400@gmail.com>
+ *
+ * Unit test for checking if board identification string retrieval function works correctly,
+ * and without any unexpected buffer overflows
+ */
+
+/**
+ * @brief Test `board_get_uuid32_formated` function for buffer overflows & correct formatting
+ *
+ * This is a deprecated function that gets used on exceptional cases where "BOARD_OVERRIDE_UUID" is defined.
+ * Which is the case in SITL target as well as few others. In that case, the generic function `board_get_px4_guid_formated`
+ * calls this function to fill out the buffer.
+ *
+ * It should write 16 bytes of UUID for SITL (PX4_CPU_UUID_BYTE_LENGTH depends on target!) in specified format, and if the
+ * buffer has size less than that, the buffer shouldn't overflow, respecting the size argument.
+ *
+ * For example, with the format "02x", which writes each byte in 2-character hexadecimal value, the 16 bytes of UUID
+ * will be written across '33' bytes of buffer, as each UUID byte will use 2 bytes, and NULL terminator will use 1 byte.
+ *
+ * If the buffer overflow occurs, the GCC compiler level will terminate the test with '*** stack smashing detected ***'
+ * message, which should cancel the test
+ *
+ * TODO: Make the test fail properly, instead of getting canceled in stack smashing case
+ */
+TEST(BoardIdentityTest, UUID32BufferOverflow)
+{
+	// Get groundtruth UUID value. Without separator, should require 33 bytes
+	char groundtruth_uuid_no_sep[50];
+	board_get_uuid32_formated(groundtruth_uuid_no_sep, sizeof(groundtruth_uuid_no_sep), "%02x", NULL); // No separator
+	EXPECT_EQ(32, strlen(groundtruth_uuid_no_sep));
+	// Get groundtruth UUID value. With separator, should be 36 bytes
+	char groundtruth_uuid_with_sep[50];
+	board_get_uuid32_formated(groundtruth_uuid_with_sep, sizeof(groundtruth_uuid_with_sep), "%02x", "-"); // With separator
+	EXPECT_EQ(35, strlen(groundtruth_uuid_with_sep));
+
+	// 12 bytes : Shouldn't be enough, but shouldn't also cause buffer overflow
+	// We check : 1. If the function causes buffer overflow | 2. If the buffer gets filled with correct data
+	// Note: Since for `char buf[N]`, the maimum string length is N-1, we give N-1 for `strncmp` function.
+	char buf_12[12];
+	board_get_uuid32_formated(buf_12, sizeof(buf_12), "%02x", NULL); // No separator
+	EXPECT_EQ(0, strncmp(groundtruth_uuid_no_sep, buf_12, sizeof(buf_12) - 1));
+	board_get_uuid32_formated(buf_12, sizeof(buf_12), "%02x", "-"); // With separator
+	EXPECT_EQ(0, strncmp(groundtruth_uuid_with_sep, buf_12, sizeof(buf_12) - 1));
+
+	// 33 bytes: Should be enough for UUID with no separator, but not enough for UUID with separator
+	char buf_33[33];
+	board_get_uuid32_formated(buf_33, sizeof(buf_33), "%02x", NULL); // No separator
+	EXPECT_EQ(0, strncmp(groundtruth_uuid_no_sep, buf_33, sizeof(buf_33) - 1));
+	board_get_uuid32_formated(buf_33, sizeof(buf_33), "%02x", "-"); // With separator
+	EXPECT_EQ(0, strncmp(groundtruth_uuid_with_sep, buf_33, sizeof(buf_33) - 1));
+
+	// 40 bytes: Should be enough for UUID for all cases
+	char buf_40[40];
+	board_get_uuid32_formated(buf_40, sizeof(buf_40), "%02x", NULL); // No separator
+	EXPECT_EQ(0, strncmp(groundtruth_uuid_no_sep, buf_40, sizeof(buf_40) - 1));
+	board_get_uuid32_formated(buf_40, sizeof(buf_40), "%02x", "-"); // With separator
+	EXPECT_EQ(0, strncmp(groundtruth_uuid_with_sep, buf_40, sizeof(buf_40) - 1));
+}
+
+/**
+ * @brief Test `board_get_mfguid_formated` function for buffer overflows & correct formatting
+ *
+ * The size that should be written is PX4_CPU_MFGUID_FORMAT_SIZE, which is usually 33 (like UUID)
+ */
+TEST(BoardIdentityTest, MFGUIDBufferOverflow)
+{
+	// Get groundtruth MFGUID value
+	char groundtruth_mfg_uid[50];
+	board_get_mfguid_formated(groundtruth_mfg_uid, sizeof(groundtruth_mfg_uid));
+
+	// Expect buffer of size PX4_CPU_MFGUID_FORMAT_SIZE, which means string length will be -1 of that.
+	EXPECT_EQ(PX4_CPU_MFGUID_FORMAT_SIZE - 1, strlen(groundtruth_mfg_uid));
+
+	// 12 bytes : Shouldn't be enough, but shouldn't also cause buffer overflow
+	// We check : 1. If the function causes buffer overflow | 2. If the buffer gets filled with correct data
+	// Note: Since for `char buf[N]`, the maimum string length is N-1, we give N-1 for `strncmp` function.
+	char buf_12[12];
+	board_get_mfguid_formated(buf_12, sizeof(buf_12));
+	EXPECT_EQ(0, strncmp(groundtruth_mfg_uid, buf_12, sizeof(buf_12) - 1));
+
+	// 33 bytes: Should be just enough for MFGUID
+	char buf_33[33];
+	board_get_mfguid_formated(buf_33, sizeof(buf_33));
+	EXPECT_EQ(0, strncmp(groundtruth_mfg_uid, buf_33, sizeof(buf_33) - 1));
+
+	// 40 bytes: Should be enough for MFGUID
+	char buf_40[40];
+	board_get_mfguid_formated(buf_40, sizeof(buf_40));
+	EXPECT_EQ(0, strncmp(groundtruth_mfg_uid, buf_40, sizeof(buf_40) - 1));
+}
+
+/**
+ * @brief Test `board_get_px4_guid_formated` function for buffer overflows & correct formatting
+ *
+ * The size that needs to be written is PX4_GUID_FORMAT_SIZE, which includes the NULL
+ * termination character into account, which is usually 33.
+ */
+TEST(BoardIdentityTest, PX4GUIDBufferOverflow)
+{
+	// Get groundtruth PX4 GUID value with enough buffer (minimum 32 + 1)
+	char groundtruth_px4_guid[50];
+	board_get_px4_guid_formated(groundtruth_px4_guid, sizeof(groundtruth_px4_guid));
+
+	// Expect buffer of size PX4_GUID_FORMAT_SIZE, which means string length will be -1 of that.
+	EXPECT_EQ(PX4_GUID_FORMAT_SIZE - 1, strlen(groundtruth_px4_guid));
+
+	// 12 bytes : Shouldn't be enough, but shouldn't also cause buffer overflow
+	// We check : 1. If the function causes buffer overflow | 2. If the buffer gets filled with correct data
+	// Note: Since for `char buf[N]`, the maimum string length is N-1, we give N-1 for `strncmp` function.
+	char buf_12[12];
+	board_get_px4_guid_formated(buf_12, sizeof(buf_12));
+	EXPECT_EQ(0, strncmp(groundtruth_px4_guid, buf_12, sizeof(buf_12) - 1));
+
+	// 33 bytes: Should be just enough for PX4 GUID
+	char buf_33[33];
+	board_get_px4_guid_formated(buf_33, sizeof(buf_33));
+	EXPECT_EQ(0, strncmp(groundtruth_px4_guid, buf_33, sizeof(buf_33) - 1));
+
+	// 40 bytes: Should be enough for PX4 GUID
+	char buf_40[40];
+	board_get_px4_guid_formated(buf_40, sizeof(buf_40));
+	EXPECT_EQ(0, strncmp(groundtruth_px4_guid, buf_40, sizeof(buf_40) - 1));
+}
diff --git a/platforms/common/include/px4_platform_common/board_common.h b/platforms/common/include/px4_platform_common/board_common.h
index 4acfa65549..56aea7b044 100644
--- a/platforms/common/include/px4_platform_common/board_common.h
+++ b/platforms/common/include/px4_platform_common/board_common.h
@@ -82,6 +82,10 @@
 #define BOARD_NUM_SPI_CFG_HW_VERSIONS 1
 #endif
 
+#ifndef BOARD_MTD_NUM_EEPROM
+#define BOARD_MTD_NUM_EEPROM 1
+#endif
+
 /* ADC defining tools
  * We want to normalize the V5 Sensing to V = (adc_dn) * ADC_V5_V_FULL_SCALE/(2 ^ ADC_BITS) * ADC_V5_SCALE)
  */
@@ -258,9 +262,12 @@
 
 #if defined(BOARD_HAS_HW_VERSIONING)
 #  define BOARD_HAS_VERSIONING 1
-#  define HW_VER_REV(v,r)       ((uint32_t)((v) & 0xff) << 8) | ((uint32_t)(r) & 0xff)
+#  define HW_VER_REV(v,r)       ((uint32_t)((v) & 0xffff) << 16) | ((uint32_t)(r) & 0xffff)
 #endif
 
+#define HW_INFO_REV_DIGITS    3
+#define HW_INFO_VER_DIGITS    3
+
 /* Default LED logical to color mapping */
 
 #if defined(BOARD_OVERLOAD_LED)
@@ -950,7 +957,7 @@ int board_get_mfguid_formated(char *format_buffer, int size); // DEPRICATED use
 int board_get_px4_guid(px4_guid_t guid);
 
 /************************************************************************************
- * Name: board_get_mfguid_formated
+ * Name: board_get_px4_guid_formated
  *
  * Description:
  *   All boards either provide a way to retrieve a formatted string of the
diff --git a/platforms/common/include/px4_platform_common/defines.h b/platforms/common/include/px4_platform_common/defines.h
index 6c325fa803..c545313bcd 100644
--- a/platforms/common/include/px4_platform_common/defines.h
+++ b/platforms/common/include/px4_platform_common/defines.h
@@ -70,6 +70,9 @@ static inline constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(
 #define PX4_O_MODE_666 0666
 #define PX4_O_MODE_600 0600
 
+#define DIRENT_REGULAR_FILE DTYPE_FILE
+#define DIRENT_DIRECTORY DTYPE_DIRECTORY
+
 #elif defined(__PX4_POSIX)
 /****************************************************************************
  * POSIX Specific defines
@@ -91,6 +94,9 @@ static inline constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(
 #define USEC_PER_TICK (1000000/PX4_TICKS_PER_SEC)
 #define USEC2TICK(x) (((x)+(USEC_PER_TICK/2))/USEC_PER_TICK)
 
+#define DIRENT_REGULAR_FILE DT_REG
+#define DIRENT_DIRECTORY DT_DIR
+
 __BEGIN_DECLS
 extern long PX4_TICKS_PER_SEC;
 __END_DECLS
diff --git a/platforms/common/include/px4_platform_common/mtd_manifest.h b/platforms/common/include/px4_platform_common/mtd_manifest.h
index 2ab9514573..0bf8b52619 100644
--- a/platforms/common/include/px4_platform_common/mtd_manifest.h
+++ b/platforms/common/include/px4_platform_common/mtd_manifest.h
@@ -37,12 +37,13 @@ typedef enum  {
 	MTD_PARAMETERS  = 1,
 	MTD_WAYPOINTS   = 2,
 	MTD_CALDATA     = 3,
-	MTD_MFT         = 4,
-	MTD_ID          = 5,
-	MTD_NET         = 6,
+	MTD_MFT_VER     = 4,
+	MTD_MFT_REV     = 5,
+	MTD_ID          = 6,
+	MTD_NET         = 7
 } px4_mtd_types_t;
-#define PX4_MFT_MTD_TYPES  {MTD_PARAMETERS, MTD_WAYPOINTS, MTD_CALDATA, MTD_MFT, MTD_ID, MTD_NET}
-#define PX4_MFT_MTD_STR_TYPES  {"MTD_PARAMETERS", "MTD_WAYPOINTS", "MTD_CALDATA", "MTD_MFT", "MTD_ID", "MTD_NET"}
+#define PX4_MFT_MTD_TYPES  {MTD_PARAMETERS, MTD_WAYPOINTS, MTD_CALDATA, MTD_MFT_VER, MTD_MFT_REV, MTD_ID, MTD_NET}
+#define PX4_MFT_MTD_STR_TYPES  {"MTD_PARAMETERS", "MTD_WAYPOINTS", "MTD_CALDATA", "MTD_MFT_VER", "MTD_MFT_REV", "MTD_ID", "MTD_NET"}
 
 typedef struct  {
 	const px4_mtd_types_t   type;
@@ -100,6 +101,6 @@ __EXPORT int px4_mtd_config(const px4_mtd_manifest_t *mft_mtd);
  *   0 (get !=null) item by type's value is returned at get;
  *
  ************************************************************************************/
-__EXPORT int px4_mtd_query(const char *type, const char *val, const char **get = nullptr);
+__EXPORT int px4_mtd_query(const char *type, const char *val, const char **get);
 
 __END_DECLS
diff --git a/platforms/common/include/px4_platform_common/px4_manifest.h b/platforms/common/include/px4_platform_common/px4_manifest.h
index cc646def8c..bdb6ad1984 100644
--- a/platforms/common/include/px4_platform_common/px4_manifest.h
+++ b/platforms/common/include/px4_platform_common/px4_manifest.h
@@ -65,7 +65,7 @@ typedef struct {
 
 typedef struct {
 	const uint32_t        nmft;
-	const px4_mft_entry_s *mfts;
+	const px4_mft_entry_s *mfts[];
 } px4_mft_s;
 
 #include "px4_platform_common/mtd_manifest.h"
@@ -88,6 +88,19 @@ __BEGIN_DECLS
 
 __EXPORT const px4_mft_s *board_get_manifest(void);
 
+/************************************************************************************
+ * Name: board_get_base_eeprom_mtd_manifest
+ *
+ * Description:
+ *   A board will provide this function to return the mtd with eeprom manifest
+ *
+ * Returned Value:
+ *   pointer to mtd manifest
+ *
+ ************************************************************************************/
+
+__EXPORT const px4_mtd_manifest_t *board_get_base_eeprom_mtd_manifest(void);
+
 /************************************************************************************
  * Name: px4_mft_configure
  *
diff --git a/platforms/common/include/px4_platform_common/px4_mtd.h b/platforms/common/include/px4_platform_common/px4_mtd.h
index 11f5edb21f..7313bd78a9 100644
--- a/platforms/common/include/px4_platform_common/px4_mtd.h
+++ b/platforms/common/include/px4_platform_common/px4_mtd.h
@@ -35,6 +35,8 @@
 
 __BEGIN_DECLS
 
+#define MAX_MTD_INSTANCES 5u
+
 // The data needed to interface with mtd device's
 
 typedef struct {
@@ -60,7 +62,7 @@ typedef struct {
  *            This can be  Null if there are no mtd instances.
  *
  */
-__EXPORT mtd_instance_s *px4_mtd_get_instances(unsigned int *count);
+__EXPORT mtd_instance_s **px4_mtd_get_instances(unsigned int *count);
 
 /*
   Get device complete geometry or a device
@@ -75,7 +77,9 @@ __EXPORT int  px4_mtd_get_geometry(const mtd_instance_s *instance, unsigned long
  */
 __EXPORT ssize_t px4_mtd_get_partition_size(const mtd_instance_s *instance, const char *partname);
 
-FAR struct mtd_dev_s *px4_at24c_initialize(FAR struct i2c_master_s *dev,
-		uint8_t address);
+int px4_at24c_initialize(FAR struct i2c_master_s *dev,
+			 uint8_t address, FAR struct mtd_dev_s **mtd_dev);
+
+void px4_at24c_deinitialize(void);
 
 __END_DECLS
diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
index 94614bf5d8..ff6193987e 100644
--- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
+++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp
@@ -65,7 +65,7 @@ static constexpr wq_config_t I2C3{"wq:I2C3", 2336, -11};
 static constexpr wq_config_t I2C4{"wq:I2C4", 2336, -12};
 
 // PX4 att/pos controllers, highest priority after sensors.
-static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 2240, -13};
+static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 2280, -13};
 
 static constexpr wq_config_t INS0{"wq:INS0", 6000, -14};
 static constexpr wq_config_t INS1{"wq:INS1", 6000, -15};
diff --git a/platforms/common/spi.cpp b/platforms/common/spi.cpp
index acce069ea3..a2033ff555 100644
--- a/platforms/common/spi.cpp
+++ b/platforms/common/spi.cpp
@@ -47,7 +47,7 @@ void px4_set_spi_buses_from_hw_version()
 #if defined(BOARD_HAS_SIMPLE_HW_VERSIONING)
 	int hw_version_revision = board_get_hw_version();
 #else
-	int hw_version_revision = (board_get_hw_version() << 8) | board_get_hw_revision();
+	int hw_version_revision = HW_VER_REV(board_get_hw_version(), board_get_hw_revision());
 #endif
 
 
@@ -66,12 +66,12 @@ void px4_set_spi_buses_from_hw_version()
 	}
 }
 
-const px4_spi_bus_t *px4_spi_buses{};
+const px4_spi_bus_t *px4_spi_buses{nullptr};
 #endif
 
 int px4_find_spi_bus(uint32_t devid)
 {
-	for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
+	for (int i = 0; px4_spi_buses != nullptr && i < SPI_BUS_MAX_BUS_ITEMS; ++i) {
 		const px4_spi_bus_t &bus_data = px4_spi_buses[i];
 
 		if (bus_data.bus == -1) {
diff --git a/platforms/nuttx/NuttX/nuttx b/platforms/nuttx/NuttX/nuttx
index 91bece51af..6bafcb22b7 160000
--- a/platforms/nuttx/NuttX/nuttx
+++ b/platforms/nuttx/NuttX/nuttx
@@ -1 +1 @@
-Subproject commit 91bece51afbe7da9db12e3695cdbb4f4bba4bc83
+Subproject commit 6bafcb22b7d62184fd1158c0b53c8dbd884275b8
diff --git a/platforms/nuttx/init/stm32f7/rc.board_arch_defaults b/platforms/nuttx/init/stm32f7/rc.board_arch_defaults
index 1a92497950..72b044447d 100644
--- a/platforms/nuttx/init/stm32f7/rc.board_arch_defaults
+++ b/platforms/nuttx/init/stm32f7/rc.board_arch_defaults
@@ -4,10 +4,10 @@
 #------------------------------------------------------------------------------
 
 # Multi-EKF (across IMUs only)
-param set-default EKF2_MULTI_IMU 3
-param set-default SENS_IMU_MODE 0
-param set-default EKF2_MULTI_MAG 0
-param set-default SENS_MAG_MODE 1
+#param set-default EKF2_MULTI_IMU 3
+#param set-default SENS_IMU_MODE 0
+#param set-default EKF2_MULTI_MAG 0
+#param set-default SENS_MAG_MODE 1
 
 param set-default -s IMU_GYRO_FFT_EN 1
 
diff --git a/platforms/nuttx/init/stm32h7/rc.board_arch_defaults b/platforms/nuttx/init/stm32h7/rc.board_arch_defaults
index 9d1fee504c..c8dafb3cce 100644
--- a/platforms/nuttx/init/stm32h7/rc.board_arch_defaults
+++ b/platforms/nuttx/init/stm32h7/rc.board_arch_defaults
@@ -4,10 +4,10 @@
 #------------------------------------------------------------------------------
 
 # Multi-EKF
-param set-default EKF2_MULTI_IMU 3
-param set-default SENS_IMU_MODE 0
-param set-default EKF2_MULTI_MAG 3
-param set-default SENS_MAG_MODE 0
+#param set-default EKF2_MULTI_IMU 3
+#param set-default SENS_IMU_MODE 0
+#param set-default EKF2_MULTI_MAG 3
+#param set-default SENS_MAG_MODE 0
 
 param set-default -s IMU_GYRO_FFT_EN 1
 
diff --git a/platforms/nuttx/src/px4/common/board_crashdump.c b/platforms/nuttx/src/px4/common/board_crashdump.c
index 035bc60443..90e541ae0e 100644
--- a/platforms/nuttx/src/px4/common/board_crashdump.c
+++ b/platforms/nuttx/src/px4/common/board_crashdump.c
@@ -38,10 +38,10 @@
  * and hardfault log support
  */
 
-#ifdef CONFIG_BOARD_CRASHDUMP
-
 #include <board_config.h>
 
+#ifdef CONFIG_BOARD_CRASHDUMP
+
 #include <stdio.h>
 #include <stdbool.h>
 #include <string.h>
diff --git a/platforms/nuttx/src/px4/common/cdc_acm_check.cpp b/platforms/nuttx/src/px4/common/cdc_acm_check.cpp
index 5d9702152f..14c0b560b6 100644
--- a/platforms/nuttx/src/px4/common/cdc_acm_check.cpp
+++ b/platforms/nuttx/src/px4/common/cdc_acm_check.cpp
@@ -31,6 +31,8 @@
  *
  ****************************************************************************/
 
+#include <board_config.h>
+
 #if defined(CONFIG_SYSTEM_CDCACM)
 __BEGIN_DECLS
 #include <arch/board/board.h>
diff --git a/platforms/nuttx/src/px4/common/gpio/mcp23009/CMakeLists.txt b/platforms/nuttx/src/px4/common/gpio/mcp23009/CMakeLists.txt
index 5660edfac1..da6dac979f 100644
--- a/platforms/nuttx/src/px4/common/gpio/mcp23009/CMakeLists.txt
+++ b/platforms/nuttx/src/px4/common/gpio/mcp23009/CMakeLists.txt
@@ -33,3 +33,4 @@
 px4_add_library(platform_gpio_mcp23009
 		mcp23009.cpp
 	)
+target_link_libraries(platform_gpio_mcp23009 PRIVATE drivers__device) # device::I2C
diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h
index e8c77771e7..95e99ffd46 100644
--- a/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h
+++ b/platforms/nuttx/src/px4/common/include/px4_platform/board_determine_hw_info.h
@@ -32,9 +32,10 @@
  ****************************************************************************/
 #pragma once
 
-
+#include "micro_hal.h"
 __BEGIN_DECLS
 
+#define HW_INFO_SUFFIX        "%0" STRINGIFY(HW_INFO_VER_DIGITS) "x%0" STRINGIFY(HW_INFO_REV_DIGITS) "x"
 /************************************************************************************
   * Name: board_determine_hw_info
  *
diff --git a/platforms/nuttx/src/px4/common/include/px4_platform/board_hw_eeprom_rev_ver.h b/platforms/nuttx/src/px4/common/include/px4_platform/board_hw_eeprom_rev_ver.h
new file mode 100644
index 0000000000..8f4f671403
--- /dev/null
+++ b/platforms/nuttx/src/px4/common/include/px4_platform/board_hw_eeprom_rev_ver.h
@@ -0,0 +1,112 @@
+/****************************************************************************
+ *
+ *   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 <errno.h>
+
+#define HW_ID_EEPROM 			0x7		//!< Get hw_info from EEPROM
+#define HW_EEPROM_ID_MIN		0x10	//!< Minimum supported id (version/revision)
+
+#pragma pack(push, 1)
+
+typedef struct {
+	uint16_t id;
+} mtd_mft_t;
+
+typedef struct {
+	mtd_mft_t version;
+	uint16_t hw_extended_id; //<! HW version for MTD_MFT_VER, HW revision for MTD_MFT_REV
+	uint16_t crc;
+} mtd_mft_v0_t;
+
+typedef struct {
+	mtd_mft_t version;
+	uint16_t hw_extended_id;
+	//{device tree overlay}
+	uint16_t crc;
+} mtd_mft_v1_t;
+
+
+#pragma pack(pop)
+
+#define MTD_MFT_v0 0U //<! EEPROM MTD MFT structure version 0
+#define MTD_MFT_v1 1U //<! EEPROM MTD MFT structure version 1
+
+#define MTD_MFT_OFFSET 0 //<! Offset in EEPROM where mtd_mft data starts
+
+__BEGIN_DECLS
+
+/************************************************************************************
+  * Name: board_set_eeprom_hw_info
+ *
+ * Description:
+ * Function for writing hardware info to EEPROM
+ *
+ * Input Parameters:
+ *   *path        - path to mtd_mft
+ *   *mtd_mft_unk - pointer to mtd_mft to write hw_info
+ *
+ * Returned Value:
+ *    0    - Successful storing to EEPROM
+ *   -1    - Error while storing to EEPROM
+ *
+ ************************************************************************************/
+
+#if !defined(BOARD_HAS_SIMPLE_HW_VERSIONING) && defined(BOARD_HAS_VERSIONING)
+__EXPORT int board_set_eeprom_hw_info(const char *path, mtd_mft_t *mtd_mft_unk);
+#else
+static inline int board_set_eeprom_hw_info(const char *path, mtd_mft_t *mtd_mft_unk) { return -ENOSYS; }
+#endif
+
+/************************************************************************************
+  * Name: board_get_eeprom_hw_info
+ *
+ * Description:
+ * Function for reading hardware info from EEPROM
+ *
+ * Output Parameters:
+ *   *mtd_mft - pointer to mtd_mft to read hw_info
+ *
+ * Returned Value:
+ *    0    - Successful reading from EEPROM
+ *   -1    - Error while reading from EEPROM
+ *
+ ************************************************************************************/
+
+#if !defined(BOARD_HAS_SIMPLE_HW_VERSIONING) && defined(BOARD_HAS_VERSIONING)
+__EXPORT int board_get_eeprom_hw_info(const char *path, mtd_mft_t *mtd_mft);
+#else
+static inline int board_get_eeprom_hw_info(const char *path, mtd_mft_t *mtd_mft) { return -ENOSYS; }
+#endif
+
+__END_DECLS
diff --git a/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c b/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c
index 42e1e76f21..bbca9c80c3 100644
--- a/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c
+++ b/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c
@@ -70,6 +70,7 @@
 #include <nuttx/mtd/mtd.h>
 
 #include <perf/perf_counter.h>
+#include <board_config.h>
 
 /************************************************************************************
  * Pre-processor Definitions
@@ -195,11 +196,8 @@ int at24c_nuke(void);
  * Private Data
  ************************************************************************************/
 
-/* At present, only a single AT24 part is supported.  In this case, a statically
- * allocated state structure may be used.
- */
-
-static struct at24c_dev_s g_at24c;
+static uint8_t number_of_instances = 0u;
+static struct at24c_dev_s g_at24c[BOARD_MTD_NUM_EEPROM];
 
 /************************************************************************************
  * Private Functions
@@ -262,7 +260,7 @@ void at24c_test(void)
 	unsigned errors = 0;
 
 	for (count = 0; count < 10000; count++) {
-		ssize_t result = at24c_bread(&g_at24c.mtd, 0, 1, buf);
+		ssize_t result = at24c_bread(&g_at24c[0].mtd, 0, 1, buf);
 
 		if (result == ERROR) {
 			if (errors++ > 2) {
@@ -538,13 +536,17 @@ static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg)
  *   other functions (such as a block or character driver front end).
  *
  ************************************************************************************/
-FAR struct mtd_dev_s *px4_at24c_initialize(FAR struct i2c_master_s *dev,
-		uint8_t address)
+int px4_at24c_initialize(FAR struct i2c_master_s *dev,
+			 uint8_t address, FAR struct mtd_dev_s **mtd_dev)
 
 {
+	if (number_of_instances >= BOARD_MTD_NUM_EEPROM) {
+		return -ENOMEM;
+	}
+
 	FAR struct at24c_dev_s *priv;
 
-	finfo("dev: %p\n", dev);
+	finfo("dev: %p, mtd_dev %p\n", dev, mtd_dev);
 
 	/* Allocate a state structure (we allocate the structure instead of using
 	 * a fixed, static allocation so that we can handle multiple FLASH devices.
@@ -553,7 +555,7 @@ FAR struct mtd_dev_s *px4_at24c_initialize(FAR struct i2c_master_s *dev,
 	 * to be extended to handle multiple FLASH parts on the same I2C bus.
 	 */
 
-	priv = &g_at24c;
+	priv = &g_at24c[number_of_instances];
 
 	if (priv) {
 		/* Initialize the allocated structure */
@@ -608,13 +610,13 @@ FAR struct mtd_dev_s *px4_at24c_initialize(FAR struct i2c_master_s *dev,
 		priv->perf_resets_retries = NULL;
 		priv->perf_errors = NULL;
 
-		return NULL;
+		return ret;
 	}
 
-	/* Return the implementation-specific state structure as the MTD device */
+	*mtd_dev = (FAR struct mtd_dev_s *)priv;
+	++number_of_instances;
 
-	finfo("Return %p\n", priv);
-	return (FAR struct mtd_dev_s *)priv;
+	return 0;
 }
 
 /*
@@ -622,5 +624,5 @@ FAR struct mtd_dev_s *px4_at24c_initialize(FAR struct i2c_master_s *dev,
  */
 int at24c_nuke(void)
 {
-	return at24c_eraseall(&g_at24c);
+	return at24c_eraseall(&g_at24c[0]);
 }
diff --git a/platforms/nuttx/src/px4/common/px4_manifest.cpp b/platforms/nuttx/src/px4/common/px4_manifest.cpp
index 6d7e322542..d7402215ab 100644
--- a/platforms/nuttx/src/px4/common/px4_manifest.cpp
+++ b/platforms/nuttx/src/px4/common/px4_manifest.cpp
@@ -58,7 +58,9 @@ static const px4_mft_entry_s mtd_mft = {
 
 static const px4_mft_s default_mft = {
 	.nmft = 1,
-	.mfts = &mtd_mft
+	.mfts =  {
+		&mtd_mft
+	}
 };
 
 
@@ -73,9 +75,9 @@ __EXPORT int px4_mft_configure(const px4_mft_s *mft)
 
 	if (mft != nullptr) {
 		for (uint32_t m = 0; m < mft->nmft; m++) {
-			switch (mft->mfts[m].type) {
+			switch (mft->mfts[m]->type) {
 			case MTD:
-				px4_mtd_config(static_cast<const px4_mtd_manifest_t *>(mft->mfts[m].pmft));
+				px4_mtd_config(static_cast<const px4_mtd_manifest_t *>(mft->mfts[m]->pmft));
 				break;
 
 			case MFT:
@@ -95,10 +97,10 @@ __EXPORT int px4_mft_query(const px4_mft_s *mft, px4_manifest_types_e type,
 
 	if (mft != nullptr) {
 		for (uint32_t m = 0; m < mft->nmft; m++) {
-			if (mft->mfts[m].type == type)
+			if (mft->mfts[m]->type == type)
 				switch (type) {
 				case MTD:
-					return px4_mtd_query(sub, val);
+					return px4_mtd_query(sub, val, nullptr);
 					break;
 
 				case MFT:
diff --git a/platforms/nuttx/src/px4/common/px4_mtd.cpp b/platforms/nuttx/src/px4/common/px4_mtd.cpp
index 9fff4470dd..bffa56900b 100644
--- a/platforms/nuttx/src/px4/common/px4_mtd.cpp
+++ b/platforms/nuttx/src/px4/common/px4_mtd.cpp
@@ -64,7 +64,8 @@ extern "C" {
 					off_t firstblock, off_t nblocks);
 }
 static int num_instances = 0;
-static mtd_instance_s *instances = nullptr;
+static int total_blocks = 0;
+static mtd_instance_s *instances[MAX_MTD_INSTANCES] = {};
 
 
 static int ramtron_attach(mtd_instance_s &instance)
@@ -147,15 +148,19 @@ static int at24xxx_attach(mtd_instance_s &instance)
 
 	/* start the MTD driver, attempt 5 times */
 	for (int i = 0; i < 5; i++) {
-		instance.mtd_dev = px4_at24c_initialize(i2c, PX4_I2C_DEVID_ADDR(instance.devid));
+		int ret_val = px4_at24c_initialize(i2c, PX4_I2C_DEVID_ADDR(instance.devid), &(instance.mtd_dev));
 
-		if (instance.mtd_dev) {
+		if (ret_val == 0) {
 			/* abort on first valid result */
 			if (i > 0) {
 				PX4_WARN("EEPROM needed %d attempts to attach", i + 1);
 			}
 
 			break;
+
+		} else if (ret_val == -ENOMEM) {
+			PX4_ERR("Number of at24c EEPROM instances reached the board limit of %d", BOARD_MTD_NUM_EEPROM);
+			break;
 		}
 	}
 
@@ -230,7 +235,7 @@ ssize_t px4_mtd_get_partition_size(const mtd_instance_s *instance, const char *p
 	return instance->partition_block_counts[partn] * blocksize;
 }
 
-mtd_instance_s *px4_mtd_get_instances(unsigned int *count)
+mtd_instance_s **px4_mtd_get_instances(unsigned int *count)
 {
 	*count = num_instances;
 	return instances;
@@ -292,62 +297,69 @@ int px4_mtd_config(const px4_mtd_manifest_t *mft_mtd)
 	}
 
 	rv = -ENOMEM;
-	int total_blocks = 0;
+	uint8_t total_new_instances = mtd_list->nconfigs + num_instances;
 
-	instances = new mtd_instance_s[mtd_list->nconfigs];
-
-	if (instances == nullptr) {
-memoryout:
-		PX4_ERR("failed to allocate memory!");
+	if (total_new_instances >= MAX_MTD_INSTANCES) {
+		PX4_ERR("reached limit of max %u mtd instances", MAX_MTD_INSTANCES);
 		return rv;
 	}
 
-	for (uint32_t i = 0; i < mtd_list->nconfigs; i++) {
+	for (uint8_t i = num_instances, num_entry = 0u; i < total_new_instances; ++i, ++num_entry) {
+
+		instances[i] = new mtd_instance_s;
+
+		if (instances == nullptr) {
+memoryout:
+			PX4_ERR("failed to allocate memory!");
+			return rv;
+		}
+
 		num_instances++;
-		uint32_t nparts = mtd_list->entries[i]->npart;
-		instances[i].devid = mtd_list->entries[i]->device->devid;
-		instances[i].mtd_dev = nullptr;
-		instances[i].n_partitions_current = 0;
+
+		uint32_t nparts = mtd_list->entries[num_entry]->npart;
+		instances[i]->devid = mtd_list->entries[num_entry]->device->devid;
+		instances[i]->mtd_dev = nullptr;
+		instances[i]->n_partitions_current = 0;
 
 		rv = -ENOMEM;
-		instances[i].part_dev = new FAR struct mtd_dev_s *[nparts];
+		instances[i]->part_dev = new FAR struct mtd_dev_s *[nparts];
 
-		if (instances[i].part_dev == nullptr) {
+		if (instances[i]->part_dev == nullptr) {
 			goto memoryout;
 		}
 
-		instances[i].partition_block_counts = new int[nparts];
+		instances[i]->partition_block_counts = new int[nparts];
 
-		if (instances[i].partition_block_counts == nullptr) {
+		if (instances[i]->partition_block_counts == nullptr) {
 			goto memoryout;
 		}
 
-		instances[i].partition_types = new int[nparts];
+		instances[i]->partition_types = new int[nparts];
 
-		if (instances[i].partition_types == nullptr) {
+		if (instances[i]->partition_types == nullptr) {
 			goto memoryout;
 		}
 
-		instances[i].partition_names = new const char *[nparts];
+		instances[i]->partition_names = new const char *[nparts];
 
-		if (instances[i].partition_names == nullptr) {
+		if (instances[i]->partition_names == nullptr) {
 			goto memoryout;
 		}
 
 		for (uint32_t p = 0; p < nparts; p++) {
-			instances[i].partition_block_counts[p] =  mtd_list->entries[i]->partd[p].nblocks;
-			instances[i].partition_names[p] = mtd_list->entries[i]->partd[p].path;
-			instances[i].partition_types[p] = mtd_list->entries[i]->partd[p].type;
+			instances[i]->partition_block_counts[p] =  mtd_list->entries[num_entry]->partd[p].nblocks;
+			instances[i]->partition_names[p] = mtd_list->entries[num_entry]->partd[p].path;
+			instances[i]->partition_types[p] = mtd_list->entries[num_entry]->partd[p].type;
 		}
 
-		if (mtd_list->entries[i]->device->bus_type == px4_mft_device_t::I2C) {
-			rv = at24xxx_attach(instances[i]);
+		if (mtd_list->entries[num_entry]->device->bus_type == px4_mft_device_t::I2C) {
+			rv = at24xxx_attach(*instances[i]);
 
-		} else if (mtd_list->entries[i]->device->bus_type == px4_mft_device_t::SPI) {
-			rv = ramtron_attach(instances[i]);
+		} else if (mtd_list->entries[num_entry]->device->bus_type == px4_mft_device_t::SPI) {
+			rv = ramtron_attach(*instances[i]);
 
-		} else if (mtd_list->entries[i]->device->bus_type == px4_mft_device_t::ONCHIP) {
-			instances[i].n_partitions_current++;
+		} else if (mtd_list->entries[num_entry]->device->bus_type == px4_mft_device_t::ONCHIP) {
+			instances[i]->n_partitions_current++;
 			return 0;
 		}
 
@@ -362,7 +374,7 @@ memoryout:
 		unsigned int  nblocks;
 		unsigned int  partsize;
 
-		rv = px4_mtd_get_geometry(&instances[i], &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize);
+		rv = px4_mtd_get_geometry(instances[i], &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize);
 
 		if (rv != 0) {
 			goto errout;
@@ -375,13 +387,13 @@ memoryout:
 		unsigned long offset;
 		unsigned part;
 
-		for (offset = 0, part = 0; rv == 0 && part < nparts; offset += instances[i].partition_block_counts[part], part++) {
+		for (offset = 0, part = 0; rv == 0 && part < nparts; offset += instances[i]->partition_block_counts[part], part++) {
 
 			/* Create the partition */
 
-			instances[i].part_dev[part] = mtd_partition(instances[i].mtd_dev, offset, instances[i].partition_block_counts[part]);
+			instances[i]->part_dev[part] = mtd_partition(instances[i]->mtd_dev, offset, instances[i]->partition_block_counts[part]);
 
-			if (instances[i].part_dev[part] == nullptr) {
+			if (instances[i]->part_dev[part] == nullptr) {
 				PX4_ERR("mtd_partition failed. offset=%lu nblocks=%u",
 					offset, nblocks);
 				rv = -ENOSPC;
@@ -392,7 +404,7 @@ memoryout:
 
 			snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", total_blocks);
 
-			rv = ftl_initialize(total_blocks, instances[i].part_dev[part]);
+			rv = ftl_initialize(total_blocks, instances[i]->part_dev[part]);
 
 			if (rv < 0) {
 				PX4_ERR("ftl_initialize %s failed: %d", blockname, rv);
@@ -403,14 +415,14 @@ memoryout:
 
 			/* Now create a character device on the block device */
 
-			rv = bchdev_register(blockname, instances[i].partition_names[part], false);
+			rv = bchdev_register(blockname, instances[i]->partition_names[part], false);
 
 			if (rv < 0) {
-				PX4_ERR("bchdev_register %s failed: %d", instances[i].partition_names[part], rv);
+				PX4_ERR("bchdev_register %s failed: %d", instances[i]->partition_names[part], rv);
 				goto errout;
 			}
 
-			instances[i].n_partitions_current++;
+			instances[i]->n_partitions_current++;
 		}
 
 errout:
@@ -418,9 +430,9 @@ errout:
 		if (rv < 0) {
 			PX4_ERR("mtd failure: %d bus %" PRId32 " address %" PRId32 " class %d",
 				rv,
-				PX4_I2C_DEVID_BUS(instances[i].devid),
-				PX4_I2C_DEVID_ADDR(instances[i].devid),
-				mtd_list->entries[i]->partd[instances[i].n_partitions_current].type);
+				PX4_I2C_DEVID_BUS(instances[i]->devid),
+				PX4_I2C_DEVID_ADDR(instances[i]->devid),
+				mtd_list->entries[num_entry]->partd[instances[i]->n_partitions_current].type);
 			break;
 		}
 	}
@@ -452,14 +464,14 @@ __EXPORT int px4_mtd_query(const char *sub, const char *val, const char **get)
 			rv = -ENOENT;
 
 			for (int i = 0; i < num_instances; i++) {
-				for (unsigned n = 0; n < instances[i].n_partitions_current; n++) {
-					if (instances[i].partition_types[n] == key) {
+				for (unsigned n = 0; n < instances[i]->n_partitions_current; n++) {
+					if (instances[i]->partition_types[n] == key) {
 						if (get != nullptr && val == nullptr) {
-							*get =  instances[i].partition_names[n];
+							*get =  instances[i]->partition_names[n];
 							return 0;
 						}
 
-						if (val != nullptr && strcmp(instances[i].partition_names[n], val) == 0) {
+						if (val != nullptr && strcmp(instances[i]->partition_names[n], val) == 0) {
 							return 0;
 						}
 					}
diff --git a/platforms/nuttx/src/px4/common/usr_mcu_version.cpp b/platforms/nuttx/src/px4/common/usr_mcu_version.cpp
index 0928bb3a10..e89693a2d1 100644
--- a/platforms/nuttx/src/px4/common/usr_mcu_version.cpp
+++ b/platforms/nuttx/src/px4/common/usr_mcu_version.cpp
@@ -37,14 +37,16 @@
  * Implementation of generic user-space version API
  */
 
+#include <systemlib/px4_macros.h>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
+#include <px4_platform/board_determine_hw_info.h>
 
 #include "board_config.h"
 
 static int hw_version = 0;
 static int hw_revision = 0;
-static char hw_info[] = HW_INFO_INIT;
+static char hw_info[] = HW_INFO_INIT_PREFIX HW_INFO_SUFFIX;
 
 __EXPORT const char *board_get_hw_type_name(void)
 {
diff --git a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c
index d4768fdd1e..bb85fc47d3 100644
--- a/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c
+++ b/platforms/nuttx/src/px4/nxp/imxrt/board_hw_info/board_hw_rev_ver.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2019, 2022 PX4 Development Team. All rights reserved.
  *   Author: @author David Sidrane <david_s5@nscdg.com>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -52,12 +52,16 @@
 #    define GPIO_HW_REV_DRIVE GPIO_HW_VER_REV_DRIVE
 #    define GPIO_HW_VER_DRIVE GPIO_HW_VER_REV_DRIVE
 #  endif
+
+#define HW_INFO_SIZE (int) arraySize(HW_INFO_INIT_PREFIX) + HW_INFO_VER_DIGITS + HW_INFO_REV_DIGITS
+
+
 /****************************************************************************
  * Private Data
  ****************************************************************************/
 static int hw_version = 0;
 static int hw_revision = 0;
-static char hw_info[] = HW_INFO_INIT;
+static char hw_info[HW_INFO_SIZE] = {0};
 
 /****************************************************************************
  * Protected Functions
@@ -193,7 +197,7 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc
 	/* Are Resistors in place ?*/
 
 	uint32_t dn_sum = 0;
-	uint16_t dn = 0;
+	uint32_t dn = 0;
 
 	if ((high ^ low) && low == 0) {
 		/* Yes - Fire up the ADC (it has once control) */
@@ -204,14 +208,14 @@ static int read_id_dn(int *id, uint32_t gpio_drive, uint32_t gpio_sense, int adc
 			for (unsigned av = 0; av < samples; av++) {
 				dn = px4_arch_adc_sample(HW_REV_VER_ADC_BASE, adc_channel);
 
-				if (dn == 0xffff) {
+				if (dn == UINT32_MAX) {
 					break;
 				}
 
 				dn_sum  += dn;
 			}
 
-			if (dn != 0xffff) {
+			if (dn != UINT32_MAX) {
 				*id = dn_sum / samples;
 				rv = OK;
 			}
@@ -338,8 +342,12 @@ int board_determine_hw_info()
 	int rv = determine_hw_info(&hw_revision, &hw_version);
 
 	if (rv == OK) {
-		hw_info[HW_INFO_INIT_REV] = board_get_hw_revision() + '0';
-		hw_info[HW_INFO_INIT_VER] = board_get_hw_version() + '0';
+
+		if (rv == OK) {
+
+			snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX, hw_version, hw_revision);
+
+		}
 	}
 
 	return rv;
diff --git a/platforms/nuttx/src/px4/nxp/imxrt/version/board_identity.c b/platforms/nuttx/src/px4/nxp/imxrt/version/board_identity.c
index 522c7400b7..6df0b18f35 100644
--- a/platforms/nuttx/src/px4/nxp/imxrt/version/board_identity.c
+++ b/platforms/nuttx/src/px4/nxp/imxrt/version/board_identity.c
@@ -100,15 +100,14 @@ int board_get_uuid32_formated(char *format_buffer, int size,
 {
 	uuid_uint32_t uuid;
 	board_get_uuid32(uuid);
-
 	int offset = 0;
 	int sep_size = seperator ? strlen(seperator) : 0;
 
-	for (unsigned int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
-		offset += snprintf(&format_buffer[offset], size - ((i * 2 * sizeof(uint32_t)) + 1), format, uuid[i]);
+	for (unsigned i = 0; (offset < size - 1) && (i < PX4_CPU_UUID_WORD32_LENGTH); i++) {
+		offset += snprintf(&format_buffer[offset], size - offset, format, uuid[i]);
 
-		if (sep_size && i < PX4_CPU_UUID_WORD32_LENGTH - 1) {
-			strcat(&format_buffer[offset], seperator);
+		if (sep_size && (offset < size - sep_size - 1) && (i < PX4_CPU_UUID_WORD32_LENGTH - 1)) {
+			strncat(&format_buffer[offset], seperator, size - offset);
 			offset += sep_size;
 		}
 	}
diff --git a/platforms/nuttx/src/px4/nxp/kinetis/version/board_identity.c b/platforms/nuttx/src/px4/nxp/kinetis/version/board_identity.c
index 45d265d47b..275746cb44 100644
--- a/platforms/nuttx/src/px4/nxp/kinetis/version/board_identity.c
+++ b/platforms/nuttx/src/px4/nxp/kinetis/version/board_identity.c
@@ -71,15 +71,14 @@ int board_get_uuid32_formated(char *format_buffer, int size,
 {
 	uuid_uint32_t uuid;
 	board_get_uuid32(uuid);
-
 	int offset = 0;
 	int sep_size = seperator ? strlen(seperator) : 0;
 
-	for (unsigned int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
-		offset += snprintf(&format_buffer[offset], size - ((i * 2 * sizeof(uint32_t)) + 1), format, uuid[i]);
+	for (unsigned i = 0; (offset < size - 1) && (i < PX4_CPU_UUID_WORD32_LENGTH); i++) {
+		offset += snprintf(&format_buffer[offset], size - offset, format, uuid[i]);
 
-		if (sep_size && i < PX4_CPU_UUID_WORD32_LENGTH - 1) {
-			strcat(&format_buffer[offset], seperator);
+		if (sep_size && (offset < size - sep_size - 1) && (i < PX4_CPU_UUID_WORD32_LENGTH - 1)) {
+			strncat(&format_buffer[offset], seperator, size - offset);
 			offset += sep_size;
 		}
 	}
diff --git a/platforms/nuttx/src/px4/nxp/s32k1xx/version/board_identity.c b/platforms/nuttx/src/px4/nxp/s32k1xx/version/board_identity.c
index df6c5a4a0c..cd7a4ede83 100644
--- a/platforms/nuttx/src/px4/nxp/s32k1xx/version/board_identity.c
+++ b/platforms/nuttx/src/px4/nxp/s32k1xx/version/board_identity.c
@@ -71,15 +71,14 @@ int board_get_uuid32_formated(char *format_buffer, int size,
 {
 	uuid_uint32_t uuid;
 	board_get_uuid32(uuid);
-
 	int offset = 0;
 	int sep_size = seperator ? strlen(seperator) : 0;
 
-	for (unsigned int i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
-		offset += snprintf(&format_buffer[offset], size - ((i * 2 * sizeof(uint32_t)) + 1), format, uuid[i]);
+	for (unsigned i = 0; (offset < size - 1) && (i < PX4_CPU_UUID_WORD32_LENGTH); i++) {
+		offset += snprintf(&format_buffer[offset], size - offset, format, uuid[i]);
 
-		if (sep_size && i < PX4_CPU_UUID_WORD32_LENGTH - 1) {
-			strcat(&format_buffer[offset], seperator);
+		if (sep_size && (offset < size - sep_size - 1) && (i < PX4_CPU_UUID_WORD32_LENGTH - 1)) {
+			strncat(&format_buffer[offset], seperator, size - offset);
 			offset += sep_size;
 		}
 	}
diff --git a/platforms/nuttx/src/px4/rpi/rpi_common/version/board_identity.c b/platforms/nuttx/src/px4/rpi/rpi_common/version/board_identity.c
index 677c3eb8a1..5e6852ff32 100644
--- a/platforms/nuttx/src/px4/rpi/rpi_common/version/board_identity.c
+++ b/platforms/nuttx/src/px4/rpi/rpi_common/version/board_identity.c
@@ -65,11 +65,11 @@ int board_get_uuid32_formated(char *format_buffer, int size,
 	int offset = 0;
 	int sep_size = seperator ? strlen(seperator) : 0;
 
-	for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
+	for (unsigned i = 0; (offset < size - 1) && (i < PX4_CPU_UUID_WORD32_LENGTH); i++) {
 		offset += snprintf(&format_buffer[offset], size - offset, format, uuid[i]);
 
-		if (sep_size && i < PX4_CPU_UUID_WORD32_LENGTH - 1) {
-			strcat(&format_buffer[offset], seperator);
+		if (sep_size && (offset < size - sep_size - 1) && (i < PX4_CPU_UUID_WORD32_LENGTH - 1)) {
+			strncat(&format_buffer[offset], seperator, size - offset);
 			offset += sep_size;
 		}
 	}
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/CMakeLists.txt b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/CMakeLists.txt
index 2d3a799654..1b0da2ff38 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/CMakeLists.txt
+++ b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/CMakeLists.txt
@@ -34,4 +34,4 @@
 px4_add_library(arch_board_hw_info
 	board_hw_rev_ver.c
 )
-target_link_libraries(arch_board_hw_info PRIVATE arch_adc)
+target_link_libraries(arch_board_hw_info PRIVATE arch_adc systemlib)
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c
index 11e699a516..49a4d2e703 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c
+++ b/platforms/nuttx/src/px4/stm/stm32_common/board_hw_info/board_hw_rev_ver.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2017 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2017, 2022 PX4 Development Team. All rights reserved.
  *   Author: @author David Sidrane <david_s5@nscdg.com>
  *
  * Redistribution and use in source and binary forms, with or without
@@ -41,10 +41,14 @@
 #include <px4_arch/adc.h>
 #include <px4_platform_common/micro_hal.h>
 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/px4_manifest.h>
 #include <px4_platform/board_determine_hw_info.h>
+#include <px4_platform/board_hw_eeprom_rev_ver.h>
 #include <stdio.h>
+#include <fcntl.h>
 #include <board_config.h>
 
+#include <systemlib/crc.h>
 #include <systemlib/px4_macros.h>
 
 #if defined(BOARD_HAS_HW_VERSIONING)
@@ -53,12 +57,16 @@
 #    define GPIO_HW_REV_DRIVE GPIO_HW_VER_REV_DRIVE
 #    define GPIO_HW_VER_DRIVE GPIO_HW_VER_REV_DRIVE
 #  endif
+
+#define HW_INFO_SIZE (int) arraySize(HW_INFO_INIT_PREFIX) + HW_INFO_VER_DIGITS + HW_INFO_REV_DIGITS
+
+
 /****************************************************************************
  * Private Data
  ****************************************************************************/
 static int hw_version = 0;
 static int hw_revision = 0;
-static char hw_info[] = HW_INFO_INIT;
+static char hw_info[HW_INFO_SIZE] = {0};
 
 /****************************************************************************
  * Protected Functions
@@ -437,18 +445,184 @@ __EXPORT int board_get_hw_revision()
 
 int board_determine_hw_info()
 {
+
+	// Read ADC jumpering hw_info
 	int rv = determine_hw_info(&hw_revision, &hw_version);
 
 	if (rv == OK) {
 
-		hw_info[HW_INFO_INIT_REV] = board_get_hw_revision() < 10 ?
-					    board_get_hw_revision() + '0' :
-					    board_get_hw_revision() + 'a' - 10;
-		hw_info[HW_INFO_INIT_VER] = board_get_hw_version()  < 10 ?
-					    board_get_hw_version() + '0' :
-					    board_get_hw_version()  + 'a' - 10;
+		// MFT supported?
+		const char *path;
+		int rvmft = px4_mtd_query("MTD_MFT_VER", NULL, &path);
+
+		if (rvmft == OK && path != NULL && hw_version == HW_ID_EEPROM) {
+
+			mtd_mft_v0_t mtd_mft = {MTD_MFT_v0};
+			rv = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);
+
+			if (rv == OK) {
+				hw_version = mtd_mft.hw_extended_id;
+			}
+		}
+
+		path = NULL;
+		rvmft = px4_mtd_query("MTD_MFT_REV", NULL, &path);
+
+		if (rvmft == OK && path != NULL && hw_revision == HW_ID_EEPROM) {
+
+			mtd_mft_v0_t mtd_mft = {MTD_MFT_v0};
+			rv = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);
+
+			if (rv == OK) {
+				hw_revision = mtd_mft.hw_extended_id;
+			}
+		}
+	}
+
+	if (rv == OK) {
+		snprintf(hw_info, sizeof(hw_info), HW_INFO_INIT_PREFIX HW_INFO_SUFFIX, hw_version, hw_revision);
 	}
 
 	return rv;
 }
+
+/************************************************************************************
+  * Name: board_set_eeprom_hw_info
+ *
+ * Description:
+ * Function for writing hardware info to EEPROM
+ *
+ * Input Parameters:
+ *   *mtd_mft_unk - pointer to mtd_mft to write hw_info
+ *
+ * Returned Value:
+ *    0    - Successful storing to EEPROM
+ *   -1    - Error while storing to EEPROM
+ *
+ ************************************************************************************/
+
+int board_set_eeprom_hw_info(const char *path, mtd_mft_t *mtd_mft_unk)
+{
+	if (mtd_mft_unk == NULL || path == NULL) {
+		return -EINVAL;
+	}
+
+	// Later this will be a demux on type
+	if (mtd_mft_unk->id != MTD_MFT_v0) {
+		printf("Version is: %d, Only mft version %d is supported\n", mtd_mft_unk->id, MTD_MFT_v0);
+		return -EINVAL;
+	}
+
+	mtd_mft_v0_t *mtd_mft = (mtd_mft_v0_t *)mtd_mft_unk;
+
+	if (mtd_mft->hw_extended_id < HW_EEPROM_ID_MIN) {
+		printf("hardware version for EEPROM must be greater than %x\n", HW_EEPROM_ID_MIN);
+		return -EINVAL;
+	}
+
+	int fd = open(path, O_WRONLY);
+
+	if (fd < 0) {
+		return -errno;
+	}
+
+	int ret_val = OK;
+
+	mtd_mft->crc = crc16_signature(CRC16_INITIAL, sizeof(*mtd_mft) - sizeof(mtd_mft->crc), (uint8_t *) mtd_mft);
+
+	if (
+		(MTD_MFT_OFFSET != lseek(fd, MTD_MFT_OFFSET, SEEK_SET)) ||
+		(sizeof(*mtd_mft) != write(fd, mtd_mft, sizeof(*mtd_mft)))
+	) {
+		ret_val = -errno;
+	}
+
+	close(fd);
+
+	return ret_val;
+}
+
+/************************************************************************************
+  * Name: board_get_eeprom_hw_info
+ *
+ * Description:
+ * Function for reading hardware info from EEPROM
+ *
+ * Output Parameters:
+ *   *mtd_mft - pointer to mtd_mft to read hw_info
+ *
+ * Returned Value:
+ *    0    - Successful reading from EEPROM
+ *   -1    - Error while reading from EEPROM
+ *
+ ************************************************************************************/
+__EXPORT int board_get_eeprom_hw_info(const char *path, mtd_mft_t *mtd_mft)
+{
+	if (mtd_mft == NULL || path == NULL) {
+		return -EINVAL;
+	}
+
+	int fd = open(path, O_RDONLY);
+
+	if (fd < 0) {
+		return -errno;
+	}
+
+	int ret_val = OK;
+	mtd_mft_t format_version = {-1};
+
+	if (
+		(MTD_MFT_OFFSET != lseek(fd, MTD_MFT_OFFSET, SEEK_SET)) ||
+		(sizeof(format_version) != read(fd, &format_version, sizeof(format_version)))
+	) {
+		ret_val = -errno;
+
+	} else if (format_version.id != mtd_mft->id) {
+		ret_val = -EPROTO;
+
+	} else {
+
+		uint16_t mft_size = 0;
+
+		switch (format_version.id) {
+		case MTD_MFT_v0: mft_size = sizeof(mtd_mft_v0_t); break;
+
+		case MTD_MFT_v1: mft_size = sizeof(mtd_mft_v1_t); break;
+
+		default:
+			printf("[boot] Error, unknown version %d of mtd_mft in EEPROM\n", format_version.id);
+			ret_val = -1;
+			break;
+		}
+
+		if (ret_val == OK) {
+
+			if (
+				(MTD_MFT_OFFSET != lseek(fd, MTD_MFT_OFFSET, SEEK_SET)) ||
+				(mft_size != read(fd, mtd_mft, mft_size))
+			) {
+				ret_val = -errno;
+
+			} else {
+
+				union {
+					uint16_t w;
+					uint8_t  b[2];
+				} crc;
+
+				uint8_t *bytes = (uint8_t *) mtd_mft;
+				crc.w = crc16_signature(CRC16_INITIAL, mft_size - sizeof(crc), bytes);
+				uint8_t *eeprom_crc = &bytes[mft_size - sizeof(crc)];
+
+				if (!(crc.b[0] == eeprom_crc[0] && crc.b[1] == eeprom_crc[1])) {
+					ret_val = -1;
+				}
+			}
+		}
+	}
+
+	close(fd);
+	return ret_val;
+}
+
 #endif
diff --git a/platforms/nuttx/src/px4/stm/stm32_common/version/board_identity.c b/platforms/nuttx/src/px4/stm/stm32_common/version/board_identity.c
index 83d4390f5f..da2d5f1746 100644
--- a/platforms/nuttx/src/px4/stm/stm32_common/version/board_identity.c
+++ b/platforms/nuttx/src/px4/stm/stm32_common/version/board_identity.c
@@ -89,11 +89,11 @@ int board_get_uuid32_formated(char *format_buffer, int size,
 	int offset = 0;
 	int sep_size = seperator ? strlen(seperator) : 0;
 
-	for (unsigned i = 0; i < PX4_CPU_UUID_WORD32_LENGTH; i++) {
+	for (unsigned i = 0; (offset < size - 1) && (i < PX4_CPU_UUID_WORD32_LENGTH); i++) {
 		offset += snprintf(&format_buffer[offset], size - offset, format, uuid[i]);
 
-		if (sep_size && i < PX4_CPU_UUID_WORD32_LENGTH - 1) {
-			strcat(&format_buffer[offset], seperator);
+		if (sep_size && (offset < size - sep_size - 1) && (i < PX4_CPU_UUID_WORD32_LENGTH - 1)) {
+			strncat(&format_buffer[offset], seperator, size - offset);
 			offset += sep_size;
 		}
 	}
diff --git a/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp
index c86b67c73f..0ab41dd384 100644
--- a/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp
+++ b/platforms/nuttx/src/px4/stm/stm32f4/px4io_serial/px4io_serial.cpp
@@ -225,9 +225,6 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg)
 int
 ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 {
-	// to be paranoid ensure all previous DMA transfers are cleared
-	_abort_dma();
-
 	_current_packet = _packet;
 
 	/* clear any lingering error status */
@@ -278,8 +275,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 		DMA_SCR_PBURST_SINGLE	|
 		DMA_SCR_MBURST_SINGLE);
 	stm32_dmastart(_tx_dma, nullptr, nullptr, false);
-	//rCR1 &= ~USART_CR1_TE;
-	//rCR1 |= USART_CR1_TE;
 	rCR3 |= USART_CR3_DMAT;
 
 	/* compute the deadline for a 10ms timeout */
@@ -294,6 +289,7 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 
 	/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
 	int ret;
+	irqstate_t irqs = enter_critical_section();
 
 	for (;;) {
 		ret = sem_timedwait(&_completion_semaphore, &abstime);
@@ -301,42 +297,53 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 		if (ret == OK) {
 			/* check for DMA errors */
 			if (_rx_dma_status & DMA_STATUS_TEIF) {
-				// stream transfer error, ensure all DMA is also stopped before exiting early
-				_abort_dma();
+				/* One of 3 things has happened:
+				 *	1. a DMA Stream error
+				 *	2. Serial parity, framing or over run error
+				 *	3. packet is malformed
+				 * In all cases DMA is stopped by either HW or the ISR error service path.
+				 */
 				perf_count(_pc_dmaerrs);
 				ret = -EIO;
 				break;
-			}
 
-			/* check packet CRC - corrupt packet errors mean IO receive CRC error */
-			uint8_t crc = _current_packet->crc;
-			_current_packet->crc = 0;
-
-			if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) {
-				_abort_dma();
-				perf_count(_pc_crcerrs);
-				ret = -EIO;
+			} else {
+				/* successful DMA completion but the crc can still fail */
 				break;
 			}
 
-			/* successful txn (may still be reporting an error) */
-			break;
+		} else {
+			if (errno == ETIMEDOUT) {
+				/* something has broken - clear out any partial DMA state and reconfigure */
+				_abort_dma();
+				_rx_dma_status = _dma_status_inactive;
+				/* Wait fot at least a character time to make sure that there is no lingering
+				 * IDLE interrupt triggering right after we re-enable interrupts for the next
+				 * exchange
+				 */
+				usleep(100);
+				perf_count(_pc_timeouts);
+				perf_cancel(_pc_txns);		/* don't count this as a transaction */
+				break;
+			}
 		}
 
-		if (errno == ETIMEDOUT) {
-			/* something has broken - clear out any partial DMA state and reconfigure */
-			_abort_dma();
-			perf_count(_pc_timeouts);
-			perf_cancel(_pc_txns);		/* don't count this as a transaction */
-			break;
-		}
-
-		/* we might? see this for EINTR */
-		syslog(LOG_ERR, "unexpected ret %d/%d\n", ret, errno);
+		/* Loop in case we are interrupted on sleep */
 	}
 
-	/* reset DMA status */
 	_rx_dma_status = _dma_status_inactive;
+	leave_critical_section(irqs);
+
+	if (ret == OK) {
+		/* check packet CRC - corrupt packet errors mean IO receive CRC error */
+		uint8_t crc = _current_packet->crc;
+		_current_packet->crc = 0;
+
+		if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) {
+			perf_count(_pc_crcerrs);
+			ret = -EIO;
+		}
+	}
 
 	/* update counters */
 	perf_end(_pc_txns);
@@ -373,6 +380,8 @@ ArchPX4IOSerial::_do_rx_dma_callback(unsigned status)
 
 		/* disable UART DMA */
 		rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+		stm32_dmastop(_tx_dma);
+		stm32_dmastop(_rx_dma);
 
 		/* complete now */
 		px4_sem_post(&_completion_semaphore);
@@ -435,7 +444,7 @@ ArchPX4IOSerial::_do_interrupt()
 				/* stop the receive DMA */
 				stm32_dmastop(_rx_dma);
 
-				/* complete the short reception */
+				/* error flag completion of short reception */
 				_do_rx_dma_callback(DMA_STATUS_TEIF);
 				return;
 			}
diff --git a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/spi_hw_description.h b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/spi_hw_description.h
index 702635fdb4..f077fc7050 100644
--- a/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/spi_hw_description.h
+++ b/platforms/nuttx/src/px4/stm/stm32f7/include/px4_arch/spi_hw_description.h
@@ -32,6 +32,8 @@
  ****************************************************************************/
 #pragma once
 
+#include <board_config.h>
+
 #if defined(CONFIG_SPI)
 
 #include "../../../stm32_common/include/px4_arch/spi_hw_description.h"
diff --git a/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp
index a640166e05..2899c8db3f 100644
--- a/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp
+++ b/platforms/nuttx/src/px4/stm/stm32f7/px4io_serial/px4io_serial.cpp
@@ -237,9 +237,6 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg)
 int
 ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 {
-	// to be paranoid ensure all previous DMA transfers are cleared
-	_abort_dma();
-
 	_current_packet = _packet;
 
 	/* clear data that may be in the RDR and clear overrun error: */
@@ -298,8 +295,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 		DMA_SCR_MBURST_SINGLE);
 	rCR3 |= USART_CR3_DMAT;
 	stm32_dmastart(_tx_dma, nullptr, nullptr, false);
-	//rCR1 &= ~USART_CR1_TE;
-	//rCR1 |= USART_CR1_TE;
 
 	/* compute the deadline for a 10ms timeout */
 	struct timespec abstime;
@@ -313,6 +308,7 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 
 	/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
 	int ret;
+	irqstate_t irqs = enter_critical_section();
 
 	for (;;) {
 		ret = sem_timedwait(&_completion_semaphore, &abstime);
@@ -320,42 +316,53 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 		if (ret == OK) {
 			/* check for DMA errors */
 			if (_rx_dma_status & DMA_STATUS_TEIF) {
-				// stream transfer error, ensure all DMA is also stopped before exiting early
-				_abort_dma();
+				/* One of 3 things has happened:
+				 *	1. a DMA Stream error
+				 *	2. Serial parity, framing or over run error
+				 *	3. packet is malformed
+				 * In all cases DMA is stopped by either HW or the ISR error service path.
+				 */
 				perf_count(_pc_dmaerrs);
 				ret = -EIO;
 				break;
-			}
 
-			/* check packet CRC - corrupt packet errors mean IO receive CRC error */
-			uint8_t crc = _current_packet->crc;
-			_current_packet->crc = 0;
-
-			if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) {
-				_abort_dma();
-				perf_count(_pc_crcerrs);
-				ret = -EIO;
+			} else {
+				/* successful DMA completion but the crc can still fail */
 				break;
 			}
 
-			/* successful txn (may still be reporting an error) */
-			break;
+		} else {
+			if (errno == ETIMEDOUT) {
+				/* something has broken - clear out any partial DMA state and reconfigure */
+				_abort_dma();
+				_rx_dma_status = _dma_status_inactive;
+				/* Wait fot at least a character time to make sure that there is no lingering
+				 * IDLE interrupt triggering right after we re-enable interrupts for the next
+				 * exchange
+				 */
+				usleep(100);
+				perf_count(_pc_timeouts);
+				perf_cancel(_pc_txns);		/* don't count this as a transaction */
+				break;
+			}
 		}
 
-		if (errno == ETIMEDOUT) {
-			/* something has broken - clear out any partial DMA state and reconfigure */
-			_abort_dma();
-			perf_count(_pc_timeouts);
-			perf_cancel(_pc_txns);		/* don't count this as a transaction */
-			break;
-		}
-
-		/* we might? see this for EINTR */
-		syslog(LOG_ERR, "unexpected ret %d/%d\n", ret, errno);
+		/* Loop in case we are interrupted on sleep */
 	}
 
-	/* reset DMA status */
 	_rx_dma_status = _dma_status_inactive;
+	leave_critical_section(irqs);
+
+	if (ret == OK) {
+		/* check packet CRC - corrupt packet errors mean IO receive CRC error */
+		uint8_t crc = _current_packet->crc;
+		_current_packet->crc = 0;
+
+		if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) {
+			perf_count(_pc_crcerrs);
+			ret = -EIO;
+		}
+	}
 
 	/* update counters */
 	perf_end(_pc_txns);
@@ -393,6 +400,8 @@ ArchPX4IOSerial::_do_rx_dma_callback(unsigned status)
 
 		/* disable UART DMA */
 		rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+		stm32_dmastop(_tx_dma);
+		stm32_dmastop(_rx_dma);
 
 		/* complete now */
 		px4_sem_post(&_completion_semaphore);
@@ -463,7 +472,7 @@ ArchPX4IOSerial::_do_interrupt()
 				/* stop the receive DMA */
 				stm32_dmastop(_rx_dma);
 
-				/* complete the short reception */
+				/* error flag completion of short reception */
 				_do_rx_dma_callback(DMA_STATUS_TEIF);
 				return;
 			}
@@ -482,12 +491,13 @@ ArchPX4IOSerial::_do_interrupt()
 void
 ArchPX4IOSerial::_abort_dma()
 {
+	/* disable UART DMA */
+	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+
 	/* stop DMA */
 	stm32_dmastop(_tx_dma);
 	stm32_dmastop(_rx_dma);
 
-	/* disable UART DMA */
-	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
 
 	/* clear data that may be in the RDR and clear overrun error: */
 	if (rISR & USART_ISR_RXNE) {
diff --git a/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp b/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp
index 1492a7fc19..3b97f834e0 100644
--- a/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp
+++ b/platforms/nuttx/src/px4/stm/stm32h7/px4io_serial/px4io_serial.cpp
@@ -275,9 +275,6 @@ ArchPX4IOSerial::ioctl(unsigned operation, unsigned &arg)
 int
 ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 {
-	// to be paranoid ensure all previous DMA transfers are cleared
-	_abort_dma();
-
 	_current_packet = _packet;
 
 	/* clear data that may be in the RDR and clear overrun error: */
@@ -345,8 +342,6 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 	stm32_dmasetup(_tx_dma, &txdmacfg);
 	rCR3 |= USART_CR3_DMAT;
 	stm32_dmastart(_tx_dma, nullptr, nullptr, false);
-	//rCR1 &= ~USART_CR1_TE;
-	//rCR1 |= USART_CR1_TE;
 
 	/* compute the deadline for a 10ms timeout */
 	struct timespec abstime;
@@ -360,6 +355,7 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 
 	/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
 	int ret;
+	irqstate_t irqs = enter_critical_section();
 
 	for (;;) {
 		ret = sem_timedwait(&_completion_semaphore, &abstime);
@@ -367,42 +363,53 @@ ArchPX4IOSerial::_bus_exchange(IOPacket *_packet)
 		if (ret == OK) {
 			/* check for DMA errors */
 			if (_rx_dma_status & DMA_STATUS_TEIF) {
-				// stream transfer error, ensure all DMA is also stopped before exiting early
-				_abort_dma();
+				/* One of 3 things has happened:
+				 *	1. a DMA Stream error
+				 *	2. Serial parity, framing or over run error
+				 *	3. packet is malformed
+				 * In all cases DMA is stopped by either HW or the ISR error service path.
+				 */
 				perf_count(_pc_dmaerrs);
 				ret = -EIO;
 				break;
-			}
 
-			/* check packet CRC - corrupt packet errors mean IO receive CRC error */
-			uint8_t crc = _current_packet->crc;
-			_current_packet->crc = 0;
-
-			if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) {
-				_abort_dma();
-				perf_count(_pc_crcerrs);
-				ret = -EIO;
+			} else {
+				/* successful DMA completion but the crc can still fail */
 				break;
 			}
 
-			/* successful txn (may still be reporting an error) */
-			break;
+		} else {
+			if (errno == ETIMEDOUT) {
+				/* something has broken - clear out any partial DMA state and reconfigure */
+				_abort_dma();
+				_rx_dma_status = _dma_status_inactive;
+				/* Wait fot at least a character time to make sure that there is no lingering
+				 * IDLE interrupt triggering right after we re-enable interrupts for the next
+				 * exchange
+				 */
+				usleep(100);
+				perf_count(_pc_timeouts);
+				perf_cancel(_pc_txns);    /* don't count this as a transaction */
+				break;
+			}
 		}
 
-		if (errno == ETIMEDOUT) {
-			/* something has broken - clear out any partial DMA state and reconfigure */
-			_abort_dma();
-			perf_count(_pc_timeouts);
-			perf_cancel(_pc_txns);    /* don't count this as a transaction */
-			break;
-		}
-
-		/* we might? see this for EINTR */
-		syslog(LOG_ERR, "unexpected ret %d/%d\n", ret, errno);
+		/* Loop in case we are interrupted on sleep */
 	}
 
-	/* reset DMA status */
 	_rx_dma_status = _dma_status_inactive;
+	leave_critical_section(irqs);
+
+	if (ret == OK) {
+		/* check packet CRC - corrupt packet errors mean IO receive CRC error */
+		uint8_t crc = _current_packet->crc;
+		_current_packet->crc = 0;
+
+		if ((crc != crc_packet(_current_packet)) || (PKT_CODE(*_current_packet) == PKT_CODE_CORRUPT)) {
+			perf_count(_pc_crcerrs);
+			ret = -EIO;
+		}
+	}
 
 	/* update counters */
 	perf_end(_pc_txns);
@@ -440,6 +447,8 @@ ArchPX4IOSerial::_do_rx_dma_callback(unsigned status)
 
 		/* disable UART DMA */
 		rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+		stm32_dmastop(_tx_dma);
+		stm32_dmastop(_rx_dma);
 
 		/* complete now */
 		px4_sem_post(&_completion_semaphore);
@@ -510,7 +519,7 @@ ArchPX4IOSerial::_do_interrupt()
 				/* stop the receive DMA */
 				stm32_dmastop(_rx_dma);
 
-				/* complete the short reception */
+				/* error flag completion of short reception */
 				_do_rx_dma_callback(DMA_STATUS_TEIF);
 				return;
 			}
@@ -529,13 +538,13 @@ ArchPX4IOSerial::_do_interrupt()
 void
 ArchPX4IOSerial::_abort_dma()
 {
+	/* disable UART DMA */
+	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
+
 	/* stop DMA */
 	stm32_dmastop(_tx_dma);
 	stm32_dmastop(_rx_dma);
 
-	/* disable UART DMA */
-	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
-
 	/* clear data that may be in the RDR and clear overrun error: */
 	if (rISR & USART_ISR_RXNE) {
 		(void)rRDR;
diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake
index a71e3efbcf..2222f2853e 100644
--- a/platforms/posix/cmake/sitl_target.cmake
+++ b/platforms/posix/cmake/sitl_target.cmake
@@ -149,6 +149,7 @@ set(models
 	if750a
 	iris
 	iris_ctrlalloc
+	iris_depth_camera
 	iris_dual_gps
 	iris_foggy_lidar
 	iris_irlock
@@ -166,6 +167,7 @@ set(models
 	px4vision
 	r1_rover
 	rover
+	rover_gimbal
 	shell
 	solo
 	standard_vtol
@@ -181,7 +183,10 @@ set(models
 
 set(worlds
 	none
+	almend
+	almend_castle
 	baylands
+	crane
 	empty
 	ksql_airport
 	mcmillan_airfield
@@ -189,6 +194,7 @@ set(worlds
 	warehouse
 	windy
 	yosemite
+	camp_roberts
 )
 
 set(all_posix_vmd_make_targets)
diff --git a/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h b/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h
index 2e5a1eaab5..05cf64665a 100644
--- a/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h
+++ b/platforms/posix/src/px4/common/lockstep_scheduler/include/lockstep_scheduler/lockstep_scheduler.h
@@ -39,6 +39,7 @@
 #include <memory>
 #include <atomic>
 #include <pthread.h>
+#include <unistd.h>
 
 #include "lockstep_components.h"
 
diff --git a/src/drivers/adc/board_adc/ADC.cpp b/src/drivers/adc/board_adc/ADC.cpp
index cccd64ff96..7c2b822bd6 100644
--- a/src/drivers/adc/board_adc/ADC.cpp
+++ b/src/drivers/adc/board_adc/ADC.cpp
@@ -39,8 +39,9 @@
 #include <nuttx/ioexpander/gpio.h>
 #endif
 
-ADC::ADC(uint32_t base_address, uint32_t channels) :
+ADC::ADC(uint32_t base_address, uint32_t channels, bool publish_adc_report) :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
+	_publish_adc_report(publish_adc_report),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sample")),
 	_base_address(base_address)
 {
@@ -116,7 +117,10 @@ void ADC::Run()
 		_samples[i].am_data = sample(_samples[i].am_channel);
 	}
 
-	update_adc_report(now);
+	if (_publish_adc_report) {
+		update_adc_report(now);
+	}
+
 	update_system_power(now);
 }
 
@@ -352,7 +356,8 @@ int ADC::custom_command(int argc, char *argv[])
 
 int ADC::task_spawn(int argc, char *argv[])
 {
-	ADC *instance = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS);
+	bool publish_adc_report = !(argc >= 2 && strcmp(argv[1], "-n") == 0);
+	ADC *instance = new ADC(SYSTEM_ADC_BASE, ADC_CHANNELS, publish_adc_report);
 
 	if (instance) {
 		_object.store(instance);
@@ -389,6 +394,7 @@ ADC driver.
 	PRINT_MODULE_USAGE_NAME("adc", "driver");
 	PRINT_MODULE_USAGE_COMMAND("start");
 	PRINT_MODULE_USAGE_COMMAND("test");
+	PRINT_MODULE_USAGE_PARAM_FLAG('n', "Do not publish ADC report, only system power", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
 	return 0;
diff --git a/src/drivers/adc/board_adc/ADC.hpp b/src/drivers/adc/board_adc/ADC.hpp
index ab78fe3329..93613440a6 100644
--- a/src/drivers/adc/board_adc/ADC.hpp
+++ b/src/drivers/adc/board_adc/ADC.hpp
@@ -64,7 +64,7 @@ using namespace time_literals;
 class ADC : public ModuleBase<ADC>, public px4::ScheduledWorkItem
 {
 public:
-	ADC(uint32_t base_address = SYSTEM_ADC_BASE, uint32_t channels = ADC_CHANNELS);
+	ADC(uint32_t base_address = SYSTEM_ADC_BASE, uint32_t channels = ADC_CHANNELS, bool publish_adc_report = true);
 
 	~ADC() override;
 
@@ -102,6 +102,8 @@ private:
 
 	static const hrt_abstime	kINTERVAL{10_ms};	/**< 100Hz base rate */
 
+	bool 				_publish_adc_report;
+
 	perf_counter_t			_sample_perf;
 
 	unsigned			_channel_count{0};
@@ -110,6 +112,7 @@ private:
 
 	uORB::Publication<adc_report_s>		_to_adc_report{ORB_ID(adc_report)};
 	uORB::Publication<system_power_s>	_to_system_power{ORB_ID(system_power)};
+
 #ifdef BOARD_GPIO_VDD_5V_COMP_VALID
 	int _5v_comp_valid_fd {-1};
 #endif
diff --git a/src/drivers/barometer/bmp388/bmp388.cpp b/src/drivers/barometer/bmp388/bmp388.cpp
index ebfa5f9b52..8178bc5785 100644
--- a/src/drivers/barometer/bmp388/bmp388.cpp
+++ b/src/drivers/barometer/bmp388/bmp388.cpp
@@ -68,11 +68,19 @@ BMP388::init()
 		return -EIO;
 	}
 
-	if (_interface->get_reg(BMP3_CHIP_ID_ADDR) != BMP3_CHIP_ID) {
-		PX4_WARN("id of your baro is not: 0x%02x", BMP3_CHIP_ID);
+	_chip_id = _interface->get_reg(BMP3_CHIP_ID_ADDR);
+
+	if (_chip_id != BMP388_CHIP_ID && _chip_id != BMP390_CHIP_ID) {
+		PX4_WARN("id of your baro is not: 0x%02x or 0x%02x", BMP388_CHIP_ID, BMP390_CHIP_ID);
 		return -EIO;
 	}
 
+	if (_chip_id == BMP390_CHIP_ID) {
+		_interface->set_device_type(DRV_BARO_DEVTYPE_BMP390);
+	}
+
+	_chip_rev_id = _interface->get_reg(BMP3_REV_ID_ADDR);
+
 	_cal = _interface->get_calibration(BMP3_CALIB_DATA_ADDR);
 
 	if (!_cal) {
@@ -99,6 +107,7 @@ void
 BMP388::print_status()
 {
 	I2CSPIDriverBase::print_status();
+	printf("chip id:  0x%02x rev id:  0x%02x\n", _chip_id, _chip_rev_id);
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_measure_perf);
 	perf_print_counter(_comms_errors);
diff --git a/src/drivers/barometer/bmp388/bmp388.h b/src/drivers/barometer/bmp388/bmp388.h
index 1fd5f4a418..2db3e22382 100644
--- a/src/drivers/barometer/bmp388/bmp388.h
+++ b/src/drivers/barometer/bmp388/bmp388.h
@@ -51,7 +51,8 @@
 
 // From https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3_defs.h
 
-#define BMP3_CHIP_ID                      (0x50)
+#define BMP388_CHIP_ID                    (0x50)
+#define BMP390_CHIP_ID                    (0x60)
 
 /* Over sampling macros */
 #define BMP3_NO_OVERSAMPLING              (0x00)
@@ -79,6 +80,7 @@
 
 /* Register Address */
 #define BMP3_CHIP_ID_ADDR                 (0x00)
+#define BMP3_REV_ID_ADDR                  (0x01)
 #define BMP3_ERR_REG_ADDR                 (0x02)
 #define BMP3_SENS_STATUS_REG_ADDR         (0x03)
 #define BMP3_DATA_ADDR                    (0x04)
@@ -304,6 +306,8 @@ public:
 	virtual uint32_t get_device_id() const = 0;
 
 	virtual uint8_t get_device_address() const = 0;
+
+	virtual void set_device_type(uint8_t devtype) = 0;
 };
 
 class BMP388 : public I2CSPIDriver<BMP388>
@@ -339,6 +343,9 @@ private:
 
 	bool			_collect_phase{false};
 
+	uint8_t                 _chip_id{0};
+	uint8_t                 _chip_rev_id{0};
+
 	void 			start();
 	int 			measure();
 	int			collect(); //get results and publish
diff --git a/src/drivers/barometer/bmp388/bmp388_i2c.cpp b/src/drivers/barometer/bmp388/bmp388_i2c.cpp
index 5a3ea5e5be..0b735d35df 100644
--- a/src/drivers/barometer/bmp388/bmp388_i2c.cpp
+++ b/src/drivers/barometer/bmp388/bmp388_i2c.cpp
@@ -58,6 +58,7 @@ public:
 
 	uint8_t get_device_address() const override { return device::I2C::get_device_address(); }
 
+	void set_device_type(uint8_t devtype);
 private:
 	struct calibration_s _cal;
 };
@@ -108,3 +109,8 @@ calibration_s *BMP388_I2C::get_calibration(uint8_t addr)
 		return nullptr;
 	}
 }
+
+void BMP388_I2C::set_device_type(uint8_t devtype)
+{
+	device::Device::set_device_type(devtype);
+}
diff --git a/src/drivers/barometer/bmp388/bmp388_spi.cpp b/src/drivers/barometer/bmp388/bmp388_spi.cpp
index 2943c03692..18479a4aaf 100644
--- a/src/drivers/barometer/bmp388/bmp388_spi.cpp
+++ b/src/drivers/barometer/bmp388/bmp388_spi.cpp
@@ -74,6 +74,8 @@ public:
 	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(); }
+
+	void set_device_type(uint8_t devtype);
 private:
 	spi_calibration_s _cal;
 };
@@ -124,3 +126,8 @@ calibration_s *BMP388_SPI::get_calibration(uint8_t addr)
 		return nullptr;
 	}
 }
+
+void BMP388_SPI::set_device_type(uint8_t devtype)
+{
+	device::Device::set_device_type(devtype);
+}
diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp
index 1dfac055b4..749c27195c 100644
--- a/src/drivers/batt_smbus/batt_smbus.cpp
+++ b/src/drivers/batt_smbus/batt_smbus.cpp
@@ -166,7 +166,7 @@ void BATT_SMBUS::RunImpl()
 	if (ret == PX4_OK) {
 		new_report.capacity = _batt_capacity;
 		new_report.cycle_count = _cycle_count;
-		new_report.serial_number = _serial_number;
+		memcpy(new_report.serial_number, _serial_number, sizeof(new_report.serial_number));
 		new_report.max_cell_voltage_delta = _max_cell_voltage_delta;
 		new_report.cell_count = _cell_count;
 		new_report.state_of_health = _state_of_health;
@@ -392,7 +392,7 @@ int BATT_SMBUS::get_startup_info()
 	ret |= _interface->read_word(BATT_SMBUS_STATE_OF_HEALTH, state_of_health);
 
 	if (!ret) {
-		_serial_number = serial_num;
+		itoa(serial_num, _serial_number, 10);
 		_batt_startup_capacity = (uint16_t)((float)remaining_cap * _c_mult);
 		_cycle_count = cycle_count;
 		_batt_capacity = (uint16_t)((float)full_cap * _c_mult);
@@ -567,7 +567,7 @@ BATT_SMBUS::custom_method(const BusCLIArguments &cli)
 		case 1: {
 			PX4_INFO("The manufacturer name: %s", _manufacturer_name);
 			PX4_INFO("The manufacturer date: %" PRId16, _manufacture_date);
-			PX4_INFO("The serial number: %d" PRId16, _serial_number);
+			PX4_INFO("The serial number: %d" PRId16, atoi(_serial_number));
 		}
 			break;
 		case 2:
diff --git a/src/drivers/batt_smbus/batt_smbus.h b/src/drivers/batt_smbus/batt_smbus.h
index 1915865c80..7b94cb44fa 100644
--- a/src/drivers/batt_smbus/batt_smbus.h
+++ b/src/drivers/batt_smbus/batt_smbus.h
@@ -257,7 +257,7 @@ private:
 	uint16_t _cycle_count{0};
 
 	/** @param _serial_number Serial number register. */
-	uint16_t _serial_number{0};
+	char _serial_number[sizeof(battery_status_s::serial_number)] {};
 
 	/** @param _crit_thr Critical battery threshold param. */
 	float _crit_thr{0.f};
diff --git a/src/drivers/camera_capture/camera_capture.cpp b/src/drivers/camera_capture/camera_capture.cpp
index 7d1183b2df..3b4c4f4930 100644
--- a/src/drivers/camera_capture/camera_capture.cpp
+++ b/src/drivers/camera_capture/camera_capture.cpp
@@ -40,11 +40,15 @@
  */
 
 #include "camera_capture.hpp"
+#include <px4_platform_common/events.h>
+#include <systemlib/mavlink_log.h>
 
 #include <board_config.h>
 
 #define commandParamToInt(n) static_cast<int>(n >= 0 ? n + 0.5f : n - 0.5f)
 
+using namespace time_literals;
+
 namespace camera_capture
 {
 CameraCapture *g_camera_capture{nullptr};
@@ -104,6 +108,21 @@ CameraCapture::~CameraCapture()
 void
 CameraCapture::capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow)
 {
+	// Maximum acceptable rate is 5kHz
+	if ((edge_time - _trigger.hrt_edge_time) < 200_us) {
+		++_trigger_rate_exceeded_counter;
+
+		if (_trigger_rate_exceeded_counter > 100) {
+
+			// Trigger rate too high, stop future interrupts
+			up_input_capture_set(_capture_channel, Disabled, 0, nullptr, nullptr);
+			_trigger_rate_failure.store(true);
+		}
+
+	} else if (_trigger_rate_exceeded_counter > 0) {
+		--_trigger_rate_exceeded_counter;
+	}
+
 	_trigger.chan_index = chan_index;
 	_trigger.hrt_edge_time = edge_time;
 	_trigger.edge_state = edge_state;
@@ -140,6 +159,13 @@ CameraCapture::publish_trigger()
 {
 	bool publish = false;
 
+	if (_trigger_rate_failure.load()) {
+		mavlink_log_warning(&_mavlink_log_pub, "Hardware fault: Camera capture disabled\t");
+		events::send(events::ID("camera_capture_trigger_rate_exceeded"),
+			     events::Log::Error, "Hardware fault: Camera capture disabled");
+		_trigger_rate_failure.store(false);
+	}
+
 	camera_trigger_s trigger{};
 
 	// MODES 1 and 2 are not fully tested
@@ -244,7 +270,7 @@ CameraCapture::Run()
 
 			command_ack.timestamp = hrt_absolute_time();
 			command_ack.command = cmd.command;
-			command_ack.result = (uint8_t)vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			command_ack.result = (uint8_t)vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 			command_ack.target_system = cmd.source_system;
 			command_ack.target_component = cmd.source_component;
 
diff --git a/src/drivers/camera_capture/camera_capture.hpp b/src/drivers/camera_capture/camera_capture.hpp
index baeebb21a3..b1905da767 100644
--- a/src/drivers/camera_capture/camera_capture.hpp
+++ b/src/drivers/camera_capture/camera_capture.hpp
@@ -96,7 +96,7 @@ public:
 	static struct work_s	_work_publisher;
 
 private:
-	int _capture_channel = 5; ///< by default, use FMU output 6
+	int _capture_channel = 6; ///< by default, use FMU output 7
 
 	// Publishers
 	uORB::Publication<vehicle_command_ack_s>	_command_ack_pub{ORB_ID(vehicle_command_ack)};
@@ -134,6 +134,10 @@ private:
 
 	hrt_abstime	_pps_hrt_timestamp{0};
 	uint64_t		_pps_rtc_timestamp{0};
+	uint8_t			_trigger_rate_exceeded_counter{0};
+	px4::atomic<bool> _trigger_rate_failure{false};
+
+	orb_advert_t _mavlink_log_pub{nullptr};
 
 	// Signal capture callback
 	void			capture_callback(uint32_t chan_index, hrt_abstime edge_time, uint32_t edge_state, uint32_t overflow);
diff --git a/src/drivers/camera_trigger/camera_trigger.cpp b/src/drivers/camera_trigger/camera_trigger.cpp
index 8885e732f2..86b48cfacd 100644
--- a/src/drivers/camera_trigger/camera_trigger.cpp
+++ b/src/drivers/camera_trigger/camera_trigger.cpp
@@ -508,7 +508,7 @@ CameraTrigger::Run()
 	int poll_interval_usec = 50000;
 
 	vehicle_command_s cmd{};
-	unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+	unsigned cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 	bool need_ack = false;
 
 	// this flag is set when the polling loop is slowed down to allow the camera to power on
@@ -530,7 +530,7 @@ CameraTrigger::Run()
 
 			if (now - _last_trigger_timestamp < _min_interval * 1000) {
 				// triggering too fast, abort
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 
 			} else {
 				if (commandParamToInt(cmd.param7) == 1) {
@@ -543,7 +543,7 @@ CameraTrigger::Run()
 					_one_shot = true;
 				}
 
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 			}
 
 		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
@@ -570,7 +570,7 @@ CameraTrigger::Run()
 				_trigger_enabled = false;
 			}
 
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST) {
 			PX4_DEBUG("received DO_SET_CAM_TRIGG_DIST");
@@ -608,7 +608,7 @@ CameraTrigger::Run()
 				_one_shot = true;
 			}
 
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL) {
 			PX4_DEBUG("received DO_SET_CAM_TRIGG_INTERVAL");
@@ -627,7 +627,7 @@ CameraTrigger::Run()
 				}
 			}
 
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_OBLIQUE_SURVEY) {
 			PX4_INFO("received OBLIQUE_SURVEY");
@@ -676,7 +676,7 @@ CameraTrigger::Run()
 				_CAMPOS_num_poses = 0;
 			}
 
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 		} else {
 			goto unknown_cmd;
diff --git a/src/drivers/cyphal/NodeManager.cpp b/src/drivers/cyphal/NodeManager.cpp
index 87596df3af..cf0d637c38 100644
--- a/src/drivers/cyphal/NodeManager.cpp
+++ b/src/drivers/cyphal/NodeManager.cpp
@@ -87,6 +87,11 @@ bool NodeManager::HandleNodeIDRequest(uavcan_pnp_NodeIDAllocationData_1_0 &msg)
 			}
 		}
 
+		if (i + 1 > sizeof(nodeid_registry) / sizeof(nodeid_registry[0])) {
+			PX4_ERR("access out of bounds nodeid_registry");
+			return false;
+		}
+
 		nodeid_registry[i].register_setup = false; // Re-instantiate register setup
 		nodeid_registry[i].register_index = 0;
 		nodeid_registry[i].retry_count = 0;
diff --git a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp
index 4a4d316ee0..8640c932de 100644
--- a/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp
+++ b/src/drivers/cyphal/Subscribers/legacy/LegacyBatteryInfo.hpp
@@ -96,7 +96,7 @@ public:
 		bat_status.connected = bat_info.status_flags & legacy_equipment_power_BatteryInfo_1_0_STATUS_FLAG_IN_USE;
 		bat_status.source = 1; // External
 		bat_status.capacity = bat_info.full_charge_capacity_wh;
-		bat_status.serial_number = bat_info.model_instance_id & 0xFFFF; // Take first 16 bits
+		sprintf(bat_status.serial_number, "%d", (int)(bat_info.model_instance_id & 0xFFFF));
 		bat_status.state_of_health = bat_info.state_of_health_pct; // External
 		bat_status.id = bat_info.battery_id;
 
diff --git a/src/drivers/cyphal/Subscribers/udral/Battery.hpp b/src/drivers/cyphal/Subscribers/udral/Battery.hpp
index 732c3dc2e8..b5216a7885 100644
--- a/src/drivers/cyphal/Subscribers/udral/Battery.hpp
+++ b/src/drivers/cyphal/Subscribers/udral/Battery.hpp
@@ -163,7 +163,7 @@ public:
 
 			bat_status.capacity = parameters.design_capacity.coulomb / 3.6f; // Coulomb -> mAh
 			bat_status.cycle_count = parameters.cycle_count;
-			bat_status.serial_number = parameters.unique_id & 0xFFFF;
+			sprintf(bat_status.serial_number, "%d", (int)(parameters.unique_id & 0xFFFF));
 			bat_status.state_of_health = parameters.state_of_health_pct;
 			bat_status.max_error = 1; // UAVCAN didn't spec'ed this, but we're optimistic
 			bat_status.id = 0; //TODO instancing
diff --git a/src/drivers/differential_pressure/Kconfig b/src/drivers/differential_pressure/Kconfig
index a34dc5a40e..cfd7e0d05a 100644
--- a/src/drivers/differential_pressure/Kconfig
+++ b/src/drivers/differential_pressure/Kconfig
@@ -7,6 +7,7 @@ menu "Differential pressure"
         select DRIVERS_DIFFERENTIAL_PRESSURE_MS4525DO
         select DRIVERS_DIFFERENTIAL_PRESSURE_MS5525DSO
         select DRIVERS_DIFFERENTIAL_PRESSURE_SDP3X
+        select DRIVERS_DIFFERENTIAL_PRESSURE_SDP6X
         ---help---
             Enable default set of differential pressure drivers
     rsource "*/Kconfig"
diff --git a/src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp b/src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp
index ddc48d36de..8b99868b32 100644
--- a/src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp
+++ b/src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp
@@ -53,6 +53,7 @@ extern "C" int sdp3x_main(int argc, char *argv[])
 	BusCLIArguments cli{true, false};
 	cli.i2c_address = I2C_ADDRESS_1_SDP3X;
 	cli.default_i2c_frequency = I2C_SPEED;
+	cli.support_keep_running = true;
 
 	const char *verb = cli.parseDefaultArguments(argc, argv);
 
diff --git a/src/drivers/differential_pressure/sdp6x/CMakeLists.txt b/src/drivers/differential_pressure/sdp6x/CMakeLists.txt
new file mode 100644
index 0000000000..e22b625e31
--- /dev/null
+++ b/src/drivers/differential_pressure/sdp6x/CMakeLists.txt
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2017 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__differential_pressure__sdp6x
+	MAIN sdp6x
+	COMPILE_FLAGS
+	SRCS
+		SDP6X.cpp
+		SDP6X_main.cpp
+	DEPENDS
+		px4_work_queue
+	)
diff --git a/src/drivers/differential_pressure/sdp6x/Kconfig b/src/drivers/differential_pressure/sdp6x/Kconfig
new file mode 100644
index 0000000000..5de55cc9a6
--- /dev/null
+++ b/src/drivers/differential_pressure/sdp6x/Kconfig
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_DIFFERENTIAL_PRESSURE_SDP6X
+	bool "sdp6x"
+	default n
+	---help---
+		Enable support for sdp6x
diff --git a/src/drivers/differential_pressure/sdp6x/SDP6X.cpp b/src/drivers/differential_pressure/sdp6x/SDP6X.cpp
new file mode 100644
index 0000000000..e596043561
--- /dev/null
+++ b/src/drivers/differential_pressure/sdp6x/SDP6X.cpp
@@ -0,0 +1,182 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file SDP6X.hpp
+ *
+ * Driver for Sensirion SDP6X Differential Pressure Sensor
+ *
+ */
+
+#include "SDP6X.hpp"
+
+SDP6X::SDP6X(const I2CSPIDriverConfig &config) :
+	I2C(config),
+	I2CSPIDriver(config)
+{
+}
+
+
+SDP6X::~SDP6X()
+{
+	perf_free(_sample_perf);
+	perf_free(_comms_errors);
+}
+
+int SDP6X::init()
+{
+	int ret = I2C::init();
+
+	if (ret != PX4_OK) {
+		DEVICE_DEBUG("I2C::init failed (%i)", ret);
+		return ret;
+	}
+
+	if (ret == PX4_OK) {
+		ScheduleNow();
+	}
+
+	return ret;
+}
+
+void SDP6X::print_status()
+{
+	I2CSPIDriverBase::print_status();
+
+	perf_print_counter(_sample_perf);
+	perf_print_counter(_comms_errors);
+}
+
+int
+SDP6X::probe()
+{
+	return !init_sdp6x();
+}
+
+bool
+SDP6X::init_sdp6x()
+{
+	uint8_t reset_cmd = SDP6X_RESET_CMD;
+	int ret = transfer(&reset_cmd, 1, nullptr, 0);
+
+	if (ret == 0) {
+		_device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP31;
+		// wait until sensor is ready
+		px4_usleep(20000);
+		ScheduleNow();
+	}
+
+	return ret == 0;
+}
+
+int
+SDP6X::collect()
+{
+	perf_begin(_sample_perf);
+
+	const hrt_abstime timestamp_sample = hrt_absolute_time();
+
+	// this takes about 5ms. But even if we split the write and read with a delay in between,
+	// the device will still block for several ms. So effectively can't run another device on that bus.
+	uint8_t measure_cmd = SDP6X_MEASURE_CMD;
+	uint8_t data[3];
+	int ret = transfer(&measure_cmd, 1, (uint8_t *)&data[0], sizeof(data));
+
+	if (ret != PX4_OK) {
+		perf_count(_comms_errors);
+		return ret;
+	}
+
+	// Check the CRC
+	if (!crc(&data[0], 2, data[2])) {
+		perf_count(_comms_errors);
+		return EAGAIN;
+	}
+
+	int16_t P = (((int16_t)data[0]) << 8) | data[1];
+
+	float diff_press_pa_raw = static_cast<float>(P) / 60.f; // 500Pa -> scale factor=60
+
+	differential_pressure_s differential_pressure{};
+
+	differential_pressure.timestamp = hrt_absolute_time();
+	differential_pressure.timestamp_sample = timestamp_sample;
+	differential_pressure.error_count = perf_event_count(_comms_errors);
+	differential_pressure.temperature = 0.f;
+	differential_pressure.differential_pressure_pa = diff_press_pa_raw;
+	differential_pressure.device_id = get_device_id();
+
+	_differential_pressure_pub.publish(differential_pressure);
+
+	perf_end(_sample_perf);
+
+	return ret;
+}
+
+void
+SDP6X::RunImpl()
+{
+	int ret = PX4_ERROR;
+
+	// measurement phase
+	ret = collect();
+
+	if (PX4_OK != ret) {
+		DEVICE_DEBUG("measure error");
+	}
+
+	// schedule a fresh cycle call when the measurement is done
+	ScheduleDelayed(CONVERSION_INTERVAL);
+}
+
+bool SDP6X::crc(const uint8_t data[], unsigned size, uint8_t checksum)
+{
+	uint8_t crc_value = 0;
+
+	// calculate 8-bit checksum with polynomial 0x31 (x^8 + x^5 + x^4 + 1)
+	for (unsigned i = 0; i < size; i++) {
+		crc_value ^= (data[i]);
+
+		for (int bit = 8; bit > 0; --bit) {
+			if (crc_value & 0x80) {
+				crc_value = (crc_value << 1) ^ 0x31;
+
+			} else {
+				crc_value = (crc_value << 1);
+			}
+		}
+	}
+
+	// verify checksum
+	return (crc_value == checksum);
+}
diff --git a/src/drivers/differential_pressure/sdp6x/SDP6X.hpp b/src/drivers/differential_pressure/sdp6x/SDP6X.hpp
new file mode 100644
index 0000000000..47bf5bef5e
--- /dev/null
+++ b/src/drivers/differential_pressure/sdp6x/SDP6X.hpp
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020-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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file SDP6X.hpp
+ *
+ * Driver for Sensirion SDP6X Differential Pressure Sensor
+ *
+ * Datasheet: https://www.sensirion.com/fileadmin/user_upload/customers/sensirion/Dokumente/8_Differential_Pressure/Datasheets/Sensirion_Differential_Pressure_Sensors_SDP600Series_Datasheet.pdf
+ *
+ * Note:
+ * - Expected to be used with SDP610 - 500Pa
+ * - The board needs to set:
+ *   CONFIG_STM32F7_I2C_DYNTIMEO_STARTSTOP=50
+ *   CONFIG_STM32F7_I2C_DYNTIMEO_USECPERBYTE=10000
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+#include <lib/drivers/device/i2c.h>
+#include <lib/perf/perf_counter.h>
+#include <px4_platform_common/i2c_spi_buses.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/differential_pressure.h>
+
+
+#define I2C_ADDRESS_SDP6X		0x40
+
+#define SDP6X_RESET_CMD			0xfe
+#define SDP6X_MEASURE_CMD			0xf1
+
+#define PATH_SDP6X "/dev/sdp6x"
+
+#define SPD6X_MEAS_RATE 20
+#define SDP6X_MEAS_DRIVER_FILTER_FREQ 3.0f
+#define CONVERSION_INTERVAL	(1000000 / SPD6X_MEAS_RATE)	/* microseconds */
+
+static constexpr uint32_t I2C_SPEED = 100 * 1000; // 100 kHz I2C serial interface
+
+class SDP6X : public device::I2C, public I2CSPIDriver<SDP6X>
+{
+public:
+	SDP6X(const I2CSPIDriverConfig &config);
+	~SDP6X() override;
+
+	static void print_usage();
+
+	void RunImpl();
+
+	int init() override;
+	void print_status() override;
+
+private:
+	int probe() override;
+
+	int collect();
+
+	bool init_sdp6x();
+
+	/**
+	 * Calculate the CRC8 for the sensor payload data
+	 */
+	bool crc(const uint8_t data[], unsigned size, uint8_t checksum);
+
+	uORB::PublicationMulti<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
+
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": communication errors")};
+};
diff --git a/src/drivers/differential_pressure/sdp6x/SDP6X_main.cpp b/src/drivers/differential_pressure/sdp6x/SDP6X_main.cpp
new file mode 100644
index 0000000000..b729f9acb3
--- /dev/null
+++ b/src/drivers/differential_pressure/sdp6x/SDP6X_main.cpp
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-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 "SDP6X.hpp"
+
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/module.h>
+
+void
+SDP6X::print_usage()
+{
+	PRINT_MODULE_USAGE_NAME("sdp6x", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
+	PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x21);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+}
+
+extern "C" int sdp6x_main(int argc, char *argv[])
+{
+	using ThisDriver = SDP6X;
+	BusCLIArguments cli{true, false};
+	cli.i2c_address = I2C_ADDRESS_SDP6X;
+	cli.default_i2c_frequency = I2C_SPEED;
+
+	const char *verb = cli.parseDefaultArguments(argc, argv);
+
+	if (!verb) {
+		ThisDriver::print_usage();
+		return -1;
+	}
+
+	BusInstanceIterator iterator(MODULE_NAME, cli, DRV_DIFF_PRESS_DEVTYPE_SDP31);
+
+	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;
+}
diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
index 2e4c6ba97d..b718f80664 100644
--- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
+++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
@@ -175,21 +175,21 @@ int LightwareLaser::init()
 
 	case 4:
 		/* SF11/c (120m 20Hz) */
-		_px4_rangefinder.set_min_distance(0.01f);
+		_px4_rangefinder.set_min_distance(0.2f);
 		_px4_rangefinder.set_max_distance(120.0f);
 		_conversion_interval = 50000;
 		break;
 
 	case 5:
 		/* SF/LW20/b (50m 48-388Hz) */
-		_px4_rangefinder.set_min_distance(0.001f);
+		_px4_rangefinder.set_min_distance(0.2f);
 		_px4_rangefinder.set_max_distance(50.0f);
 		_conversion_interval = 20834;
 		break;
 
 	case 6:
 		/* SF/LW20/c (100m 48-388Hz) */
-		_px4_rangefinder.set_min_distance(0.001f);
+		_px4_rangefinder.set_min_distance(0.2f);
 		_px4_rangefinder.set_max_distance(100.0f);
 		_conversion_interval = 20834;
 		_type = Type::LW20c;
diff --git a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp
index 359a62f575..415d9b8f48 100644
--- a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp
+++ b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp
@@ -106,7 +106,7 @@ LightwareLaserSerial::init()
 
 	case 5:
 		/* SF11/c (120m 20Hz) */
-		_px4_rangefinder.set_min_distance(0.01f);
+		_px4_rangefinder.set_min_distance(0.2f);
 		_px4_rangefinder.set_max_distance(120.0f);
 		_interval = 50000;
 		break;
diff --git a/src/drivers/distance_sensor/tfmini/tfmini_parser.cpp b/src/drivers/distance_sensor/tfmini/tfmini_parser.cpp
index 76f754634a..09f57723d3 100644
--- a/src/drivers/distance_sensor/tfmini/tfmini_parser.cpp
+++ b/src/drivers/distance_sensor/tfmini/tfmini_parser.cpp
@@ -58,7 +58,7 @@ const char *parser_state[] = {
 	"5_GOT_STRENGTH_L",
 	"6_GOT_STRENGTH_H",
 	"7_GOT_PRESERVED",
-	"8_GOT_QUALITY"
+	"8_GOT_QUALITY",
 	"9_GOT_CHECKSUM"
 };
 #endif
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 43721f9cb0..d6096d1f52 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -86,6 +86,11 @@ struct pwm_output_values {
  */
 #define PWM_MOTOR_OFF	900
 
+/**
+ * Default value for a servo stop
+ */
+#define PWM_SERVO_STOP	1500
+
 /**
  * Default minimum PWM in us
  */
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index 360c5b9913..48499bf522 100644
--- a/src/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
@@ -140,7 +140,7 @@
 #define DRV_FLOW_DEVTYPE_PAW3902	0x6d
 #define DRV_FLOW_DEVTYPE_PX4FLOW	0x6e
 #define DRV_FLOW_DEVTYPE_PAA3905	0x6f
-
+#define DRV_BARO_DEVTYPE_BMP390         0x6E
 
 #define DRV_DIST_DEVTYPE_LL40LS       0x70
 #define DRV_DIST_DEVTYPE_MAPPYDOT     0x71
diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp
index 122cfa391e..de97422716 100644
--- a/src/drivers/dshot/DShot.cpp
+++ b/src/drivers/dshot/DShot.cpp
@@ -624,7 +624,7 @@ void DShot::handle_vehicle_commands()
 			command_ack.command = vehicle_command.command;
 			command_ack.target_system = vehicle_command.source_system;
 			command_ack.target_component = vehicle_command.source_component;
-			command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+			command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 
 			if (index != -1) {
 				PX4_DEBUG("setting command: index: %i type: %i", index, type);
@@ -646,7 +646,7 @@ void DShot::handle_vehicle_commands()
 					PX4_WARN("unknown command: %i", type);
 
 				} else {
-					command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+					command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 					_current_command.motor_mask = 1 << index;
 					_current_command.num_repetitions = 10;
 					_current_command.save = true;
diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices
index 016c37cd1f..fa2177d690 160000
--- a/src/drivers/gps/devices
+++ b/src/drivers/gps/devices
@@ -1 +1 @@
-Subproject commit 016c37cd1f18c716427e2465d8daa6aa1054b0f1
+Subproject commit fa2177d690207e42e0d8c92e9663578340d44fe4
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index a4b3615657..9973af8075 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -103,7 +103,7 @@ struct GPS_Sat_Info {
 	satellite_info_s _data;
 };
 
-static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(1760);
+static constexpr int TASK_STACK_SIZE = PX4_STACK_ADJUSTED(1940);
 
 
 class GPS : public ModuleBase<GPS>, public device::Device
@@ -317,7 +317,7 @@ GPS::GPS(const char *path, gps_driver_mode_t mode, GPSHelper::Interface interfac
 		set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SERIAL);
 
 		char c = _port[strlen(_port) - 1]; // last digit of path (eg /dev/ttyS2)
-		set_device_bus(atoi(&c));
+		set_device_bus(c - 48); // sub 48 to convert char to integer
 
 	} else if (_interface == GPSHelper::Interface::SPI) {
 		set_device_bus_type(device::Device::DeviceBusType::DeviceBusType_SPI);
@@ -761,9 +761,20 @@ GPS::run()
 
 		} else if (gps_ubx_mode == 4) {
 			ubx_mode = GPSDriverUBX::UBXMode::MovingBaseUART1;
+
+		} else if (gps_ubx_mode == 5) { // rover with static base on Uart2
+			ubx_mode = GPSDriverUBX::UBXMode::RoverWithStaticBaseUart2;
+
 		}
 	}
 
+	handle = param_find("GPS_UBX_BAUD2");
+	int32_t f9p_uart2_baudrate = 57600;
+
+	if (handle != PARAM_INVALID) {
+		param_get(handle, &f9p_uart2_baudrate);
+	}
+
 	int32_t gnssSystemsParam = static_cast<int32_t>(GPSHelper::GNSSSystemsMask::RECEIVER_DEFAULTS);
 
 	if (_instance == Instance::Main) {
@@ -824,7 +835,7 @@ GPS::run()
 		/* FALLTHROUGH */
 		case gps_driver_mode_t::UBX:
 			_helper = new GPSDriverUBX(_interface, &GPS::callback, this, &_report_gps_pos, _p_report_sat_info,
-						   gps_ubx_dynmodel, heading_offset, ubx_mode);
+						   gps_ubx_dynmodel, heading_offset, f9p_uart2_baudrate, ubx_mode);
 			set_device_type(DRV_GPS_DEVTYPE_UBX);
 			break;
 #ifndef CONSTRAINED_FLASH
diff --git a/src/drivers/gps/params.c b/src/drivers/gps/params.c
index 25a0cb2934..256099c485 100644
--- a/src/drivers/gps/params.c
+++ b/src/drivers/gps/params.c
@@ -87,6 +87,7 @@ PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
  * Select the u-blox configuration setup. Most setups will use the default, including RTK and
  * dual GPS without heading.
  *
+ * If rover has RTCM corrections from a static base (or other static correction source) coming in on UART2, then select Mode 5.
  * The Heading mode requires 2 F9P devices to be attached. The main GPS will act as rover and output
  * heading information, whereas the secondary will act as moving base.
  * Modes 1 and 2 require each F9P UART1 to be connected to the Autopilot. In addition, UART2 on the
@@ -101,6 +102,7 @@ PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
  * @value 2 Moving Base (UART1 Connected To Autopilot, UART2 Connected To Rover)
  * @value 3 Heading (Rover With Moving Base UART1 Connected to Autopilot Or Can Node At 921600)
  * @value 4 Moving Base (Moving Base UART1 Connected to Autopilot Or Can Node At 921600)
+ * @value 5 Rover with Static Base on UART2 (similar to Default, except coming in on UART2)
  *
  * @reboot_required true
  * @group GPS
@@ -108,6 +110,22 @@ PARAM_DEFINE_INT32(GPS_SAT_INFO, 0);
 PARAM_DEFINE_INT32(GPS_UBX_MODE, 0);
 
 
+/**
+ * u-blox F9P UART2 Baudrate
+ *
+ * Select a baudrate for the F9P's UART2 port.
+ * In GPS_UBX_MODE 1, 2, and 3, the F9P's UART2 port is configured to send/receive RTCM corrections.
+ * Set this to 57600 if you want to attach a telemetry radio on UART2.
+ *
+ * @min 0
+ * @unit B/s
+ *
+ * @reboot_required true
+ * @group GPS
+ */
+PARAM_DEFINE_INT32(GPS_UBX_BAUD2, 230400);
+
+
 /**
  * Heading/Yaw offset for dual antenna GPS
  *
diff --git a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp
index 682539fe33..7ef5af0494 100644
--- a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp
+++ b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp
@@ -41,6 +41,7 @@
 #pragma once
 
 #include <cstdint>
+#include <cstddef>
 
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
diff --git a/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp b/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp
index 11999e9aeb..754812d3ea 100644
--- a/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp
+++ b/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp
@@ -41,6 +41,7 @@
 #pragma once
 
 #include <cstdint>
+#include <cstddef>
 
 // TODO: move to a central header
 static constexpr uint8_t Bit0 = (1 << 0);
diff --git a/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp b/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp
index 12fcbdbd62..827b2351d4 100644
--- a/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp
+++ b/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp
@@ -41,6 +41,7 @@
 #pragma once
 
 #include <cstdint>
+#include <cstddef>
 
 namespace InvenSense_ICM20948
 {
diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
index 8f149bdf5d..31382c0111 100644
--- a/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
+++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-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
@@ -171,21 +171,9 @@ void ICM42688P::RunImpl()
 
 	case STATE::CONFIGURE:
 		if (Configure()) {
-			// if configure succeeded then start reading from FIFO
-			_state = STATE::FIFO_READ;
-
-			if (DataReadyInterruptConfigure()) {
-				_data_ready_interrupt_enabled = true;
-
-				// backup schedule as a watchdog timeout
-				ScheduleDelayed(100_ms);
-
-			} else {
-				_data_ready_interrupt_enabled = false;
-				ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
-			}
-
-			FIFOReset();
+			// if configure succeeded then reset the FIFO
+			_state = STATE::FIFO_RESET;
+			ScheduleDelayed(1_ms);
 
 		} else {
 			// CONFIGURE not complete
@@ -202,6 +190,24 @@ void ICM42688P::RunImpl()
 
 		break;
 
+	case STATE::FIFO_RESET:
+
+		_state = STATE::FIFO_READ;
+		FIFOReset();
+
+		if (DataReadyInterruptConfigure()) {
+			_data_ready_interrupt_enabled = true;
+
+			// backup schedule as a watchdog timeout
+			ScheduleDelayed(100_ms);
+
+		} else {
+			_data_ready_interrupt_enabled = false;
+			ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us);
+		}
+
+		break;
+
 	case STATE::FIFO_READ: {
 			hrt_abstime timestamp_sample = now;
 			uint8_t samples = 0;
@@ -643,6 +649,9 @@ void ICM42688P::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DA
 		}
 	}
 
+	// avoid scale changes for now as it's not handled correctly down the pipeline (fixed in upstream with 8478d1ea37d2bf)
+	scale_20bit = true;
+
 	if (!scale_20bit) {
 		// if highres enabled accel data is always 8192 LSB/g
 		_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
@@ -722,6 +731,9 @@ void ICM42688P::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DAT
 		gyro.samples++;
 	}
 
+	// avoid scale changes for now as it's not handled correctly down the pipeline (fixed in upstream with 8478d1ea37d2bf)
+	scale_20bit = true;
+
 	if (!scale_20bit) {
 		// if highres enabled gyro data is always 131 LSB/dps
 		_px4_gyro.set_scale(math::radians(1.f / 131.f));
diff --git a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
index 96f1d1d8f0..b118f8c920 100644
--- a/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
+++ b/src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
@@ -165,6 +165,7 @@ private:
 		RESET,
 		WAIT_FOR_RESET,
 		CONFIGURE,
+		FIFO_RESET,
 		FIFO_READ,
 	} _state{STATE::RESET};
 
diff --git a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
index bf44666a01..b8d9260a49 100644
--- a/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
+++ b/src/drivers/imu/invensense/icm42688p/InvenSense_ICM42688P_registers.hpp
@@ -41,6 +41,7 @@
 #pragma once
 
 #include <cstdint>
+#include <cstddef>
 
 namespace InvenSense_ICM42688P
 {
diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp
index b7d8512ce9..e31e3f2ff7 100644
--- a/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp
+++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.cpp
@@ -35,6 +35,10 @@
 
 using namespace time_literals;
 
+#define IST8310_TEMP_RESOLUTION	(1/67.2)
+#define IST8310_TEMP_25C_OFFSET	27100
+#define IST8310_OTP_SENSITIVITY 330
+
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
 {
 	return (msb << 8u) | lsb;
@@ -81,18 +85,41 @@ void IST8310::print_status()
 	perf_print_counter(_reset_perf);
 	perf_print_counter(_bad_register_perf);
 	perf_print_counter(_bad_transfer_perf);
+	print_cross_axis_info();
+}
+
+void
+IST8310::print_cross_axis_info()
+{
+	PX4_INFO("cross axis cal: [%+3.2f,%+3.2f,%+3.2f|%+3.2f,%+3.2f,%+3.2f|%+3.2f,%+3.2f,%+3.2f]",
+		 (double)_crossaxis_inv[0],
+		 (double)_crossaxis_inv[1],
+		 (double)_crossaxis_inv[2],
+		 (double)_crossaxis_inv[3],
+		 (double)_crossaxis_inv[4],
+		 (double)_crossaxis_inv[5],
+		 (double)_crossaxis_inv[6],
+		 (double)_crossaxis_inv[7],
+		 (double)_crossaxis_inv[8]);
 }
 
 int IST8310::probe()
 {
-	const uint8_t WAI = RegisterRead(Register::WAI);
+	_retries = 1;
 
-	if (WAI != Device_ID) {
-		DEVICE_DEBUG("unexpected WAI 0x%02x", WAI);
-		return PX4_ERROR;
+	for (int i = 0; i < 10; i++) {
+		const uint8_t WAI = RegisterRead(Register::WAI);
+
+		if (WAI == Device_ID) {
+			return PX4_OK;
+
+		} else {
+			PX4_DEBUG("unexpected WAI 0x%02x", WAI);
+			px4_usleep(10);
+		}
 	}
 
-	return PX4_OK;
+	return PX4_ERROR;
 }
 
 void IST8310::RunImpl()
@@ -173,21 +200,34 @@ void IST8310::RunImpl()
 				uint8_t DATAZH;
 			} buffer{};
 
+			struct Temp {
+				uint8_t TEMPL;
+				uint8_t TEMPH;
+			} buffer_temp;
+
 			bool success = false;
 			uint8_t cmd = static_cast<uint8_t>(Register::STAT1);
+			uint8_t cmd_temp = static_cast<uint8_t>(Register::TEMPL);
 
-			if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {
+			if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK
+			    && transfer(&cmd_temp, 1, (uint8_t *)&buffer_temp, sizeof(buffer_temp)) == PX4_OK) {
 
 				if (buffer.STAT1 & STAT1_BIT::DRDY) {
 					int16_t x = combine(buffer.DATAXH, buffer.DATAXL);
 					int16_t y = combine(buffer.DATAYH, buffer.DATAYL);
 					int16_t z = combine(buffer.DATAZH, buffer.DATAZL);
+					int16_t t = combine(buffer_temp.TEMPH, buffer_temp.TEMPL);
 
-					// sensor's frame is +x forward, +y right, +z up
-					z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z
+					_px4_mag.set_temperature((IST8310_TEMP_25C_OFFSET - t) * IST8310_TEMP_RESOLUTION + 25);
+
+					float xf = (float)x * _crossaxis_inv[0] + (float)y * _crossaxis_inv[1] + (float)z * _crossaxis_inv[2];
+					float yf = (float)x * _crossaxis_inv[3] + (float)y * _crossaxis_inv[4] + (float)z * _crossaxis_inv[5];
+					float zf = (float)x * _crossaxis_inv[6] + (float)y * _crossaxis_inv[7] + (float)z * _crossaxis_inv[8];
 
 					_px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf));
-					_px4_mag.update(now, x, y, z);
+
+					// sensor's frame is +x forward, +y right, +z up
+					_px4_mag.update(now, xf, yf, -zf);
 
 					success = true;
 
@@ -251,9 +291,158 @@ bool IST8310::Configure()
 
 	_px4_mag.set_scale(1.f / 1320.f); // 1320 LSB/Gauss
 
+	/* Pull & process cross-axis compensation */
+	cross_axis_comp();
+
 	return success;
 }
 
+void
+IST8310::cross_axis_comp()
+{
+	int ret;
+	uint8_t bOTPDataFlags;
+	uint8_t tempBuff[2];
+	bool crossaxis_enable = 0;
+	float inv[9] = {0};
+	short OTPcrossaxis[9] = {0};
+
+	uint8_t crossxbuf[6];
+	uint8_t crossybuf[6];
+	uint8_t crosszbuf[6];
+
+	update_crossaxis();
+
+	//check otp_date (Sensors before some date appears to have the matrix stored transposed)
+	bOTPDataFlags = 0;
+
+	ret = read(Register::OTP, (uint8_t *)&tempBuff, sizeof(tempBuff));
+
+	if (OK != ret) {
+		perf_count(_bad_transfer_perf);
+		return;
+	}
+
+
+	if (tempBuff[0] == 0xff) {
+		bOTPDataFlags = 0;
+
+	} else if (tempBuff[0] <= 0x12) {
+		if (tempBuff[1] <= 0x07) {
+			bOTPDataFlags = 1;
+		}
+	}
+
+	ret = read(Register::XX_CROSS_L, (uint8_t *)&tempBuff, sizeof(tempBuff));
+
+	if (OK != ret) {
+		perf_count(_bad_transfer_perf);
+		return;
+	}
+
+	if ((tempBuff[0] == 0xFF) && (tempBuff[1] == 0xFF)) {
+		crossaxis_enable = 0;
+
+	} else {
+		crossaxis_enable = 1;
+	}
+
+	if (crossaxis_enable == 0) {
+		update_crossaxis();
+		return;
+
+	} else {
+		ret = read(Register::XX_CROSS_L, (uint8_t *)&crossxbuf, sizeof(crossxbuf));
+
+		if (OK != ret) {
+			perf_count(_bad_transfer_perf);
+			return;
+		}
+
+		ret = read(Register::YX_CROSS_L, (uint8_t *)&crossybuf, sizeof(crossybuf));
+
+		if (OK != ret) {
+			perf_count(_bad_transfer_perf);
+			return;
+		}
+
+		ret = read(Register::ZX_CROSS_L, (uint8_t *)&crosszbuf, sizeof(crosszbuf));
+
+		if (OK != ret) {
+			perf_count(_bad_transfer_perf);
+			return;
+		}
+
+		if (bOTPDataFlags) {
+			//before
+			OTPcrossaxis[0] = ((int16_t) crossxbuf[1]) << 8 | crossxbuf[0];
+			OTPcrossaxis[1] = ((int16_t) crossxbuf[3]) << 8 | crossxbuf[2];
+			OTPcrossaxis[2] = ((int16_t) crossxbuf[5]) << 8 | crossxbuf[4];
+			OTPcrossaxis[3] = ((int16_t) crossybuf[1]) << 8 | crossybuf[0];
+			OTPcrossaxis[4] = ((int16_t) crossybuf[3]) << 8 | crossybuf[2];
+			OTPcrossaxis[5] = ((int16_t) crossybuf[5]) << 8 | crossybuf[4];
+			OTPcrossaxis[6] = ((int16_t) crosszbuf[1]) << 8 | crosszbuf[0];
+			OTPcrossaxis[7] = ((int16_t) crosszbuf[3]) << 8 | crosszbuf[2];
+			OTPcrossaxis[8] = ((int16_t) crosszbuf[5]) << 8 | crosszbuf[4];
+
+		} else {
+			//after
+			OTPcrossaxis[0] = ((int16_t) crossxbuf[1]) << 8 | crossxbuf[0];
+			OTPcrossaxis[3] = ((int16_t) crossxbuf[3]) << 8 | crossxbuf[2];
+			OTPcrossaxis[6] = ((int16_t) crossxbuf[5]) << 8 | crossxbuf[4];
+			OTPcrossaxis[1] = ((int16_t) crossybuf[1]) << 8 | crossybuf[0];
+			OTPcrossaxis[4] = ((int16_t) crossybuf[3]) << 8 | crossybuf[2];
+			OTPcrossaxis[7] = ((int16_t) crossybuf[5]) << 8 | crossybuf[4];
+			OTPcrossaxis[2] = ((int16_t) crosszbuf[1]) << 8 | crosszbuf[0];
+			OTPcrossaxis[5] = ((int16_t) crosszbuf[3]) << 8 | crosszbuf[2];
+			OTPcrossaxis[8] = ((int16_t) crosszbuf[5]) << 8 | crosszbuf[4];
+		}
+
+		_crossaxis_det = ((int32_t)OTPcrossaxis[0]) * ((int32_t)OTPcrossaxis[4]) * ((int32_t)OTPcrossaxis[8]) +
+				 ((int32_t)OTPcrossaxis[1]) * ((int32_t)OTPcrossaxis[5]) * ((int32_t)OTPcrossaxis[6]) +
+				 ((int32_t)OTPcrossaxis[2]) * ((int32_t)OTPcrossaxis[3]) * ((int32_t)OTPcrossaxis[7]) -
+				 ((int32_t)OTPcrossaxis[0]) * ((int32_t)OTPcrossaxis[5]) * ((int32_t)OTPcrossaxis[7]) -
+				 ((int32_t)OTPcrossaxis[2]) * ((int32_t)OTPcrossaxis[4]) * ((int32_t)OTPcrossaxis[6]) -
+				 ((int32_t)OTPcrossaxis[1]) * ((int32_t)OTPcrossaxis[3]) * ((int32_t)OTPcrossaxis[8]);
+
+		if (_crossaxis_det == 0) {
+			update_crossaxis();
+		}
+
+		inv[0] = (float)OTPcrossaxis[4] * (float)OTPcrossaxis[8] - (float)OTPcrossaxis[5] * (float)OTPcrossaxis[7];
+		inv[1] = (float)OTPcrossaxis[2] * (float)OTPcrossaxis[7] - (float)OTPcrossaxis[1] * (float)OTPcrossaxis[8];
+		inv[2] = (float)OTPcrossaxis[1] * (float)OTPcrossaxis[5] - (float)OTPcrossaxis[2] * (float)OTPcrossaxis[4];
+		inv[3] = (float)OTPcrossaxis[5] * (float)OTPcrossaxis[6] - (float)OTPcrossaxis[3] * (float)OTPcrossaxis[8];
+		inv[4] = (float)OTPcrossaxis[0] * (float)OTPcrossaxis[8] - (float)OTPcrossaxis[2] * (float)OTPcrossaxis[6];
+		inv[5] = (float)OTPcrossaxis[2] * (float)OTPcrossaxis[3] - (float)OTPcrossaxis[0] * (float)OTPcrossaxis[5];
+		inv[6] = (float)OTPcrossaxis[3] * (float)OTPcrossaxis[7] - (float)OTPcrossaxis[4] * (float)OTPcrossaxis[6];
+		inv[7] = (float)OTPcrossaxis[1] * (float)OTPcrossaxis[6] - (float)OTPcrossaxis[0] * (float)OTPcrossaxis[7];
+		inv[8] = (float)OTPcrossaxis[0] * (float)OTPcrossaxis[4] - (float)OTPcrossaxis[1] * (float)OTPcrossaxis[3];
+
+		for (int i = 0; i < 9; i++) {
+			_crossaxis_inv[i] = inv[i] * ((float)IST8310_OTP_SENSITIVITY) / ((float)_crossaxis_det);
+		}
+
+	}
+
+	return;
+}
+
+void
+IST8310::update_crossaxis()
+{
+	*_crossaxis_inv = 1;
+	*(_crossaxis_inv + 1) = 0;
+	*(_crossaxis_inv + 2) = 0;
+	*(_crossaxis_inv + 3) = 0;
+	*(_crossaxis_inv + 4) = 1;
+	*(_crossaxis_inv + 5) = 0;
+	*(_crossaxis_inv + 6) = 0;
+	*(_crossaxis_inv + 7) = 0;
+	*(_crossaxis_inv + 8) = 1;
+	_crossaxis_det = 1;
+}
+
 bool IST8310::RegisterCheck(const register_config_t &reg_cfg)
 {
 	bool success = true;
@@ -281,6 +470,12 @@ uint8_t IST8310::RegisterRead(Register reg)
 	return buffer;
 }
 
+int IST8310::read(Register reg, void *data, unsigned count)
+{
+	const uint8_t cmd = static_cast<uint8_t>(reg);
+	return transfer(&cmd, 1, (uint8_t *)data, count);
+}
+
 void IST8310::RegisterWrite(Register reg, uint8_t value)
 {
 	uint8_t buffer[2] { (uint8_t)reg, value };
diff --git a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp
index a251f49cf0..8e07b48bb0 100644
--- a/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp
+++ b/src/drivers/magnetometer/isentek/ist8310/IST8310.hpp
@@ -83,6 +83,27 @@ private:
 	void RegisterWrite(Register reg, uint8_t value);
 	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
 
+	int read(Register reg, void *data, unsigned count);
+
+
+	/**
+	* Pull cross axis compensation from device and calculate compensation matrix.
+	*
+	*/
+	void        cross_axis_comp();
+
+	/**
+	* Update cross axis matrix to identity
+	*
+	*/
+	void	    update_crossaxis();
+
+	/**
+	* Print cross axis matrix
+	*
+	*/
+	void	    print_cross_axis_info();
+
 	PX4Magnetometer _px4_mag;
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
@@ -110,4 +131,9 @@ private:
 		{ Register::AVGCNTL,      AVGCNTL_BIT::Y_16TIMES_SET | AVGCNTL_BIT::XZ_16TIMES_SET, AVGCNTL_BIT::Y_16TIMES_CLEAR | AVGCNTL_BIT::XZ_16TIMES_CLEAR },
 		{ Register::PDCNTL,       PDCNTL_BIT::PULSE_NORMAL, 0 },
 	};
+
+	float _crossaxis_inv[9];
+
+	int32_t _crossaxis_det;
+
 };
diff --git a/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp b/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp
index 2a49ee03e9..eae0da7ddd 100644
--- a/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp
+++ b/src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp
@@ -82,6 +82,8 @@ enum class Register : uint8_t {
 	AVGCNTL    = 0x41, // Average Control Register
 	PDCNTL     = 0x42, // Pulse Duration Control Register
 
+	OTP        = 0x99,
+
 	XX_CROSS_L = 0x9C, // cross axis xx low byte
 	XX_CROSS_H = 0x9D, // cross axis xx high byte
 	XY_CROSS_L = 0x9E, // cross axis xy low byte
diff --git a/src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt b/src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt
new file mode 100644
index 0000000000..b715c5cec5
--- /dev/null
+++ b/src/drivers/power_monitor/pm_selector_auterion/CMakeLists.txt
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+px4_add_module(
+	MODULE drivers__power_monitor_pm_selector_auterion
+	MAIN pm_selector_auterion
+	SRCS
+		PowerMonitorSelectorAuterion.cpp
+	DEPENDS
+		px4_work_queue
+	)
diff --git a/src/drivers/power_monitor/pm_selector_auterion/Kconfig b/src/drivers/power_monitor/pm_selector_auterion/Kconfig
new file mode 100644
index 0000000000..e9d1c3f16f
--- /dev/null
+++ b/src/drivers/power_monitor/pm_selector_auterion/Kconfig
@@ -0,0 +1,5 @@
+menuconfig DRIVERS_POWER_MONITOR_PM_SELECTOR_AUTERION
+	bool "pm_selector_auterion"
+	default n
+	---help---
+		Enable support for pm_selector_auterion
diff --git a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp
new file mode 100644
index 0000000000..97c89bea61
--- /dev/null
+++ b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp
@@ -0,0 +1,223 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * Automatic handling power monitors
+ */
+
+#include "PowerMonitorSelectorAuterion.h"
+#include "../ina226/ina226.h"
+
+#include <builtin/builtin.h>
+#include <sys/wait.h>
+
+PowerMonitorSelectorAuterion::PowerMonitorSelectorAuterion() :
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
+{
+}
+
+PowerMonitorSelectorAuterion::~PowerMonitorSelectorAuterion() = default;
+
+bool PowerMonitorSelectorAuterion::init()
+{
+	int32_t sens_en = 0;
+	param_get(param_find("SENS_EN_INA226"), &sens_en);
+
+	if (sens_en == 1) {
+
+		sens_en = 0;
+		param_set(param_find("SENS_EN_INA226"), &sens_en);
+		const char *stop_argv[] {"ina226", "stop", NULL};
+		exec_builtin("ina226", (char **)stop_argv, NULL, 0);
+	}
+
+	ScheduleNow();
+	return true;
+}
+
+void PowerMonitorSelectorAuterion::Run()
+{
+	if (should_exit()) {
+		exit_and_cleanup();
+		return;
+	}
+
+	actuator_armed_s actuator_armed{};
+	_actuator_armed_sub.copy(&actuator_armed);
+
+	if (actuator_armed.armed) {
+		exit_and_cleanup();
+		return;
+	}
+
+	for (uint32_t i = 0U; i <  SENSORS_NUMBER; ++i) {
+
+		if (!_sensors[i].started) {
+
+			int ret_val = ina226_probe(i);
+
+			if (ret_val == PX4_OK) {
+
+				float current_shunt_value = 0.0f;
+				param_get(param_find("INA226_SHUNT"), &current_shunt_value);
+
+				if (fabsf(current_shunt_value - _sensors[i].shunt_value) > FLT_EPSILON) {
+					param_set(param_find("INA226_SHUNT"), &(_sensors[i].shunt_value));
+				}
+
+				char bus_number[4] = {0};
+				itoa(_sensors[i].bus_number, bus_number, 10);
+				const char *start_argv[] {
+					_sensors[i].name,
+					"-X", "-b", bus_number, "-a", _sensors[i].i2c_addr,
+					"-t", _sensors[i].id, "-q", "start", NULL
+				};
+
+				int status = PX4_ERROR;
+				int pid =  exec_builtin(_sensors[i].name, (char **)start_argv, NULL, 0);
+
+				if (pid != -1) {
+					waitpid(pid, &status, WUNTRACED);
+				}
+
+				if (status == PX4_OK) {
+					_sensors[i].started = true;
+				}
+			}
+		}
+	}
+
+	ScheduleDelayed(RUN_INTERVAL);
+}
+
+int PowerMonitorSelectorAuterion::ina226_probe(uint32_t instance)
+{
+	struct i2c_master_s *i2c = px4_i2cbus_initialize(_sensors[instance].bus_number);
+	int ret = PX4_ERROR;
+
+	if (i2c != nullptr) {
+
+		struct i2c_msg_s msgv[2];
+
+		uint8_t txdata[1] = {0};
+		uint8_t rxdata[2] = {0};
+
+		msgv[0].frequency = I2C_SPEED_STANDARD;
+		msgv[0].addr = static_cast<uint16_t>(strtol(_sensors[instance].i2c_addr, NULL, 0));
+		msgv[0].flags = 0;
+		msgv[0].buffer = txdata;
+		msgv[0].length = sizeof(txdata);
+
+		msgv[1].frequency = I2C_SPEED_STANDARD;
+		msgv[1].addr = static_cast<uint16_t>(strtol(_sensors[instance].i2c_addr, NULL, 0));
+		msgv[1].flags = I2C_M_READ;
+		msgv[1].buffer = rxdata;
+		msgv[1].length = sizeof(rxdata);
+
+		txdata[0] = {INA226_MFG_ID};
+		ret = I2C_TRANSFER(i2c, msgv, 2);
+		uint16_t value = static_cast<uint16_t>(rxdata[1] | rxdata[0] << 8);
+
+		if (ret != PX4_OK || value != INA226_MFG_ID_TI) {
+
+			ret = PX4_ERROR;
+
+		} else {
+
+			txdata[0] = {INA226_MFG_DIEID};
+			ret = I2C_TRANSFER(i2c, msgv, 2);
+			value = static_cast<uint16_t>(rxdata[1] | rxdata[0] << 8);
+
+			if (ret != PX4_OK || value != INA226_MFG_DIE) {
+				ret = PX4_ERROR;
+			}
+		}
+
+		px4_i2cbus_uninitialize(i2c);
+	}
+
+	return ret;
+}
+
+int PowerMonitorSelectorAuterion::task_spawn(int argc, char *argv[])
+{
+	PowerMonitorSelectorAuterion *instance = new PowerMonitorSelectorAuterion();
+
+	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 PowerMonitorSelectorAuterion::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int PowerMonitorSelectorAuterion::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Driver for starting and auto-detecting different power monitors.
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("pm_selector_auterion", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+extern "C" __EXPORT int pm_selector_auterion_main(int argc, char *argv[])
+{
+	return PowerMonitorSelectorAuterion::main(argc, argv);
+}
diff --git a/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h
new file mode 100644
index 0000000000..2bc9bffbe3
--- /dev/null
+++ b/src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.h
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ *   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 <stdarg.h>
+#include <drivers/drv_hrt.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/Subscription.hpp>
+#include <uORB/topics/actuator_armed.h>
+
+using namespace time_literals;
+
+class PowerMonitorSelectorAuterion : public ModuleBase<PowerMonitorSelectorAuterion>, public px4::ScheduledWorkItem
+{
+
+public:
+	PowerMonitorSelectorAuterion();
+	virtual ~PowerMonitorSelectorAuterion();
+
+	/** @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);
+
+private:
+
+	void Run() override;
+
+	bool init();
+
+	int ina226_probe(uint32_t instance);
+
+	uORB::Subscription	_actuator_armed_sub{ORB_ID(actuator_armed)};		///< system armed control topic
+
+	struct Sensor {
+		const char *name;
+		const char *i2c_addr;
+		const uint8_t bus_number;
+		float shunt_value;
+		bool started;
+		const char *id;
+	};
+
+	static constexpr int RUN_INTERVAL{500_ms};
+	static constexpr int SENSORS_NUMBER{4};
+
+	Sensor _sensors[SENSORS_NUMBER] = {
+		{
+			.name = "ina226",
+			.i2c_addr = "0x41",
+			.bus_number = 1,
+			.shunt_value = 0.0008f,
+			.started = false,
+			.id = "1"
+		},
+		{
+			.name = "ina226",
+			.i2c_addr = "0x40",
+			.bus_number = 1,
+			.shunt_value = 0.0005f,
+			.started = false,
+			.id = "1"
+		},
+		{
+			.name = "ina226",
+			.i2c_addr = "0x41",
+			.bus_number = 2,
+			.shunt_value = 0.0008f,
+			.started = false,
+			.id = "2"
+		},
+		{
+			.name = "ina226",
+			.i2c_addr = "0x40",
+			.bus_number = 2,
+			.shunt_value = 0.0005f,
+			.started = false,
+			.id = "2"
+		}
+	};
+};
diff --git a/src/drivers/pps_capture/Kconfig b/src/drivers/pps_capture/Kconfig
index 9a41a5eae3..e948467fa8 100644
--- a/src/drivers/pps_capture/Kconfig
+++ b/src/drivers/pps_capture/Kconfig
@@ -2,4 +2,4 @@ menuconfig DRIVERS_PPS_CAPTURE
 	bool "pps_capture"
 	default n
 	---help---
-		Enable support for pps_capture
\ No newline at end of file
+		Enable support for pps_capture
diff --git a/src/drivers/pps_capture/PPSCapture.cpp b/src/drivers/pps_capture/PPSCapture.cpp
index aeb8eb0e8d..9d2a3fa1bd 100644
--- a/src/drivers/pps_capture/PPSCapture.cpp
+++ b/src/drivers/pps_capture/PPSCapture.cpp
@@ -42,6 +42,8 @@
 #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>
 
 PPSCapture::PPSCapture() :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
@@ -130,6 +132,7 @@ void PPSCapture::Run()
 
 	pps_capture_s pps_capture;
 	pps_capture.timestamp = _hrt_timestamp;
+	pps_capture.pps_rate_exceeded_counter = _pps_rate_exceeded_counter;
 	// GPS UTC time when the GPIO interrupt was triggered
 	// Last UTC time received from the GPS + elapsed time to the PPS interrupt
 	uint64_t gps_utc_time = _last_gps_utc_timestamp + (_hrt_timestamp - _last_gps_timestamp);
@@ -141,13 +144,32 @@ void PPSCapture::Run()
 	pps_capture.rtc_timestamp = gps_utc_time - (gps_utc_time % USEC_PER_SEC) + USEC_PER_SEC;
 
 	_pps_capture_pub.publish(pps_capture);
+
+	if (_pps_rate_failure.load()) {
+		mavlink_log_warning(&_mavlink_log_pub, "Hardware fault: GPS PPS disabled\t");
+		events::send(events::ID("pps_capture_pps_rate_exceeded"),
+			     events::Log::Error, "Hardware fault: GPS PPS disabled");
+		_pps_rate_failure.store(false);
+	}
 }
 
 int PPSCapture::gpio_interrupt_callback(int irq, void *context, void *arg)
 {
 	PPSCapture *instance = static_cast<PPSCapture *>(arg);
 
-	instance->_hrt_timestamp = hrt_absolute_time();
+	hrt_abstime interrupt_time = hrt_absolute_time();
+
+	if ((interrupt_time - instance->_hrt_timestamp) < 50_ms) {
+		++(instance->_pps_rate_exceeded_counter);
+
+		if (instance->_pps_rate_exceeded_counter >= 10) {
+			// Trigger rate too high, stop future interrupts
+			px4_arch_gpiosetevent(instance->_pps_capture_gpio, false, false, false, nullptr, nullptr);
+			instance->_pps_rate_failure.store(true);
+		}
+	}
+
+	instance->_hrt_timestamp = interrupt_time;
 	instance->ScheduleNow(); // schedule work queue to publish PPS captured time
 
 	return PX4_OK;
diff --git a/src/drivers/pps_capture/PPSCapture.hpp b/src/drivers/pps_capture/PPSCapture.hpp
index cb29c842da..48a0774495 100644
--- a/src/drivers/pps_capture/PPSCapture.hpp
+++ b/src/drivers/pps_capture/PPSCapture.hpp
@@ -73,10 +73,12 @@ private:
 	uint32_t _pps_capture_gpio{0};
 	uORB::Publication<pps_capture_s>	_pps_capture_pub{ORB_ID(pps_capture)};
 	uORB::Subscription								_sensor_gps_sub{ORB_ID(sensor_gps)};
+	orb_advert_t											_mavlink_log_pub{nullptr};
 
 	hrt_abstime _hrt_timestamp{0};
 
 	hrt_abstime	_last_gps_timestamp{0};
 	uint64_t		_last_gps_utc_timestamp{0};
-
+	uint8_t			_pps_rate_exceeded_counter{0};
+	px4::atomic<bool>			_pps_rate_failure{false};
 };
diff --git a/src/drivers/pwm_out/PWMOut.cpp b/src/drivers/pwm_out/PWMOut.cpp
index f32f579597..27d1a9ed70 100644
--- a/src/drivers/pwm_out/PWMOut.cpp
+++ b/src/drivers/pwm_out/PWMOut.cpp
@@ -613,6 +613,14 @@ void PWMOut::update_params()
 						int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
 						param_set(param_find(str), &pwm_fail_new);
 					}
+
+				} else {
+					if (pwm_default_channel_mask & 1 << i) {
+						_mixing_output.failsafeValue(i) = PWM_MOTOR_OFF;
+
+					} else {
+						_mixing_output.failsafeValue(i) = PWM_SERVO_STOP;
+					}
 				}
 
 			} else {
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 82a4979093..58e6b19f00 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -590,10 +590,10 @@ void PX4IO::Run()
 
 				/* publish ACK */
 				if (dsm_ret == OK) {
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 				} else {
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_FAILED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED);
 				}
 			}
 		}
@@ -803,6 +803,14 @@ void PX4IO::update_params()
 						int32_t pwm_fail_new = _mixing_output.failsafeValue(i);
 						param_set(param_find(str), &pwm_fail_new);
 					}
+
+				} else {
+					if (pwm_default_channel_mask & 1 << i) {
+						_mixing_output.failsafeValue(i) = PWM_MOTOR_OFF;
+
+					} else {
+						_mixing_output.failsafeValue(i) = PWM_SERVO_STOP;
+					}
 				}
 			}
 		}
@@ -1155,7 +1163,7 @@ int PX4IO::io_get_status()
 
 		uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
 
-		for (unsigned i = 0; i < raw_inputs; i++) {
+		for (unsigned i = 0; (i < raw_inputs) && (i < _max_rc_input); i++) {
 			status.raw_inputs[i] = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i);
 		}
 
diff --git a/src/drivers/rc_input/RCInput.cpp b/src/drivers/rc_input/RCInput.cpp
index c21ace77d5..4d1d023896 100644
--- a/src/drivers/rc_input/RCInput.cpp
+++ b/src/drivers/rc_input/RCInput.cpp
@@ -334,7 +334,7 @@ void RCInput::Run()
 			// Check for a pairing command
 			if (vcmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) {
 
-				uint8_t cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+				uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 #if defined(SPEKTRUM_POWER)
 
 				if (!_rc_scan_locked && !_armed) {
@@ -356,11 +356,11 @@ void RCInput::Run()
 
 						bind_spektrum(dsm_bind_pulses);
 
-						cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+						cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 					}
 
 				} else {
-					cmd_ret = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 				}
 
 #endif // SPEKTRUM_POWER
diff --git a/src/drivers/smart_battery/batmon/batmon.cpp b/src/drivers/smart_battery/batmon/batmon.cpp
index e3e54db2f2..997477428f 100644
--- a/src/drivers/smart_battery/batmon/batmon.cpp
+++ b/src/drivers/smart_battery/batmon/batmon.cpp
@@ -192,7 +192,7 @@ void Batmon::RunImpl()
 	if (ret == PX4_OK) {
 		new_report.capacity = _batt_capacity;
 		new_report.cycle_count = _cycle_count;
-		new_report.serial_number = _serial_number;
+		sprintf(new_report.serial_number, "%d", _serial_number);
 		new_report.max_cell_voltage_delta = _max_cell_voltage_delta;
 		new_report.cell_count = _cell_count;
 		new_report.state_of_health = _state_of_health;
diff --git a/src/drivers/uavcan/rgbled.cpp b/src/drivers/uavcan/rgbled.cpp
index 645ad4b792..5c4ee8dc21 100644
--- a/src/drivers/uavcan/rgbled.cpp
+++ b/src/drivers/uavcan/rgbled.cpp
@@ -89,27 +89,27 @@ void UavcanRGBController::periodic_update(const uavcan::TimerEvent &)
 
 		// FALLTHROUGH
 		case led_control_s::COLOR_YELLOW:
-			cmd.color.red = brightness >> 3;
-			cmd.color.green = brightness >> 2;
+			cmd.color.red = (brightness / 2) >> 3;
+			cmd.color.green = (brightness / 2) >> 2;
 			cmd.color.blue = 0;
 			break;
 
 		case led_control_s::COLOR_PURPLE:
-			cmd.color.red = brightness >> 3;
+			cmd.color.red = (brightness / 2) >> 3;
 			cmd.color.green = 0;
-			cmd.color.blue = brightness >> 3;
+			cmd.color.blue = (brightness / 2) >> 3;
 			break;
 
 		case led_control_s::COLOR_CYAN:
 			cmd.color.red = 0;
-			cmd.color.green = brightness >> 2;
-			cmd.color.blue = brightness >> 3;
+			cmd.color.green = (brightness / 2) >> 2;
+			cmd.color.blue = (brightness / 2) >> 3;
 			break;
 
 		case led_control_s::COLOR_WHITE:
-			cmd.color.red = brightness >> 3;
-			cmd.color.green = brightness >> 2;
-			cmd.color.blue = brightness >> 3;
+			cmd.color.red = (brightness / 3) >> 3;
+			cmd.color.green = (brightness / 3) >> 2;
+			cmd.color.blue = (brightness / 3) >> 3;
 			break;
 
 		default: // led_control_s::COLOR_OFF
diff --git a/src/drivers/uavcan/sensors/battery.cpp b/src/drivers/uavcan/sensors/battery.cpp
index 7afbc63151..d9344d2f48 100644
--- a/src/drivers/uavcan/sensors/battery.cpp
+++ b/src/drivers/uavcan/sensors/battery.cpp
@@ -41,10 +41,7 @@ const char *const UavcanBatteryBridge::NAME = "battery";
 UavcanBatteryBridge::UavcanBatteryBridge(uavcan::INode &node) :
 	UavcanSensorBridgeBase("uavcan_battery", ORB_ID(battery_status)),
 	ModuleParams(nullptr),
-	_sub_battery(node),
-	_sub_battery_aux(node),
-	_warning(battery_status_s::BATTERY_WARNING_NONE),
-	_last_timestamp(0)
+	_sub_battery(node)
 {
 }
 
@@ -57,149 +54,22 @@ int UavcanBatteryBridge::init()
 		return res;
 	}
 
-	res = _sub_battery_aux.start(BatteryInfoAuxCbBinder(this, &UavcanBatteryBridge::battery_aux_sub_cb));
-
-	if (res < 0) {
-		PX4_ERR("failed to start uavcan sub: %d", res);
-		return res;
-	}
-
 	return 0;
 }
 
 void
 UavcanBatteryBridge::battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg)
 {
-	uint8_t instance = 0;
+	_battery.setConnected(true);
+	_battery.updateVoltage(msg.voltage);
+	_battery.updateCurrent(msg.current);
+	_battery.updateBatteryStatus(hrt_absolute_time());
 
-	for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
-		if (battery_status[instance].id == msg.getSrcNodeID().get() || battery_status[instance].id == 0) {
-			break;
-		}
-	}
+	/* Override data that is expected to arrive from UAVCAN msg*/
+	battery_status_s battery_status = _battery.getBatteryStatus();
+	battery_status.temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // Kelvin to Celcius
+	itoa(msg.model_instance_id, battery_status.serial_number, 10);
+	battery_status.id = msg.getSrcNodeID().get(); // overwrite zeroed index from _battery
 
-	if (instance >= battery_status_s::MAX_INSTANCES) {
-		return;
-	}
-
-	battery_status[instance].timestamp = hrt_absolute_time();
-	battery_status[instance].voltage_v = msg.voltage;
-	battery_status[instance].voltage_filtered_v = msg.voltage;
-	battery_status[instance].current_a = msg.current;
-	battery_status[instance].current_filtered_a = msg.current;
-	battery_status[instance].current_average_a = msg.current;
-
-	if (battery_aux_support[instance] == false) {
-		sumDischarged(battery_status[instance].timestamp, battery_status[instance].current_a);
-		battery_status[instance].discharged_mah = _discharged_mah;
-	}
-
-	battery_status[instance].remaining = msg.state_of_charge_pct / 100.0f; // between 0 and 1
-	// battery_status[instance].scale = msg.; // Power scaling factor, >= 1, or -1 if unknown
-	battery_status[instance].temperature = msg.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // Kelvin to Celcius
-	// battery_status[instance].cell_count = msg.;
-	battery_status[instance].connected = true;
-	battery_status[instance].source = msg.status_flags & uavcan::equipment::power::BatteryInfo::STATUS_FLAG_IN_USE;
-	// battery_status[instance].priority = msg.;
-	battery_status[instance].capacity = msg.full_charge_capacity_wh;
-	battery_status[instance].full_charge_capacity_wh = msg.full_charge_capacity_wh;
-	battery_status[instance].remaining_capacity_wh = msg.remaining_capacity_wh;
-	// battery_status[instance].cycle_count = msg.;
-	battery_status[instance].time_remaining_s = NAN;
-	// battery_status[instance].average_time_to_empty = msg.;
-	battery_status[instance].serial_number = msg.model_instance_id;
-	battery_status[instance].id = msg.getSrcNodeID().get();
-
-	if (battery_aux_support[instance] == false) {
-		// Mavlink 2 needs individual cell voltages or cell[0] if cell voltages are not available.
-		battery_status[instance].voltage_cell_v[0] = msg.voltage;
-
-		// Set cell count to 1 so the the battery code in mavlink_messages.cpp copies the values correctly (hack?)
-		battery_status[instance].cell_count = 1;
-	}
-
-	// battery_status[instance].max_cell_voltage_delta = msg.;
-
-	// battery_status[instance].is_powering_off = msg.;
-
-	determineWarning(battery_status[instance].remaining);
-	battery_status[instance].warning = _warning;
-
-	if (battery_aux_support[instance] == false) {
-		publish(msg.getSrcNodeID().get(), &battery_status[instance]);
-	}
-}
-
-void
-UavcanBatteryBridge::battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux>
-					&msg)
-{
-	uint8_t instance = 0;
-
-	for (instance = 0; instance < battery_status_s::MAX_INSTANCES; instance++) {
-		if (battery_status[instance].id == msg.getSrcNodeID().get()) {
-			break;
-		}
-	}
-
-	if (instance >= battery_status_s::MAX_INSTANCES) {
-		return;
-	}
-
-	battery_aux_support[instance] = true;
-
-	battery_status[instance].discharged_mah = (battery_status[instance].full_charge_capacity_wh -
-			battery_status[instance].remaining_capacity_wh) / msg.nominal_voltage *
-			1000;
-	battery_status[instance].cell_count = math::min((uint8_t)msg.voltage_cell.size(), (uint8_t)14);
-	battery_status[instance].cycle_count = msg.cycle_count;
-	battery_status[instance].over_discharge_count = msg.over_discharge_count;
-	battery_status[instance].nominal_voltage = msg.nominal_voltage;
-	battery_status[instance].time_remaining_s = math::isZero(battery_status[instance].current_a) ? 0 :
-			(battery_status[instance].remaining_capacity_wh /
-			 battery_status[instance].nominal_voltage / battery_status[instance].current_a * 3600);
-	battery_status[instance].is_powering_off = msg.is_powering_off;
-
-	for (uint8_t i = 0; i < battery_status[instance].cell_count; i++) {
-		battery_status[instance].voltage_cell_v[i] = msg.voltage_cell[i];
-	}
-
-	publish(msg.getSrcNodeID().get(), &battery_status[instance]);
-}
-
-void
-UavcanBatteryBridge::sumDischarged(hrt_abstime timestamp, float current_a)
-{
-	// Not a valid measurement
-	if (current_a < 0.f) {
-		// Because the measurement was invalid we need to stop integration
-		// and re-initialize with the next valid measurement
-		_last_timestamp = 0;
-		return;
-	}
-
-	// Ignore first update because we don't know dt.
-	if (_last_timestamp != 0) {
-		const float dt = (timestamp - _last_timestamp) / 1e6;
-		// mAh since last loop: (current[A] * 1000 = [mA]) * (dt[s] / 3600 = [h])
-		_discharged_mah_loop = (current_a * 1e3f) * (dt / 3600.f);
-		_discharged_mah += _discharged_mah_loop;
-	}
-
-	_last_timestamp = timestamp;
-}
-
-void
-UavcanBatteryBridge::determineWarning(float remaining)
-{
-	// propagate warning state only if the state is higher, otherwise remain in current warning state
-	if (remaining < _param_bat_emergen_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
-		_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
-
-	} else if (remaining < _param_bat_crit_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_CRITICAL)) {
-		_warning = battery_status_s::BATTERY_WARNING_CRITICAL;
-
-	} else if (remaining < _param_bat_low_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_LOW)) {
-		_warning = battery_status_s::BATTERY_WARNING_LOW;
-	}
+	publish(msg.getSrcNodeID().get(), &battery_status);
 }
diff --git a/src/drivers/uavcan/sensors/battery.hpp b/src/drivers/uavcan/sensors/battery.hpp
index d041db7d10..e55d431e42 100644
--- a/src/drivers/uavcan/sensors/battery.hpp
+++ b/src/drivers/uavcan/sensors/battery.hpp
@@ -38,11 +38,13 @@
 #pragma once
 
 #include "sensor_bridge.hpp"
-#include <uORB/topics/battery_status.h>
 #include <uavcan/equipment/power/BatteryInfo.hpp>
-#include <ardupilot/equipment/power/BatteryInfoAux.hpp>
+#include <battery/battery.h>
 #include <drivers/drv_hrt.h>
 #include <px4_platform_common/module_params.h>
+#include <uORB/topics/battery_status.h>
+
+using namespace time_literals;
 
 class UavcanBatteryBridge : public UavcanSensorBridgeBase, public ModuleParams
 {
@@ -58,32 +60,15 @@ public:
 private:
 
 	void battery_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &msg);
-	void battery_aux_sub_cb(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux> &msg);
-	void sumDischarged(hrt_abstime timestamp, float current_a);
-	void determineWarning(float remaining);
 
 	typedef uavcan::MethodBinder < UavcanBatteryBridge *,
 		void (UavcanBatteryBridge::*)
 		(const uavcan::ReceivedDataStructure<uavcan::equipment::power::BatteryInfo> &) >
 		BatteryInfoCbBinder;
-	typedef uavcan::MethodBinder < UavcanBatteryBridge *,
-		void (UavcanBatteryBridge::*)
-		(const uavcan::ReceivedDataStructure<ardupilot::equipment::power::BatteryInfoAux> &) >
-		BatteryInfoAuxCbBinder;
+
+	static constexpr int BATTERY_INDEX = 1;
+	static constexpr int SAMPLE_INTERVAL_US = 20_ms; // assume higher frequency UAVCAN feedback than 50Hz
+	Battery _battery{BATTERY_INDEX, this, SAMPLE_INTERVAL_US, battery_status_s::BATTERY_SOURCE_EXTERNAL};
 
 	uavcan::Subscriber<uavcan::equipment::power::BatteryInfo, BatteryInfoCbBinder> _sub_battery;
-	uavcan::Subscriber<ardupilot::equipment::power::BatteryInfoAux, BatteryInfoAuxCbBinder> _sub_battery_aux;
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
-		(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
-		(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr
-	)
-
-	float _discharged_mah = 0.f;
-	float _discharged_mah_loop = 0.f;
-	uint8_t _warning;
-	hrt_abstime _last_timestamp;
-	battery_status_s battery_status[battery_status_s::MAX_INSTANCES] {};
-	bool battery_aux_support[battery_status_s::MAX_INSTANCES] {false};
 };
diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp
index cc87e8e070..9760b54a8c 100644
--- a/src/drivers/uavcan/uavcan_main.cpp
+++ b/src/drivers/uavcan/uavcan_main.cpp
@@ -866,7 +866,7 @@ UavcanNode::Run()
 		vehicle_command_s cmd{};
 		_vcmd_sub.copy(&cmd);
 
-		uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+		uint8_t cmd_ack_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 		if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_STORAGE) {
 			acknowledge = true;
@@ -935,7 +935,8 @@ UavcanNode::enable_idle_throttle_when_armed(bool value)
 
 	if (!_mixing_interface_esc.mixingOutput().useDynamicMixing()) {
 		if (value != _idle_throttle_when_armed) {
-			_mixing_interface_esc.mixingOutput().setAllMinValues(value ? 1 : 0);
+			_mixing_interface_esc.mixingOutput().setAllMinValues(
+				value ? _idle_throttle_when_armed_param : 0);
 			_idle_throttle_when_armed = value;
 		}
 	}
@@ -1200,7 +1201,8 @@ UavcanNode::param_count(uavcan::NodeID node_id)
 	req.index = 0;
 	int call_res = _param_getset_client.call(node_id, req);
 
-	if (call_res < 0) {
+	// -ErrInvalidParam is returned when no UAVCAN device is connected to the CAN bus
+	if ((call_res < 0) && (-uavcan::ErrInvalidParam != call_res)) {
 		PX4_ERR("couldn't start parameter count: %d", call_res);
 
 	} else {
diff --git a/src/drivers/uavcan/uavcan_module.hpp b/src/drivers/uavcan/uavcan_module.hpp
index 9941248dbf..3ce8c9b6b2 100644
--- a/src/drivers/uavcan/uavcan_module.hpp
+++ b/src/drivers/uavcan/uavcan_module.hpp
@@ -50,6 +50,7 @@
 // firmware paths
 #define UAVCAN_MAX_PATH_LENGTH (128 + 40)
 #define UAVCAN_SD_ROOT_PATH    "/fs/microsd/"
+#define UAVCAN_SD_ALT_PATH     "/fs/microsd/ext_autostart/uavcan/"
 #define UAVCAN_FIRMWARE_PATH   UAVCAN_SD_ROOT_PATH"ufw"
 #define UAVCAN_ROMFS_FW_PATH   "/etc/uavcan/fw"
 #define UAVCAN_ROMFS_FW_PREFIX "_"
diff --git a/src/drivers/uavcan/uavcan_params.c b/src/drivers/uavcan/uavcan_params.c
index 61d6df4816..0d25a4a406 100644
--- a/src/drivers/uavcan/uavcan_params.c
+++ b/src/drivers/uavcan/uavcan_params.c
@@ -78,9 +78,10 @@ PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
 PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);
 
 /**
- * UAVCAN ESC will spin at idle throttle when armed, even if the mixer outputs zero setpoints.
+ * UAVCAN ESC will spin at specified idle throttle when armed, even if the mixer outputs zero setpoints.
  *
- * @boolean
+ * @min 0
+ * @max 2048
  * @reboot_required true
  * @group UAVCAN
  */
diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp
index d6a05dba67..5b0127c34e 100644
--- a/src/drivers/uavcan/uavcan_servers.cpp
+++ b/src/drivers/uavcan/uavcan_servers.cpp
@@ -131,6 +131,14 @@ int UavcanServers::init()
 		return ret;
 	}
 
+	/*
+	Check for firmware in the ext_autostart directory, move it to appropriate location on
+	the SD card, as defined by the APDesc.
+	These firmware files will be overwritten by binaries located in the root directory,
+	which will be copied in the next step.
+	*/
+	migrateFWFromRoot(UAVCAN_FIRMWARE_PATH, UAVCAN_SD_ALT_PATH);
+
 	/*
 	Check for firmware in the root directory, move it to appropriate location on
 	the SD card, as defined by the APDesc.
diff --git a/src/drivers/uavcannode/Publishers/BatteryInfo.hpp b/src/drivers/uavcannode/Publishers/BatteryInfo.hpp
index 9a68744436..cdc3d32329 100644
--- a/src/drivers/uavcannode/Publishers/BatteryInfo.hpp
+++ b/src/drivers/uavcannode/Publishers/BatteryInfo.hpp
@@ -83,7 +83,7 @@ public:
 			battery_info.state_of_charge_pct_stdev = battery.max_error;
 			battery_info.model_instance_id = 0; // TODO: what goes here?
 			battery_info.model_name = "ARK BMS Rev 0.2";
-			battery_info.battery_id = battery.serial_number;
+			battery_info.battery_id = atoi(battery.serial_number);
 			battery_info.hours_to_full_charge = 0; // TODO: Read BQ40Z80_TIME_TO_FULL
 			battery_info.state_of_health_pct = battery.state_of_health;
 
diff --git a/src/examples/ext_update_checker/CMakeLists.txt b/src/examples/ext_update_checker/CMakeLists.txt
new file mode 100644
index 0000000000..c74bbd5b2f
--- /dev/null
+++ b/src/examples/ext_update_checker/CMakeLists.txt
@@ -0,0 +1,39 @@
+############################################################################
+#
+#   Copyright (c) 2020 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 examples__ext_update_checker
+	MAIN update_checker
+	SRCS
+		update_checker.cpp
+	DEPENDS
+	)
diff --git a/src/examples/ext_update_checker/Kconfig b/src/examples/ext_update_checker/Kconfig
new file mode 100644
index 0000000000..2c57956dad
--- /dev/null
+++ b/src/examples/ext_update_checker/Kconfig
@@ -0,0 +1,5 @@
+menuconfig EXAMPLES_EXT_UPDATE_CHECKER
+	bool "ext_update_checker"
+	default n
+	---help---
+		Enable support for ext_update_checker
diff --git a/src/examples/ext_update_checker/update_checker.cpp b/src/examples/ext_update_checker/update_checker.cpp
new file mode 100644
index 0000000000..b5411f2b4a
--- /dev/null
+++ b/src/examples/ext_update_checker/update_checker.cpp
@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 update_checker.cpp
+ * fake update checker application to test external component updates preflight checks.
+ *
+ * @author Claudio Micheli <claudio@auterion.com>
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <poll.h>
+#include <string.h>
+#include <sys/stat.h>
+
+extern "C" __EXPORT int update_checker_main(int argc, char *argv[]);
+
+static constexpr char ext_component_path[] = PX4_STORAGEDIR "/ext_component_updated";
+static constexpr int FLASH_DURATION = 15;
+
+static void usage(const char *reason)
+{
+	if (reason) {
+		fprintf(stderr, "%s\n", reason);
+	}
+
+	fprintf(stderr, "usage: update_checker {fake_success|fake_fail} \n\n");
+}
+
+bool file_exists(const char *filename)
+{
+	struct stat buffer;
+	return stat(filename, &buffer) == 0;
+}
+
+void fake_update(const char flash_result[], uint8_t return_code)
+{
+	FILE *fp = fopen(ext_component_path, "w");
+
+	if (fp == nullptr) {
+		PX4_ERR("Error unable to open file");
+		return;
+	}
+
+	PX4_INFO("Start fake flashing, duration: %d s", FLASH_DURATION);
+	sleep(FLASH_DURATION);
+
+	PX4_INFO("Fake flashing done %s", flash_result);
+	fprintf(fp, "%s %d\n", flash_result, return_code);
+	fclose(fp);
+}
+
+int update_checker_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		usage("missing command");
+		return 1;
+	}
+
+	if (!file_exists(ext_component_path)) {
+
+		if (!strcmp(argv[1], "fake_success")) {
+			fake_update("SUCCESS", 0);
+		}
+
+		if (!strcmp(argv[1], "fake_fail")) {
+			fake_update("FAIL", (rand() % 10) + 1);
+		}
+
+	} else {
+		PX4_INFO("Update file already exists - Nothing to do");
+	}
+
+	PX4_INFO("exiting");
+
+	return 0;
+}
diff --git a/src/include/containers/Bitset.hpp b/src/include/containers/Bitset.hpp
index 8098554b93..aeeeab0cad 100644
--- a/src/include/containers/Bitset.hpp
+++ b/src/include/containers/Bitset.hpp
@@ -34,6 +34,7 @@
 #pragma once
 
 #include <stdint.h>
+#include <stddef.h>
 
 namespace px4
 {
diff --git a/src/include/visibility.h b/src/include/visibility.h
index 5de2682a8c..b56ef0b4d9 100644
--- a/src/include/visibility.h
+++ b/src/include/visibility.h
@@ -60,14 +60,19 @@
 #endif
 
 
+#define system_exit exit
+#define system_clock_gettime clock_gettime
+#define system_clock_settime clock_settime
+#define system_pthread_cond_timedwait pthread_cond_timedwait
+#define system_usleep usleep
+#define system_sleep sleep
 
-
+#ifndef PX4_DISABLE_GCC_POISON
 
 /* exit() is used on NuttX to exit a task. However on Posix, it will exit the
  * whole application, so we prevent its use there. There are cases where it
  * still needs to be used, thus we remap system_exit to exit.
  */
-#define system_exit exit
 #if !defined(__PX4_NUTTX)
 #include <stdlib.h>
 #ifdef __cplusplus
@@ -83,8 +88,6 @@
 /* For SITL lockstep we fake the clock, sleeping, and timedwaits
  * Therefore, we prefix these syscalls with system_. */
 #include <time.h>
-#define system_clock_gettime clock_gettime
-#define system_clock_settime clock_settime
 /* We can't poison clock_settime/clock_gettime because they are
  * used in DriverFramework. */
 
@@ -93,20 +96,17 @@
 // We can't include this for NuttX otherwise we get conflicts for read/write
 // symbols in cannode.
 #endif // !defined(__PX4_NUTTX)
-#define system_pthread_cond_timedwait pthread_cond_timedwait
 /* We can't poison pthread_cond_timedwait because it seems to be used in the
  * <string> include. */
 
 
 /* We don't poison usleep and sleep because it is used in dependencies
- * like uavcan and DriverFramework. */
+ * like uavcan. */
 #if !defined(__PX4_NUTTX)
 #include <unistd.h>
 // We can't include this for NuttX otherwise we get conflicts for read/write
 // symbols in cannode.
 #endif // !defined(__PX4_NUTTX)
-#define system_usleep usleep
-#define system_sleep sleep
 
 
 /* On NuttX we call clearenv() so we cannot use getenv() and others (see
@@ -123,3 +123,5 @@
  * need to get changed. */
 #pragma GCC poison getenv setenv putenv
 #endif // defined(__PX4_NUTTX)
+
+#endif // PX4_DISABLE_GCC_POISON
diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt
index 0552b21db7..811c30ff5e 100644
--- a/src/lib/CMakeLists.txt
+++ b/src/lib/CMakeLists.txt
@@ -31,6 +31,7 @@
 #
 ############################################################################
 
+add_subdirectory(adsb)
 add_subdirectory(airspeed)
 add_subdirectory(avoidance)
 add_subdirectory(battery)
@@ -48,7 +49,6 @@ add_subdirectory(field_sensor_bias_estimator)
 add_subdirectory(geo)
 add_subdirectory(hysteresis)
 add_subdirectory(l1)
-add_subdirectory(landing_slope)
 add_subdirectory(led)
 add_subdirectory(matrix)
 add_subdirectory(mathlib)
@@ -66,6 +66,7 @@ add_subdirectory(slew_rate)
 add_subdirectory(systemlib)
 add_subdirectory(system_identification)
 add_subdirectory(tecs)
+add_subdirectory(terrain)
 add_subdirectory(terrain_estimation)
 add_subdirectory(tunes)
 add_subdirectory(version)
diff --git a/src/lib/adsb/CMakeLists.txt b/src/lib/adsb/CMakeLists.txt
new file mode 100644
index 0000000000..2012ef827d
--- /dev/null
+++ b/src/lib/adsb/CMakeLists.txt
@@ -0,0 +1,34 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(adsb adsb.cpp)
diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp b/src/lib/adsb/adsb.cpp
similarity index 80%
rename from src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp
rename to src/lib/adsb/adsb.cpp
index 195472465a..36bffcc179 100644
--- a/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.cpp
+++ b/src/lib/adsb/adsb.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   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
@@ -30,20 +30,3 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
-/**
- * @file FlightTaskAutoFollowMe.cpp
- */
-
-#include "FlightTaskAutoFollowMe.hpp"
-#include <mathlib/mathlib.h>
-
-using namespace matrix;
-
-bool FlightTaskAutoFollowMe::update()
-{
-	_position_setpoint = _target;
-	matrix::Vector2f vel_sp = _getTargetVelocityXY();
-	_velocity_setpoint = matrix::Vector3f(vel_sp(0), vel_sp(1), 0.0f);
-	return true;
-}
diff --git a/src/lib/adsb/parameters.c b/src/lib/adsb/parameters.c
new file mode 100644
index 0000000000..6bf8865cc5
--- /dev/null
+++ b/src/lib/adsb/parameters.c
@@ -0,0 +1,228 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * ADSB-Out squawk code configuration
+ *
+ * This parameter defines the squawk code. Value should be between 0000 and 7777.
+ *
+ * @reboot_required false
+ * @min 0
+ * @max 7777
+ * @group ADSB
+ *
+ */
+PARAM_DEFINE_INT32(ADSB_SQUAWK, 1200);
+
+/**
+ * ADSB-Out Ident Configuration
+ *
+ * Enable Identification of Position feature
+ *
+ * @boolean
+ * @reboot_required false
+ * @group ADSB
+ */
+PARAM_DEFINE_INT32(ADSB_IDENT, 0);
+
+/**
+ * ADSB-In Vehicle List Size
+ *
+ * Change number of targets to track
+ *
+ * @min 0
+ * @max 50
+ * @reboot_required true
+ * @group ADSB
+ */
+PARAM_DEFINE_INT32(ADSB_LIST_MAX, 25);
+
+/**
+ * ADSB-Out ICAO configuration
+ *
+ * Defines the ICAO ID of the vehicle
+ *
+ * @reboot_required true
+ * @min -1
+ * @max 16777215
+ * @group ADSB
+ *
+ */
+PARAM_DEFINE_INT32(ADSB_ICAO_ID, 1194684);
+
+/**
+ * ADSB-Out Vehicle Size Configuration
+ *
+ * Report the length and width of the vehicle in meters. In most cases, use '1' for the smallest vehicle size.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 15
+ * @group ADSB
+ *
+ * @value 0 SizeUnknown
+ * @value 1 Len15_Wid23
+ * @value 2 Len25_Wid28
+ * @value 3 Len25_Wid34
+ * @value 4 Len35_Wid33
+ * @value 5 Len35_Wid38
+ * @value 6 Len45_Wid39
+ * @value 7 Len45_Wid45
+ * @value 8 Len55_Wid45
+ * @value 9 Len55_Wid52
+ * @value 10 Len65_Wid59
+ * @value 11 Len65_Wid67
+ * @value 12 Len75_Wid72
+ * @value 13 Len75_Wid80
+ * @value 14 Len85_Wid80
+ * @value 15 Len85_Wid90
+ */
+PARAM_DEFINE_INT32(ADSB_LEN_WIDTH, 1);
+
+/**
+ * ADSB-Out Vehicle Emitter Type
+ *
+ * Configure the emitter type of the vehicle.
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 15
+ * @group ADSB
+ *
+ * @value 0 Unknown
+ * @value 1 Light
+ * @value 2 Small
+ * @value 3 Large
+ * @value 4 HighVortex
+ * @value 5 Heavy
+ * @value 6 Performance
+ * @value 7 Rotorcraft
+ * @value 8 RESERVED
+ * @value 9 Glider
+ * @value 10 LightAir
+ * @value 11 Parachute
+ * @value 12 UltraLight
+ * @value 13 RESERVED
+ * @value 14 UAV
+ * @value 15 Space
+ * @value 16 RESERVED
+ * @value 17 EmergencySurf
+ * @value 18 ServiceSurf
+ * @value 19 PointObstacle
+ */
+PARAM_DEFINE_INT32(ADSB_EMIT_TYPE, 14);
+
+/**
+ * ADSB-Out Vehicle Max Speed
+ *
+ * Informs ADSB vehicles of this vehicle's max speed capability
+ *
+ * @reboot_required true
+ * @min 0
+ * @max 6
+ * @value 0 UnknownMaxSpeed
+ * @value 1 75Kts
+ * @value 2 150Kts
+ * @value 3 300Kts
+ * @value 4 600Kts
+ * @value 5 1200Kts
+ * @value 6 Over1200Kts
+ * @group ADSB
+ */
+PARAM_DEFINE_INT32(ADSB_MAX_SPEED, 0);
+
+/**
+ * ADSB-In Special ICAO configuration
+ *
+ * This vehicle is always tracked. Use 0 to disable.
+ *
+ * @reboot_required false
+ * @min 0
+ * @max 16777215
+ * @group ADSB
+ *
+ */
+PARAM_DEFINE_INT32(ADSB_ICAO_SPECL, 0);
+
+/**
+ * ADSB-Out Emergency State
+ *
+ * Sets the vehicle emergency state
+ *
+ * @reboot_required false
+ * @min 0
+ * @max 6
+ * @value 0 NoEmergency
+ * @value 1 General
+ * @value 2 Medical
+ * @value 3 LowFuel
+ * @value 4 NoCommunications
+ * @value 5 Interference
+ * @value 6 Downed
+ * @group ADSB
+ */
+PARAM_DEFINE_INT32(ADSB_EMERGC, 0);
+
+/**
+ * ADSB-Out GPS Offset lat
+ *
+ * Sets GPS lataral offset encoding
+ *
+ * @reboot_required false
+ * @min 0
+ * @max 7
+ * @value 0 NoData
+ * @value 1 LatLeft2M
+ * @value 2 LatLeft4M
+ * @value 3 LatLeft6M
+ * @value 4 LatRight0M
+ * @value 5 LatRight2M
+ * @value 6 LatRight4M
+ * @value 7 LatRight6M
+ * @group ADSB
+ */
+PARAM_DEFINE_INT32(ADSB_GPS_OFF_LAT, 0);
+
+/**
+ * ADSB-Out GPS Offset lon
+ *
+ * Sets GPS longitudinal offset encoding
+ *
+ * @reboot_required false
+ * @min 0
+ * @max 1
+ * @value 0 NoData
+ * @value 1 AppliedBySensor
+ * @group ADSB
+ */
+PARAM_DEFINE_INT32(ADSB_GPS_OFF_LON, 0);
diff --git a/src/lib/airspeed/airspeed.cpp b/src/lib/airspeed/airspeed.cpp
index f3ed3506b6..6f4345dc6a 100644
--- a/src/lib/airspeed/airspeed.cpp
+++ b/src/lib/airspeed/airspeed.cpp
@@ -232,3 +232,14 @@ float calc_CAS_from_TAS(float speed_true, float pressure_ambient, float temperat
 {
 	return speed_true * sqrtf(get_air_density(pressure_ambient, temperature_celsius) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
 }
+
+float calc_true_from_calibrated_airspeed(float speed_calibrated, float air_density)
+{
+
+	return speed_calibrated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / air_density);
+}
+
+float calc_calibrated_from_true_airspeed(float speed_true, float air_density)
+{
+	return speed_true * sqrtf(air_density / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+}
diff --git a/src/lib/airspeed/airspeed.h b/src/lib/airspeed/airspeed.h
index 6b53a8710c..80898f67a1 100644
--- a/src/lib/airspeed/airspeed.h
+++ b/src/lib/airspeed/airspeed.h
@@ -141,6 +141,24 @@ __EXPORT float get_air_density(float static_pressure, float temperature_celsius)
 __EXPORT float calc_CAS_from_TAS(float speed_true, float pressure_ambient,
 				 float temperature_celsius);
 
+/**
+ * @brief Calculates true airspeed from calibrated airspeed and air density
+ *
+ * @param speed_calibrated calibrated airspeed [m/s]
+ * @param air_density air density [kg/m3]
+ * @return true airspeed [m/s]
+ */
+__EXPORT float calc_true_from_calibrated_airspeed(float speed_calibrated, float air_density);
+
+/**
+ * @brief Calculates calibrated airspeed from true airspeed and air density
+ *
+ * @param speed_true true airspeed [m/s]
+ * @param air_density air density [kg/m3]
+ * @return calibrated airspeed [m/s]
+ */
+__EXPORT float calc_calibrated_from_true_airspeed(float speed_true, float air_density);
+
 __END_DECLS
 
 #endif
diff --git a/src/lib/avoidance/ObstacleAvoidance.cpp b/src/lib/avoidance/ObstacleAvoidance.cpp
index 067f6fbafd..184b7fc300 100644
--- a/src/lib/avoidance/ObstacleAvoidance.cpp
+++ b/src/lib/avoidance/ObstacleAvoidance.cpp
@@ -67,10 +67,11 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 	const auto &wp_msg = _sub_vehicle_trajectory_waypoint.get();
 	const auto &bezier_msg = _sub_vehicle_trajectory_bezier.get();
 
-	const bool avoidance_data_timeout =
-		hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US &&
-		hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime(bezier_msg.control_points[bezier_msg.bezier_order -
-							1].delta * 1e6f);
+	const bool wp_msg_timeout = hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US;
+	const bool bezier_msg_timeout = hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime(
+						bezier_msg.control_points[bezier_msg.bezier_order -
+											1].delta * 1e6f);
+	const bool avoidance_data_timeout = wp_msg_timeout && bezier_msg_timeout;
 
 	const bool avoidance_point_valid = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid;
 	const bool avoidance_bezier_valid = bezier_msg.bezier_order > 0;
@@ -86,8 +87,12 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 	}
 
 	if (avoidance_invalid) {
-		PX4_WARN("Obstacle Avoidance system failed, loitering");
-		_publishVehicleCmdDoLoiter();
+		if (_avoidance_activated) {
+			// Invalid point received: deactivate
+			PX4_WARN("Obstacle Avoidance system failed, loitering");
+			_publishVehicleCmdDoLoiter();
+			_avoidance_activated = false;
+		}
 
 		if (!PX4_ISFINITE(_failsafe_position(0)) || !PX4_ISFINITE(_failsafe_position(1))
 		    || !PX4_ISFINITE(_failsafe_position(2))) {
@@ -99,13 +104,18 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 		vel_sp.setNaN();
 		yaw_sp = NAN;
 		yaw_speed_sp = NAN;
+
+		// Do nothing further - wait until activation
 		return;
 
-	} else {
+	} else if (!_avoidance_activated) {
+		// First setpoint has been received: activate
+		PX4_INFO("Obstacle Avoidance system activated");
 		_failsafe_position.setNaN();
+		_avoidance_activated = true;
 	}
 
-	if (avoidance_point_valid) {
+	if (avoidance_point_valid && !wp_msg_timeout) {
 		const auto &point0 = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0];
 		pos_sp = Vector3f(point0.position);
 		vel_sp = Vector3f(point0.velocity);
@@ -116,8 +126,7 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
 			yaw_speed_sp = point0.yaw_speed;
 		}
 
-	} else if (avoidance_bezier_valid) {
-
+	} else if (avoidance_bezier_valid && !bezier_msg_timeout) {
 		float yaw = NAN, yaw_speed = NAN;
 		_generateBezierSetpoints(pos_sp, vel_sp, yaw, yaw_speed);
 
diff --git a/src/lib/avoidance/ObstacleAvoidance.hpp b/src/lib/avoidance/ObstacleAvoidance.hpp
index 64ebc0c049..8a27ffbbf1 100644
--- a/src/lib/avoidance/ObstacleAvoidance.hpp
+++ b/src/lib/avoidance/ObstacleAvoidance.hpp
@@ -130,6 +130,8 @@ protected:
 	matrix::Vector3f _position = {}; /**< current vehicle position */
 	matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */
 
+	bool _avoidance_activated{false}; /**< true after the first avoidance setpoint is received */
+
 	systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */
 	systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */
 
diff --git a/src/lib/battery/battery.cpp b/src/lib/battery/battery.cpp
index c01b018dc2..fcf7669dcb 100644
--- a/src/lib/battery/battery.cpp
+++ b/src/lib/battery/battery.cpp
@@ -155,6 +155,7 @@ battery_status_s Battery::getBatteryStatus()
 	battery_status.id = static_cast<uint8_t>(_index);
 	battery_status.warning = _warning;
 	battery_status.timestamp = hrt_absolute_time();
+	battery_status.faults = determineFaults();
 	return battery_status;
 }
 
@@ -215,7 +216,7 @@ void Battery::estimateStateOfCharge(const float voltage_v, const float current_a
 		cell_voltage += throttle * _params.v_load_drop;
 	}
 
-	_state_of_charge_volt_based = math::gradual(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
+	_state_of_charge_volt_based = math::interpolate(cell_voltage, _params.v_empty, _params.v_charged, 0.f, 1.f);
 
 	// choose which quantity we're using for final reporting
 	if (_params.capacity > 0.f && _battery_initialized) {
@@ -251,6 +252,18 @@ uint8_t Battery::determineWarning(float state_of_charge)
 	}
 }
 
+uint16_t Battery::determineFaults()
+{
+	uint16_t faults{0};
+
+	if ((_params.n_cells > 0)
+	    && (_voltage_v > (_params.n_cells * _params.v_charged * 1.05f))) {
+		faults |= (1 << battery_status_s::BATTERY_FAULT_SPIKES);
+	}
+
+	return faults;
+}
+
 void Battery::computeScale()
 {
 	const float voltage_range = (_params.v_charged - _params.v_empty);
diff --git a/src/lib/battery/battery.h b/src/lib/battery/battery.h
index 24470b6a0d..c9b958aa3d 100644
--- a/src/lib/battery/battery.h
+++ b/src/lib/battery/battery.h
@@ -146,6 +146,7 @@ private:
 	void sumDischarged(const hrt_abstime &timestamp, float current_a);
 	void estimateStateOfCharge(const float voltage_v, const float current_a);
 	uint8_t determineWarning(float state_of_charge);
+	uint16_t determineFaults();
 	void computeScale();
 	float computeRemainingTime(float current_a);
 
diff --git a/src/lib/cdev/posix/cdev_platform.cpp b/src/lib/cdev/posix/cdev_platform.cpp
index 16a956b673..0a977a1bba 100644
--- a/src/lib/cdev/posix/cdev_platform.cpp
+++ b/src/lib/cdev/posix/cdev_platform.cpp
@@ -40,6 +40,8 @@
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/time.h>
 
+#include <stdlib.h>
+
 const cdev::px4_file_operations_t cdev::CDev::fops = {};
 
 pthread_mutex_t devmutex = PTHREAD_MUTEX_INITIALIZER;
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
index 80a9910d8b..22420d891d 100644
--- a/src/lib/conversion/rotation.h
+++ b/src/lib/conversion/rotation.h
@@ -92,6 +92,9 @@ enum Rotation : uint8_t {
 	ROTATION_PITCH_315                = 39,
 	ROTATION_ROLL_90_PITCH_315        = 40,
 
+	ROTATION_ROLL_270_YAW_225    = 41,
+	ROTATION_ROLL_270_YAW_315    = 42,
+	ROTATION_PITCH_90_YAW_45     = 43,
 	ROTATION_MAX
 };
 
@@ -143,6 +146,9 @@ static constexpr rot_lookup_t rot_lookup[ROTATION_MAX] = {
 	{ 90,  68, 293 },
 	{  0, 315,   0 },
 	{ 90, 315,   0 },
+	{270,   0, 225 },
+	{270,   0, 315 },
+	{  0,  90,  45 },
 };
 
 /**
diff --git a/src/lib/drivers/smbus_sbs/SBS.hpp b/src/lib/drivers/smbus_sbs/SBS.hpp
index de636d4cf5..dcff9c9180 100644
--- a/src/lib/drivers/smbus_sbs/SBS.hpp
+++ b/src/lib/drivers/smbus_sbs/SBS.hpp
@@ -288,7 +288,7 @@ int SMBUS_SBS_BaseClass<T>::populate_smbus_data(battery_status_s &data)
 
 	// Read serial number.
 	ret |= _interface->read_word(BATT_SMBUS_SERIAL_NUMBER, result);
-	data.serial_number = result;
+	sprintf(data.serial_number, "%d", result);
 
 	// Read battery temperature and covert to Celsius.
 	ret |= _interface->read_word(BATT_SMBUS_TEMP, result);
diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json
index 95efd34331..5793e3101c 100644
--- a/src/lib/events/enums.json
+++ b/src/lib/events/enums.json
@@ -243,6 +243,10 @@
                             "name": "auto_vtol_takeoff",
                             "description": "Vtol Takeoff"
                         },
+                        "16": {
+                            "name": "init",
+                            "description": "Initialization"
+                        },
                         "255": {
                             "name": "unknown",
                             "description": "[Unknown]"
diff --git a/src/lib/l1/ECL_L1_Pos_Controller.cpp b/src/lib/l1/ECL_L1_Pos_Controller.cpp
index 88fb6378a5..f21679587f 100644
--- a/src/lib/l1/ECL_L1_Pos_Controller.cpp
+++ b/src/lib/l1/ECL_L1_Pos_Controller.cpp
@@ -213,10 +213,12 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
 
 void
 ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f &vector_curr_position, float radius,
-				       int8_t loiter_direction, const Vector2f &ground_speed_vector)
+				       const bool loiter_direction_counter_clockwise, const Vector2f &ground_speed_vector)
 {
 	_has_guidance_updated = true;
 
+	const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
+
 	/* the complete guidance logic in this section was proposed by [2] */
 
 	/* calculate the gains for the PD loop (circle tracking) */
@@ -274,7 +276,7 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
 	float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);
 
 	/* calculate velocity on circle / along tangent */
-	float tangent_vel = xtrack_vel_center * loiter_direction;
+	float tangent_vel = xtrack_vel_center * loiter_direction_multiplier;
 
 	/* prevent PD output from turning the wrong way when in circle mode */
 	const float l1_op_tan_vel = 2.f; // hard coded max tangential velocity in the opposite direction
@@ -288,7 +290,8 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
 			(radius + xtrack_err_circle));
 
 	/* add PD control on circle and centripetal acceleration for total circle command */
-	float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal);
+	float lateral_accel_sp_circle = loiter_direction_multiplier * (lateral_accel_sp_circle_pd +
+					lateral_accel_sp_circle_centripetal);
 
 	/*
 	 * Switch between circle (loiter) and capture (towards waypoint center) mode when
@@ -296,8 +299,10 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
 	 */
 
 	// XXX check switch over
-	if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) ||
-	    (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
+	if ((lateral_accel_sp_center < lateral_accel_sp_circle && !loiter_direction_counter_clockwise
+	     && xtrack_err_circle > 0.0f)
+	    ||
+	    (lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction_counter_clockwise && xtrack_err_circle > 0.0f)) {
 		_lateral_accel = lateral_accel_sp_center;
 		_circle_mode = false;
 		/* angle between requested and current velocity vector */
@@ -309,8 +314,9 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
 		_lateral_accel = lateral_accel_sp_circle;
 		_circle_mode = true;
 		_bearing_error = 0.0f;
-		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
+		// once loitering, the bearing is tangentially to the loiter,
+		// so subtract PI/2 from the bearing from current position to loiter center
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0)) - M_PI_2_F;
 	}
 
 	update_roll_setpoint();
diff --git a/src/lib/l1/ECL_L1_Pos_Controller.hpp b/src/lib/l1/ECL_L1_Pos_Controller.hpp
index 8190e3e533..e6e7464859 100644
--- a/src/lib/l1/ECL_L1_Pos_Controller.hpp
+++ b/src/lib/l1/ECL_L1_Pos_Controller.hpp
@@ -154,7 +154,7 @@ public:
 	 * @return sets _lateral_accel setpoint
 	 */
 	void navigate_loiter(const matrix::Vector2f &vector_A, const matrix::Vector2f &vector_curr_position, float radius,
-			     int8_t loiter_direction, const matrix::Vector2f &ground_speed_vector);
+			     const bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_speed_vector);
 
 	/**
 	 * Navigate on a fixed bearing.
diff --git a/src/lib/landing_slope/Landingslope.cpp b/src/lib/landing_slope/Landingslope.cpp
deleted file mode 100644
index 61aeb2ac53..0000000000
--- a/src/lib/landing_slope/Landingslope.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2017 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 landingslope.cpp
- *
- * @author Thomas Gubler <thomasgubler@gmail.com>
- */
-
-#include "Landingslope.hpp"
-
-#include <mathlib/mathlib.h>
-#include <matrix/math.hpp>
-
-void
-Landingslope::update(float landing_slope_angle_rad_new,
-		     float flare_relative_alt_new,
-		     float motor_lim_relative_alt_new,
-		     float H1_virt_new)
-{
-	_landing_slope_angle_rad = landing_slope_angle_rad_new;
-	_flare_relative_alt = flare_relative_alt_new;
-	_motor_lim_relative_alt = motor_lim_relative_alt_new;
-	_H1_virt = H1_virt_new;
-
-	calculateSlopeValues();
-}
-
-void
-Landingslope::calculateSlopeValues()
-{
-	_H0 =  _flare_relative_alt + _H1_virt;
-	_d1 = _flare_relative_alt / tanf(_landing_slope_angle_rad);
-	_flare_constant = (_H0 * _d1) / _flare_relative_alt;
-	_flare_length = -logf(_H1_virt / _H0) * _flare_constant;
-	_horizontal_slope_displacement = (_flare_length - _d1);
-}
-
-float
-Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance)
-{
-	return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement,
-			_landing_slope_angle_rad);
-}
-
-float
-Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
-		float bearing_airplane_currwp)
-{
-	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
-	if (fabsf(matrix::wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) < math::radians(90.0f)) {
-		return getLandingSlopeRelativeAltitude(wp_landing_distance);
-
-	}
-
-	return 0.0f;
-}
-
-float
-Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
-		float bearing_airplane_currwp)
-{
-	/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
-	if (fabsf(matrix::wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) < math::radians(90.0f)) {
-		return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance) / _flare_constant) - _H1_virt;
-
-	}
-
-	return 0.0f;
-}
-
-/**
- *
- * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
- */
-float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement,
-		float landing_slope_angle_rad)
-{
-	// flare_relative_alt is negative
-	return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad);
-}
-
-/**
- *
- * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
- */
-float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude,
-		float horizontal_slope_displacement, float landing_slope_angle_rad)
-{
-	return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement,
-					       landing_slope_angle_rad) + wp_landing_altitude;
-}
-
-/**
- *
- * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
- */
-float Landingslope::getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude,
-		float horizontal_slope_displacement, float landing_slope_angle_rad)
-{
-	return (slope_altitude - wp_landing_altitude) / tanf(landing_slope_angle_rad) + horizontal_slope_displacement;
-}
diff --git a/src/lib/landing_slope/Landingslope.hpp b/src/lib/landing_slope/Landingslope.hpp
deleted file mode 100644
index 6bdda717a1..0000000000
--- a/src/lib/landing_slope/Landingslope.hpp
+++ /dev/null
@@ -1,118 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2017 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 landingslope.h
- *
- * @author Thomas Gubler <thomasgubler@gmail.com>
- */
-
-#ifndef LANDINGSLOPE_H_
-#define LANDINGSLOPE_H_
-
-#include <math.h>
-
-class Landingslope
-{
-private:
-	/* see Documentation/fw_landing.png for a plot of the landing slope */
-	float _landing_slope_angle_rad{0.0f};		/**< phi in the plot */
-	float _flare_relative_alt{0.0f};		/**< h_flare,rel in the plot */
-	float _motor_lim_relative_alt{0.0f};
-	float _H1_virt{0.0f};				/**< H1 in the plot */
-	float _H0{0.0f};				/**< h_flare,rel + H1 in the plot */
-	float _d1{0.0f};				/**< d1 in the plot */
-	float _flare_constant{0.0f};
-	float _flare_length{0.0f};			/**< d1 + delta d in the plot */
-	float _horizontal_slope_displacement{0.0f};	/**< delta d in the plot */
-
-	void calculateSlopeValues();
-
-public:
-	Landingslope() = default;
-	~Landingslope() = default;
-
-	/**
-	 *
-	 * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
-	 */
-	float getLandingSlopeRelativeAltitude(float wp_landing_distance);
-
-	/**
-	 *
-	 * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
-	 * Performs check if aircraft is in front of waypoint to avoid climbout
-	 */
-	float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
-			float bearing_airplane_currwp);
-
-	/**
-	 *
-	 * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
-	 */
-	static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement,
-			float landing_slope_angle_rad);
-
-	/**
-	 *
-	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
-	 */
-	static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude,
-			float horizontal_slope_displacement, float landing_slope_angle_rad);
-
-	/**
-	 *
-	 * @return distance to landing waypoint of point on landing slope at altitude=slope_altitude
-	 */
-	static float getLandingSlopeWPDistance(float slope_altitude, float wp_landing_altitude,
-					       float horizontal_slope_displacement, float landing_slope_angle_rad);
-
-	float getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp,
-						float bearing_airplane_currwp);
-
-	void update(float landing_slope_angle_rad_new,
-		    float flare_relative_alt_new,
-		    float motor_lim_relative_alt_new,
-		    float H1_virt_new);
-
-
-	float landing_slope_angle_rad() { return _landing_slope_angle_rad; }
-	float flare_relative_alt() { return _flare_relative_alt; }
-	float motor_lim_relative_alt() { return _motor_lim_relative_alt; }
-	float flare_length() { return _flare_length; }
-	float horizontal_slope_displacement() { return _horizontal_slope_displacement; }
-
-};
-
-
-#endif /* LANDINGSLOPE_H_ */
diff --git a/src/lib/mathlib/math/Functions.hpp b/src/lib/mathlib/math/Functions.hpp
index 13de8e8ad7..b4e1162817 100644
--- a/src/lib/mathlib/math/Functions.hpp
+++ b/src/lib/mathlib/math/Functions.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2017-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
@@ -149,7 +149,7 @@ const T expo_deadzone(const T &value, const T &e, const T &dz)
  *         x_low   x_high
  */
 template<typename T>
-const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low, const T &y_high)
+const T interpolate(const T &value, const T &x_low, const T &x_high, const T &y_low, const T &y_high)
 {
 	if (value <= x_low) {
 		return y_low;
@@ -166,28 +166,47 @@ const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low,
 }
 
 /*
- * Constant, linear, linear, constant function with the three corner points as parameters
- *  y_high               -------
- *                      /
- *                    /
- *  y_middle        /
- *                /
+ * Constant, piecewise linear, constant function with 1/N size intervalls and N corner points as parameters
+ * y[N-1]               -------
+ *                     /
+ *                   /
+ * y[1]            /
  *               /
  *              /
- * y_low -------
- *         x_low x_middle x_high
+ *             /
+ * y[0] -------
+ *        0 1/(N-1) 2/(N-1) ... 1
  */
-template<typename T>
-const T gradual3(const T &value,
-		 const T &x_low, const T &x_middle, const T &x_high,
-		 const T &y_low, const T &y_middle, const T &y_high)
+template<typename T, size_t N>
+const T interpolateN(const T &value, const T(&y)[N])
 {
-	if (value < x_middle) {
-		return gradual(value, x_low, x_middle, y_low, y_middle);
+	size_t index = constrain((int)(value * (N - 1)), 0, (int)(N - 2));
+	return interpolate(value, (T)index / (T)(N - 1), (T)(index + 1) / (T)(N - 1), y[index], y[index + 1]);
+}
 
-	} else {
-		return gradual(value, x_middle, x_high, y_middle, y_high);
+/*
+ * Constant, piecewise linear, constant function with N corner points as parameters
+ * y[N-1]               -------
+ *                     /
+ *                   /
+ * y[1]            /
+ *               /
+ *              /
+ *             /
+ * y[0] -------
+ *          x[0] x[1] ... x[N-1]
+ * Note: x[N] corner coordinates have to be sorted in ascending order
+ */
+template<typename T, size_t N>
+const T interpolateNXY(const T &value, const T(&x)[N], const T(&y)[N])
+{
+	size_t index = 0;
+
+	while ((value > x[index + 1]) && (index < (N - 2))) {
+		index++;
 	}
+
+	return interpolate(value, x[index], x[index + 1], y[index], y[index + 1]);
 }
 
 /*
diff --git a/src/lib/mathlib/math/FunctionsTest.cpp b/src/lib/mathlib/math/FunctionsTest.cpp
index 48ac63fcec..a8c2315419 100644
--- a/src/lib/mathlib/math/FunctionsTest.cpp
+++ b/src/lib/mathlib/math/FunctionsTest.cpp
@@ -140,63 +140,57 @@ TEST(FunctionsTest, expo_deadzone)
 	EXPECT_FLOAT_EQ(expo_deadzone(-.5f, .9f, .4f), -.020833336f);
 }
 
-TEST(FunctionsTest, gradual)
+TEST(FunctionsTest, interpolate)
 {
 	// factor of *2, offset +1
-	EXPECT_FLOAT_EQ(gradual(-12.f, 0.f, 1.f, 1.f, 3.f), 1.f);
-	EXPECT_FLOAT_EQ(gradual(0.f, 0.f, 1.f, 1.f, 3.f), 1.f);
-	EXPECT_FLOAT_EQ(gradual(.25f, 0.f, 1.f, 1.f, 3.f), 1.5f);
-	EXPECT_FLOAT_EQ(gradual(.5f, 0.f, 1.f, 1.f, 3.f), 2.f);
-	EXPECT_FLOAT_EQ(gradual(.75f, 0.f, 1.f, 1.f, 3.f), 2.5f);
-	EXPECT_FLOAT_EQ(gradual(1.f, 0.f, 1.f, 1.f, 3.f), 3.f);
-	EXPECT_FLOAT_EQ(gradual(12.f, 0.f, 1.f, 1.f, 3.f), 3.f);
+	EXPECT_FLOAT_EQ(interpolate(-12.f, 0.f, 1.f, 1.f, 3.f), 1.f);
+	EXPECT_FLOAT_EQ(interpolate(0.f, 0.f, 1.f, 1.f, 3.f), 1.f);
+	EXPECT_FLOAT_EQ(interpolate(.25f, 0.f, 1.f, 1.f, 3.f), 1.5f);
+	EXPECT_FLOAT_EQ(interpolate(.5f, 0.f, 1.f, 1.f, 3.f), 2.f);
+	EXPECT_FLOAT_EQ(interpolate(.75f, 0.f, 1.f, 1.f, 3.f), 2.5f);
+	EXPECT_FLOAT_EQ(interpolate(1.f, 0.f, 1.f, 1.f, 3.f), 3.f);
+	EXPECT_FLOAT_EQ(interpolate(12.f, 0.f, 1.f, 1.f, 3.f), 3.f);
 
 	// factor of *2, offset +3
-	EXPECT_FLOAT_EQ(gradual(-12.f, 1.f, 2.f, 4.f, 6.f), 4.f);
-	EXPECT_FLOAT_EQ(gradual(1.f, 1.f, 2.f, 4.f, 6.f), 4.f);
-	EXPECT_FLOAT_EQ(gradual(1.25f, 1.f, 2.f, 4.f, 6.f), 4.5f);
-	EXPECT_FLOAT_EQ(gradual(1.5f, 1.f, 2.f, 4.f, 6.f), 5.f);
-	EXPECT_FLOAT_EQ(gradual(1.75f, 1.f, 2.f, 4.f, 6.f), 5.5f);
-	EXPECT_FLOAT_EQ(gradual(2.f, 1.f, 2.f, 4.f, 6.f), 6.f);
-	EXPECT_FLOAT_EQ(gradual(12.f, 1.f, 2.f, 4.f, 6.f), 6.f);
+	EXPECT_FLOAT_EQ(interpolate(-12.f, 1.f, 2.f, 4.f, 6.f), 4.f);
+	EXPECT_FLOAT_EQ(interpolate(1.f, 1.f, 2.f, 4.f, 6.f), 4.f);
+	EXPECT_FLOAT_EQ(interpolate(1.25f, 1.f, 2.f, 4.f, 6.f), 4.5f);
+	EXPECT_FLOAT_EQ(interpolate(1.5f, 1.f, 2.f, 4.f, 6.f), 5.f);
+	EXPECT_FLOAT_EQ(interpolate(1.75f, 1.f, 2.f, 4.f, 6.f), 5.5f);
+	EXPECT_FLOAT_EQ(interpolate(2.f, 1.f, 2.f, 4.f, 6.f), 6.f);
+	EXPECT_FLOAT_EQ(interpolate(12.f, 1.f, 2.f, 4.f, 6.f), 6.f);
 
 	// corner case when x_low == x_high == value
-	EXPECT_FLOAT_EQ(gradual(1.f, 1.f, 1.f, 4.f, 6.f), 4.f);
+	EXPECT_FLOAT_EQ(interpolate(1.f, 1.f, 1.f, 4.f, 6.f), 4.f);
 
 	// corner case when x_low > x_high
-	EXPECT_FLOAT_EQ(gradual(1.05f, 1.1f, 1.f, 4.f, 6.f), 4.f);
+	EXPECT_FLOAT_EQ(interpolate(1.05f, 1.1f, 1.f, 4.f, 6.f), 4.f);
+
+	// corner case when y_low == y_high == value
+	EXPECT_FLOAT_EQ(interpolate(1.f, 1.f, 1.f, 4.f, 6.f), 4.f);
+
+	// corner case when y_low > y_high
+	EXPECT_FLOAT_EQ(interpolate(1.5f, 1.f, 2.f, 6.f, 4.f), 5.f);
+
+	// corner case when x_low == x_high ==  y_low == y_high == 0.f
+	EXPECT_FLOAT_EQ(interpolate(0.f, 0.f, 0.f, 0.f, 0.f), 0.f);
 }
 
-TEST(FunctionsTest, gradual3)
+TEST(FunctionsTest, interpolateNXY)
 {
+	float x[3] = {0.f, .5f, 1.5f};
+	float y[3] = {1.f, 2.f, 3.f};
+
 	// factor of *2, offset +1
-	EXPECT_FLOAT_EQ(gradual3(-12.f,
-				 0.f, .5f, 1.5f,
-				 1.f, 2.f, 3.f), 1.f);
-	EXPECT_FLOAT_EQ(gradual3(0.f,
-				 0.f, .5f, 1.5f,
-				 1.f, 2.f, 3.f), 1.f);
-	EXPECT_FLOAT_EQ(gradual3(.25f,
-				 0.f, .5f, 1.5f,
-				 1.f, 2.f, 3.f), 1.5f);
-	EXPECT_FLOAT_EQ(gradual3(.5f,
-				 0.f, .5f, 1.5f,
-				 1.f, 2.f, 3.f), 2.f);
-	EXPECT_FLOAT_EQ(gradual3(.75f,
-				 0.f, .5f, 1.5f,
-				 1.f, 2.f, 3.f), 2.25f);
-	EXPECT_FLOAT_EQ(gradual3(1.f,
-				 0.f, .5f, 1.5f,
-				 1.f, 2.f, 3.f), 2.5f);
-	EXPECT_FLOAT_EQ(gradual3(1.25f,
-				 0.f, .5f, 1.5f,
-				 1.f, 2.f, 3.f), 2.75f);
-	EXPECT_FLOAT_EQ(gradual3(1.5f,
-				 0.f, .5f, 1.5f,
-				 1.f, 2.f, 3.f), 3.f);
-	EXPECT_FLOAT_EQ(gradual3(12.f,
-				 0.f, .5f, 1.5f,
-				 1.f, 2.f, 3.f), 3.f);
+	EXPECT_FLOAT_EQ(interpolateNXY(-12.f, x, y), 1.f);
+	EXPECT_FLOAT_EQ(interpolateNXY(0.f, x, y), 1.f);
+	EXPECT_FLOAT_EQ(interpolateNXY(.25f, x, y), 1.5f);
+	EXPECT_FLOAT_EQ(interpolateNXY(.5f, x, y), 2.f);
+	EXPECT_FLOAT_EQ(interpolateNXY(.75f, x, y), 2.25f);
+	EXPECT_FLOAT_EQ(interpolateNXY(1.f, x, y), 2.5f);
+	EXPECT_FLOAT_EQ(interpolateNXY(1.25f, x, y), 2.75f);
+	EXPECT_FLOAT_EQ(interpolateNXY(1.5f, x, y), 3.f);
+	EXPECT_FLOAT_EQ(interpolateNXY(12.f, x, y), 3.f);
 }
 
 TEST(FunctionsTest, sqrt_linear)
diff --git a/src/lib/mathlib/math/WelfordMean.hpp b/src/lib/mathlib/math/WelfordMean.hpp
index e5b0240533..80d6b68a60 100644
--- a/src/lib/mathlib/math/WelfordMean.hpp
+++ b/src/lib/mathlib/math/WelfordMean.hpp
@@ -42,22 +42,25 @@
 namespace math
 {
 
-template<typename T>
+template <typename Type, size_t N>
 class WelfordMean
 {
 public:
 	// For a new value, compute the new count, new mean, the new M2.
-	void update(const T &new_value)
+	void update(const matrix::Vector<Type, N> &new_value)
 	{
 		_count++;
 
 		// mean accumulates the mean of the entire dataset
-		const T delta{new_value - _mean};
+		const matrix::Vector<Type, N> delta{new_value - _mean};
 		_mean += delta / _count;
 
 		// M2 aggregates the squared distance from the mean
 		// count aggregates the number of samples seen so far
 		_M2 += delta.emult(new_value - _mean);
+
+		// protect against floating point precision causing negative variances
+		_M2 = matrix::max(_M2, {});
 	}
 
 	bool valid() const { return _count > 2; }
@@ -71,12 +74,12 @@ public:
 	}
 
 	// Retrieve the mean, variance and sample variance
-	T mean() const { return _mean; }
-	T variance() const { return _M2 / _count; }
-	T sample_variance() const { return _M2 / (_count - 1); }
+	matrix::Vector<Type, N> mean() const { return _mean; }
+	matrix::Vector<Type, N> variance() const { return _M2 / _count; }
+	matrix::Vector<Type, N> sample_variance() const { return _M2 / (_count - 1); }
 private:
-	T _mean{};
-	T _M2{};
+	matrix::Vector<Type, N> _mean{};
+	matrix::Vector<Type, N> _M2{};
 	unsigned _count{0};
 };
 
diff --git a/src/lib/mathlib/math/WelfordMeanTest.cpp b/src/lib/mathlib/math/WelfordMeanTest.cpp
index 06b59e6dfb..15abbe10c4 100644
--- a/src/lib/mathlib/math/WelfordMeanTest.cpp
+++ b/src/lib/mathlib/math/WelfordMeanTest.cpp
@@ -45,7 +45,7 @@ TEST(WelfordMeanTest, NoisySignal)
 	std::normal_distribution<float> standard_normal_distribution{0.f, std_dev};
 	std::default_random_engine random_generator{}; // Pseudo-random generator with constant seed
 	random_generator.seed(42);
-	WelfordMean<Vector3f> welford{};
+	WelfordMean<float, 3> welford{};
 
 	for (int i = 0; i < 50; i++) {
 		const float noisy_value = standard_normal_distribution(random_generator);
diff --git a/src/lib/matrix/matrix/Vector2.hpp b/src/lib/matrix/matrix/Vector2.hpp
index 35ceb7c1ac..82d1b152a5 100644
--- a/src/lib/matrix/matrix/Vector2.hpp
+++ b/src/lib/matrix/matrix/Vector2.hpp
@@ -71,6 +71,32 @@ public:
 		return (*this).cross(b);
 	}
 
+	/**
+	 * @brief rotate vector
+	 *
+	 * @param[in] angle [rad]	The angle around the positive out of plane axis.
+	 */
+	void rotate(const Type angle)
+	{
+		Vector2 &v(*this);
+		Vector2 rotated;
+		Type sin_angle = Type(sin(angle));
+		Type cos_angle = Type(cos(angle));
+		rotated(0) = cos_angle * v(0) - sin_angle * v(1);
+		rotated(1) = sin_angle * v(0) + cos_angle * v(1);
+		v = rotated;
+	}
+
+	/**
+	 * @brief Transform vector in new coordinate system
+	 *
+	 * @param[in] angle [rad]	The rotation of new coordinate system around the positive out of plane axis.
+	 */
+	void transform(const Type angle)
+	{
+		this->rotate(-angle);
+	}
+
 };
 
 
diff --git a/src/lib/matrix/test/MatrixVector2Test.cpp b/src/lib/matrix/test/MatrixVector2Test.cpp
index 23effd262b..7fc19cef00 100644
--- a/src/lib/matrix/test/MatrixVector2Test.cpp
+++ b/src/lib/matrix/test/MatrixVector2Test.cpp
@@ -34,6 +34,7 @@
 
 #include <gtest/gtest.h>
 #include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
 
 using namespace matrix;
 
@@ -64,4 +65,20 @@ TEST(MatrixVector2Test, Vector2)
 	Vector2f h(g);
 	EXPECT_FLOAT_EQ(h(0), 1.23f);
 	EXPECT_FLOAT_EQ(h(1), 423.4f);
+
+	// rotate 2D vector
+	Vector2f i(1.0f, 0.0f);
+	i.rotate(math::radians(45.0));
+	EXPECT_FLOAT_EQ(i(0), i(1));
+	i.rotate(math::radians(45.0));
+	EXPECT_FLOAT_EQ(i(0), 0.f);
+	EXPECT_FLOAT_EQ(i(1), 1.f);
+
+	// transform vector
+	Vector2f j(1.0f, 0.0f);
+	j.transform(math::radians(-45.0));
+	EXPECT_FLOAT_EQ(j(0), j(1));
+	j.transform(math::radians(-45.0));
+	EXPECT_FLOAT_EQ(j(0), 0.f);
+	EXPECT_FLOAT_EQ(j(1), 1.f);
 }
diff --git a/src/lib/mixer_module/CMakeLists.txt b/src/lib/mixer_module/CMakeLists.txt
index 0500e265d7..3f9822027f 100644
--- a/src/lib/mixer_module/CMakeLists.txt
+++ b/src/lib/mixer_module/CMakeLists.txt
@@ -44,14 +44,25 @@ add_custom_command(OUTPUT ${functions_header}
 add_custom_target(output_functions_header DEPENDS ${functions_header})
 
 px4_add_library(mixer_module
+	functions/FunctionActuatorSet.hpp
+	functions/FunctionConstantMax.hpp
+	functions/FunctionConstantMin.hpp
+	functions/FunctionGimbal.hpp
+	functions/FunctionLandingGear.hpp
+	functions/FunctionManualRC.hpp
+	functions/FunctionMotors.hpp
+	functions/FunctionParachute.hpp
+	functions/FunctionServos.hpp
+
 	actuator_test.cpp
+	actuator_test.hpp
 	mixer_module.cpp
-	functions.cpp
+	mixer_module.hpp
 	)
 
 add_dependencies(mixer_module output_functions_header)
 target_compile_options(mixer_module PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
-
+target_include_directories(mixer_module PRIVATE ${CMAKE_CURRENT_BINARY_DIR})
 
 px4_add_functional_gtest(SRC mixer_module_tests.cpp LINKLIBS mixer_module mixer)
 
diff --git a/src/lib/mixer_module/actuator_test.cpp b/src/lib/mixer_module/actuator_test.cpp
index b022327429..3ff8d4a7c3 100644
--- a/src/lib/mixer_module/actuator_test.cpp
+++ b/src/lib/mixer_module/actuator_test.cpp
@@ -33,6 +33,8 @@
 
 #include "actuator_test.hpp"
 
+#include "functions/FunctionMotors.hpp"
+
 using namespace time_literals;
 
 ActuatorTest::ActuatorTest(const OutputFunction function_assignments[MAX_ACTUATORS])
diff --git a/src/lib/mixer_module/actuator_test.hpp b/src/lib/mixer_module/actuator_test.hpp
index 26fcd80e21..e3e4816b7e 100644
--- a/src/lib/mixer_module/actuator_test.hpp
+++ b/src/lib/mixer_module/actuator_test.hpp
@@ -33,7 +33,7 @@
 
 #pragma once
 
-#include "functions.hpp"
+#include <mixer_module/output_functions.hpp>
 
 #include <drivers/drv_pwm_output.h>
 #include <uORB/topics/actuator_test.h>
diff --git a/src/lib/mixer_module/functions.cpp b/src/lib/mixer_module/functions.cpp
deleted file mode 100644
index 4b70676e9f..0000000000
--- a/src/lib/mixer_module/functions.cpp
+++ /dev/null
@@ -1,135 +0,0 @@
-/****************************************************************************
- *
- *   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 "functions.hpp"
-
-FunctionMotors::FunctionMotors(const Context &context)
-	: _topic(&context.work_item, ORB_ID(actuator_motors)),
-	  _thrust_factor(context.thrust_factor)
-{
-	for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) {
-		_data.control[i] = NAN;
-	}
-}
-void FunctionMotors::update()
-{
-	if (_topic.update(&_data)) {
-		updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS);
-	}
-}
-
-FunctionServos::FunctionServos(const Context &context)
-	: _topic(&context.work_item, ORB_ID(actuator_servos))
-{
-	for (int i = 0; i < actuator_servos_s::NUM_CONTROLS; ++i) {
-		_data.control[i] = NAN;
-	}
-}
-
-FunctionActuatorSet::FunctionActuatorSet()
-{
-	for (int i = 0; i < max_num_actuators; ++i) {
-		_data[i] = NAN;
-	}
-}
-
-void FunctionActuatorSet::update()
-{
-	vehicle_command_s vehicle_command;
-
-	while (_topic.update(&vehicle_command)) {
-		if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR) {
-			int index = (int)(vehicle_command.param7 + 0.5f);
-
-			if (index == 0) {
-				_data[0] = vehicle_command.param1;
-				_data[1] = vehicle_command.param2;
-				_data[2] = vehicle_command.param3;
-				_data[3] = vehicle_command.param4;
-				_data[4] = vehicle_command.param5;
-				_data[5] = vehicle_command.param6;
-			}
-		}
-	}
-}
-
-void FunctionLandingGear::update()
-{
-	landing_gear_s landing_gear;
-
-	if (_topic.update(&landing_gear)) {
-		if (landing_gear.landing_gear == landing_gear_s::GEAR_DOWN) {
-			_data = -1.f;
-
-		} else if (landing_gear.landing_gear == landing_gear_s::GEAR_UP) {
-			_data = 1.f;
-		}
-	}
-}
-
-FunctionManualRC::FunctionManualRC()
-{
-	for (int i = 0; i < num_data_points; ++i) {
-		_data[i] = NAN;
-	}
-}
-
-void FunctionManualRC::update()
-{
-	manual_control_setpoint_s manual_control_setpoint;
-
-	if (_topic.update(&manual_control_setpoint)) {
-		_data[0] = manual_control_setpoint.y; // roll
-		_data[1] = manual_control_setpoint.x; // pitch
-		_data[2] = manual_control_setpoint.z * 2.f - 1.f; // throttle
-		_data[3] = manual_control_setpoint.r; // yaw
-		_data[4] = manual_control_setpoint.flaps;
-		_data[5] = manual_control_setpoint.aux1;
-		_data[6] = manual_control_setpoint.aux2;
-		_data[7] = manual_control_setpoint.aux3;
-		_data[8] = manual_control_setpoint.aux4;
-		_data[9] = manual_control_setpoint.aux5;
-		_data[10] = manual_control_setpoint.aux6;
-	}
-}
-
-void FunctionGimbal::update()
-{
-	actuator_controls_s actuator_controls;
-
-	if (_topic.update(&actuator_controls)) {
-		_data[0] = actuator_controls.control[0];
-		_data[1] = actuator_controls.control[1];
-		_data[2] = actuator_controls.control[2];
-	}
-}
diff --git a/src/lib/mixer_module/functions.hpp b/src/lib/mixer_module/functions.hpp
deleted file mode 100644
index 1059baee2a..0000000000
--- a/src/lib/mixer_module/functions.hpp
+++ /dev/null
@@ -1,295 +0,0 @@
-/****************************************************************************
- *
- *   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 <limits.h>
-
-#include <mixer_module/output_functions.hpp>
-
-#include <uORB/Subscription.hpp>
-#include <uORB/SubscriptionCallback.hpp>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/actuator_servos.h>
-#include <uORB/topics/vehicle_command.h>
-#include <uORB/topics/landing_gear.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-
-class FunctionProviderBase
-{
-public:
-	struct Context {
-		px4::WorkItem &work_item;
-		const float &thrust_factor;
-	};
-
-	FunctionProviderBase() = default;
-	virtual ~FunctionProviderBase() = default;
-
-	virtual void update() = 0;
-
-	/**
-	 * Get the current output value for a given function
-	 * @return NAN (=disarmed) or value in range [-1, 1]
-	 */
-	virtual float value(OutputFunction func) = 0;
-
-	virtual float defaultFailsafeValue(OutputFunction func) const { return NAN; }
-	virtual bool allowPrearmControl() const { return true; }
-
-	virtual uORB::SubscriptionCallbackWorkItem *subscriptionCallback() { return nullptr; }
-
-	virtual bool getLatestSampleTimestamp(hrt_abstime &t) const { return false; }
-
-	/**
-	 * Check whether the output (motor) is configured to be reversible
-	 */
-	virtual bool reversible(OutputFunction func) const { return false; }
-};
-
-/**
- * Functions: Constant_Min
- */
-class FunctionConstantMin : public FunctionProviderBase
-{
-public:
-	static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMin(); }
-
-	float value(OutputFunction func) override { return -1.f; }
-	void update() override { }
-
-	float defaultFailsafeValue(OutputFunction func) const override { return -1.f; }
-};
-
-/**
- * Functions: Constant_Max
- */
-class FunctionConstantMax : public FunctionProviderBase
-{
-public:
-	static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMax(); }
-
-	float value(OutputFunction func) override { return 1.f; }
-	void update() override { }
-
-	float defaultFailsafeValue(OutputFunction func) const override { return 1.f; }
-};
-
-/**
- * Functions: Motor1 ... MotorMax
- */
-class FunctionMotors : public FunctionProviderBase
-{
-public:
-	static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::MotorMax - (int)OutputFunction::Motor1 + 1,
-		      "Unexpected num motors");
-
-	static_assert(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "Unexpected motor idx");
-
-	FunctionMotors(const Context &context);
-	static FunctionProviderBase *allocate(const Context &context) { return new FunctionMotors(context); }
-
-	void update() override;
-	float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Motor1]; }
-
-	bool allowPrearmControl() const override { return false; }
-
-	uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; }
-
-	bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; }
-
-	static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values);
-
-	bool reversible(OutputFunction func) const override
-	{ return _data.reversible_flags & (1u << ((int)func - (int)OutputFunction::Motor1)); }
-private:
-	uORB::SubscriptionCallbackWorkItem _topic;
-	actuator_motors_s _data{};
-	const float &_thrust_factor;
-};
-
-void FunctionMotors::updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values)
-{
-	if (thrust_factor > 0.f && thrust_factor <= 1.f) {
-		// thrust factor
-		//  rel_thrust = factor * x^2 + (1-factor) * x,
-		const float a = thrust_factor;
-		const float b = (1.f - thrust_factor);
-
-		// don't recompute for all values (ax^2+bx+c=0)
-		const float tmp1 = b / (2.f * a);
-		const float tmp2 = b * b / (4.f * a * a);
-
-		for (int i = 0; i < num_values; ++i) {
-			float control = values[i];
-
-			if (control > 0.f) {
-				values[i] = -tmp1 + sqrtf(tmp2 + (control / a));
-
-			} else if (control < -0.f) {
-				values[i] =  tmp1 - sqrtf(tmp2 - (control / a));
-
-			} else {
-				values[i] = 0.f;
-			}
-		}
-	}
-
-	for (int i = 0; i < num_values; ++i) {
-		if ((reversible & (1u << i)) == 0) {
-			if (values[i] < -FLT_EPSILON) {
-				values[i] = NAN;
-
-			} else {
-				// remap from [0, 1] to [-1, 1]
-				values[i] = values[i] * 2.f - 1.f;
-			}
-		}
-	}
-}
-
-
-/**
- * Functions: Servo1 ... ServoMax
- */
-class FunctionServos : public FunctionProviderBase
-{
-public:
-	static_assert(actuator_servos_s::NUM_CONTROLS == (int)OutputFunction::ServoMax - (int)OutputFunction::Servo1 + 1,
-		      "Unexpected num servos");
-
-	FunctionServos(const Context &context);
-	static FunctionProviderBase *allocate(const Context &context) { return new FunctionServos(context); }
-
-	void update() override { _topic.update(&_data); }
-	float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Servo1]; }
-
-	uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; }
-
-	float defaultFailsafeValue(OutputFunction func) const override { return 0.f; }
-private:
-	uORB::SubscriptionCallbackWorkItem _topic;
-	actuator_servos_s _data{};
-};
-
-
-/**
- * Functions: Offboard_Actuator_Set1 ... Offboard_Actuator_Set6
- */
-class FunctionActuatorSet : public FunctionProviderBase
-{
-public:
-	FunctionActuatorSet();
-	static FunctionProviderBase *allocate(const Context &context) { return new FunctionActuatorSet(); }
-
-	void update() override;
-	float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Offboard_Actuator_Set1]; }
-
-private:
-	static constexpr int max_num_actuators = 6;
-
-	uORB::Subscription _topic{ORB_ID(vehicle_command)};
-	float _data[max_num_actuators];
-};
-
-/**
- * Functions: Landing_Gear
- */
-class FunctionLandingGear : public FunctionProviderBase
-{
-public:
-	FunctionLandingGear() = default;
-	static FunctionProviderBase *allocate(const Context &context) { return new FunctionLandingGear(); }
-
-	void update() override;
-	float value(OutputFunction func) override { return _data; }
-
-private:
-	uORB::Subscription _topic{ORB_ID(landing_gear)};
-	float _data{-1.f};
-};
-
-/**
- * Functions: Parachute
- */
-class FunctionParachute : public FunctionProviderBase
-{
-public:
-	FunctionParachute() = default;
-	static FunctionProviderBase *allocate(const Context &context) { return new FunctionParachute(); }
-
-	void update() override {}
-	float value(OutputFunction func) override { return -1.f; }
-	float defaultFailsafeValue(OutputFunction func) const override { return 1.f; }
-};
-
-/**
- * Functions: RC_Roll .. RCAUX_Max
- */
-class FunctionManualRC : public FunctionProviderBase
-{
-public:
-	FunctionManualRC();
-	static FunctionProviderBase *allocate(const Context &context) { return new FunctionManualRC(); }
-
-	void update() override;
-	float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::RC_Roll]; }
-
-private:
-	static constexpr int num_data_points = 11;
-
-	static_assert(num_data_points == (int)OutputFunction::RC_AUXMax - (int)OutputFunction::RC_Roll + 1,
-		      "number of functions mismatch");
-
-	uORB::Subscription _topic{ORB_ID(manual_control_setpoint)};
-	float _data[num_data_points];
-};
-
-/**
- * Functions: Gimbal_Roll .. Gimbal_Yaw
- */
-class FunctionGimbal : public FunctionProviderBase
-{
-public:
-	FunctionGimbal() = default;
-	static FunctionProviderBase *allocate(const Context &context) { return new FunctionGimbal(); }
-
-	void update() override;
-	float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Gimbal_Roll]; }
-
-private:
-	uORB::Subscription _topic{ORB_ID(actuator_controls_2)};
-	float _data[3] { NAN, NAN, NAN };
-};
diff --git a/src/lib/mixer_module/functions/FunctionActuatorSet.hpp b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp
new file mode 100644
index 0000000000..c9369addd5
--- /dev/null
+++ b/src/lib/mixer_module/functions/FunctionActuatorSet.hpp
@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-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 "FunctionProviderBase.hpp"
+
+#include <uORB/topics/vehicle_command.h>
+
+/**
+ * Functions: Offboard_Actuator_Set1 ... Offboard_Actuator_Set6
+ */
+class FunctionActuatorSet : public FunctionProviderBase
+{
+public:
+	FunctionActuatorSet()
+	{
+		for (int i = 0; i < max_num_actuators; ++i) {
+			_data[i] = NAN;
+		}
+	}
+
+	static FunctionProviderBase *allocate(const Context &context) { return new FunctionActuatorSet(); }
+
+	void update() override
+	{
+		vehicle_command_s vehicle_command;
+
+		while (_topic.update(&vehicle_command)) {
+			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR) {
+				int index = (int)(vehicle_command.param7 + 0.5f);
+
+				if (index == 0) {
+					_data[0] = vehicle_command.param1;
+					_data[1] = vehicle_command.param2;
+					_data[2] = vehicle_command.param3;
+					_data[3] = vehicle_command.param4;
+					_data[4] = vehicle_command.param5;
+					_data[5] = vehicle_command.param6;
+				}
+			}
+		}
+	}
+
+	float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Offboard_Actuator_Set1]; }
+
+private:
+	static constexpr int max_num_actuators = 6;
+
+	uORB::Subscription _topic{ORB_ID(vehicle_command)};
+	float _data[max_num_actuators] {};
+};
diff --git a/src/lib/mixer_module/functions/FunctionConstantMax.hpp b/src/lib/mixer_module/functions/FunctionConstantMax.hpp
new file mode 100644
index 0000000000..317a209ee2
--- /dev/null
+++ b/src/lib/mixer_module/functions/FunctionConstantMax.hpp
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-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 "FunctionProviderBase.hpp"
+
+/**
+ * Functions: Constant_Max
+ */
+class FunctionConstantMax : public FunctionProviderBase
+{
+public:
+	static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMax(); }
+
+	float value(OutputFunction func) override { return 1.f; }
+	void update() override { }
+
+	float defaultFailsafeValue(OutputFunction func) const override { return 1.f; }
+};
diff --git a/src/lib/mixer_module/functions/FunctionConstantMin.hpp b/src/lib/mixer_module/functions/FunctionConstantMin.hpp
new file mode 100644
index 0000000000..1516d80520
--- /dev/null
+++ b/src/lib/mixer_module/functions/FunctionConstantMin.hpp
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-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 "FunctionProviderBase.hpp"
+
+/**
+ * Functions: Constant_Min
+ */
+class FunctionConstantMin : public FunctionProviderBase
+{
+public:
+	static FunctionProviderBase *allocate(const Context &context) { return new FunctionConstantMin(); }
+
+	float value(OutputFunction func) override { return -1.f; }
+	void update() override { }
+
+	float defaultFailsafeValue(OutputFunction func) const override { return -1.f; }
+};
diff --git a/src/lib/mixer_module/functions/FunctionGimbal.hpp b/src/lib/mixer_module/functions/FunctionGimbal.hpp
new file mode 100644
index 0000000000..513a344460
--- /dev/null
+++ b/src/lib/mixer_module/functions/FunctionGimbal.hpp
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-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 "FunctionProviderBase.hpp"
+
+#include <uORB/topics/actuator_controls.h>
+
+/**
+ * Functions: Gimbal_Roll .. Gimbal_Yaw
+ */
+class FunctionGimbal : public FunctionProviderBase
+{
+public:
+	FunctionGimbal() = default;
+	static FunctionProviderBase *allocate(const Context &context) { return new FunctionGimbal(); }
+
+	void update() override
+	{
+		actuator_controls_s actuator_controls;
+
+		if (_topic.update(&actuator_controls)) {
+			_data[0] = actuator_controls.control[0];
+			_data[1] = actuator_controls.control[1];
+			_data[2] = actuator_controls.control[2];
+		}
+	}
+
+	float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::Gimbal_Roll]; }
+
+private:
+	uORB::Subscription _topic{ORB_ID(actuator_controls_2)};
+	float _data[3] { NAN, NAN, NAN };
+};
diff --git a/src/lib/mixer_module/functions/FunctionGripper.hpp b/src/lib/mixer_module/functions/FunctionGripper.hpp
new file mode 100644
index 0000000000..c9cdc3d592
--- /dev/null
+++ b/src/lib/mixer_module/functions/FunctionGripper.hpp
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   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 "FunctionProviderBase.hpp"
+
+#include <uORB/topics/gripper.h>
+
+/**
+ * @brief Function: Gripper (Used for actuating a Gripper)
+ */
+class FunctionGripper : public FunctionProviderBase
+{
+public:
+	FunctionGripper() = default;
+	static FunctionProviderBase *allocate(const Context &context) { return new FunctionGripper(); }
+
+	void update() override
+	{
+		gripper_s gripper;
+
+		if (_gripper_sub.update(&gripper)) {
+			if (gripper.command == gripper_s::COMMAND_RELEASE) {
+				_data = -1.f; // Minimum command for release
+
+			} else if (gripper.command == gripper_s::COMMAND_GRAB) {
+				_data = 1.f; // Maximum command for grab
+
+			}
+		}
+	}
+
+	float value(OutputFunction func) override { return _data; }
+
+private:
+	uORB::Subscription _gripper_sub{ORB_ID(gripper)};
+	float _data{-1.f};
+};
diff --git a/src/lib/mixer_module/functions/FunctionLandingGear.hpp b/src/lib/mixer_module/functions/FunctionLandingGear.hpp
new file mode 100644
index 0000000000..80c87bc9cd
--- /dev/null
+++ b/src/lib/mixer_module/functions/FunctionLandingGear.hpp
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-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 "FunctionProviderBase.hpp"
+
+#include <uORB/topics/landing_gear.h>
+
+/**
+ * Functions: Landing_Gear
+ */
+class FunctionLandingGear : public FunctionProviderBase
+{
+public:
+	FunctionLandingGear() = default;
+	static FunctionProviderBase *allocate(const Context &context) { return new FunctionLandingGear(); }
+
+	void update() override
+	{
+		landing_gear_s landing_gear;
+
+		if (_topic.update(&landing_gear)) {
+			if (landing_gear.landing_gear == landing_gear_s::GEAR_DOWN) {
+				_data = -1.f;
+
+			} else if (landing_gear.landing_gear == landing_gear_s::GEAR_UP) {
+				_data = 1.f;
+			}
+		}
+	}
+
+	float value(OutputFunction func) override { return _data; }
+
+private:
+	uORB::Subscription _topic{ORB_ID(landing_gear)};
+	float _data{-1.f};
+};
diff --git a/src/lib/mixer_module/functions/FunctionManualRC.hpp b/src/lib/mixer_module/functions/FunctionManualRC.hpp
new file mode 100644
index 0000000000..9ec2db2b6d
--- /dev/null
+++ b/src/lib/mixer_module/functions/FunctionManualRC.hpp
@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-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 "FunctionProviderBase.hpp"
+
+#include <uORB/topics/manual_control_setpoint.h>
+
+/**
+ * Functions: RC_Roll .. RCAUX_Max
+ */
+class FunctionManualRC : public FunctionProviderBase
+{
+public:
+	FunctionManualRC()
+	{
+		for (int i = 0; i < num_data_points; ++i) {
+			_data[i] = NAN;
+		}
+	}
+
+	static FunctionProviderBase *allocate(const Context &context) { return new FunctionManualRC(); }
+
+	void update() override
+	{
+		manual_control_setpoint_s manual_control_setpoint;
+
+		if (_topic.update(&manual_control_setpoint)) {
+			_data[0] = manual_control_setpoint.y; // roll
+			_data[1] = manual_control_setpoint.x; // pitch
+			_data[2] = manual_control_setpoint.z * 2.f - 1.f; // throttle
+			_data[3] = manual_control_setpoint.r; // yaw
+			_data[4] = manual_control_setpoint.flaps;
+			_data[5] = manual_control_setpoint.aux1;
+			_data[6] = manual_control_setpoint.aux2;
+			_data[7] = manual_control_setpoint.aux3;
+			_data[8] = manual_control_setpoint.aux4;
+			_data[9] = manual_control_setpoint.aux5;
+			_data[10] = manual_control_setpoint.aux6;
+		}
+	}
+
+	float value(OutputFunction func) override { return _data[(int)func - (int)OutputFunction::RC_Roll]; }
+
+private:
+	static constexpr int num_data_points = 11;
+
+	static_assert(num_data_points == (int)OutputFunction::RC_AUXMax - (int)OutputFunction::RC_Roll + 1,
+		      "number of functions mismatch");
+
+	uORB::Subscription _topic{ORB_ID(manual_control_setpoint)};
+	float _data[num_data_points] {};
+};
diff --git a/src/lib/mixer_module/functions/FunctionMotors.hpp b/src/lib/mixer_module/functions/FunctionMotors.hpp
new file mode 100644
index 0000000000..6873a3919f
--- /dev/null
+++ b/src/lib/mixer_module/functions/FunctionMotors.hpp
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-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 "FunctionProviderBase.hpp"
+
+#include <uORB/topics/actuator_servos.h>
+
+/**
+ * Functions: Motor1 ... MotorMax
+ */
+class FunctionMotors : public FunctionProviderBase
+{
+public:
+	static_assert(actuator_motors_s::NUM_CONTROLS == (int)OutputFunction::MotorMax - (int)OutputFunction::Motor1 + 1,
+		      "Unexpected num motors");
+
+	static_assert(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "Unexpected motor idx");
+
+	FunctionMotors(const Context &context) :
+		_topic(&context.work_item, ORB_ID(actuator_motors)),
+		_thrust_factor(context.thrust_factor)
+	{
+		for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) {
+			_data.control[i] = NAN;
+		}
+	}
+
+	static FunctionProviderBase *allocate(const Context &context) { return new FunctionMotors(context); }
+
+	void update() override
+	{
+		if (_topic.update(&_data)) {
+			updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS);
+		}
+	}
+
+	float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Motor1]; }
+
+	bool allowPrearmControl() const override { return false; }
+
+	uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; }
+
+	bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; }
+
+	static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values)
+	{
+		if (thrust_factor > 0.f && thrust_factor <= 1.f) {
+			// thrust factor
+			//  rel_thrust = factor * x^2 + (1-factor) * x,
+			const float a = thrust_factor;
+			const float b = (1.f - thrust_factor);
+
+			// don't recompute for all values (ax^2+bx+c=0)
+			const float tmp1 = b / (2.f * a);
+			const float tmp2 = b * b / (4.f * a * a);
+
+			for (int i = 0; i < num_values; ++i) {
+				float control = values[i];
+
+				if (control > 0.f) {
+					values[i] = -tmp1 + sqrtf(tmp2 + (control / a));
+
+				} else if (control < -0.f) {
+					values[i] =  tmp1 - sqrtf(tmp2 - (control / a));
+
+				} else {
+					values[i] = 0.f;
+				}
+			}
+		}
+
+		for (int i = 0; i < num_values; ++i) {
+			if ((reversible & (1u << i)) == 0) {
+				if (values[i] < -FLT_EPSILON) {
+					values[i] = NAN;
+
+				} else {
+					// remap from [0, 1] to [-1, 1]
+					values[i] = values[i] * 2.f - 1.f;
+				}
+			}
+		}
+	}
+
+	bool reversible(OutputFunction func) const override { return _data.reversible_flags & (1u << ((int)func - (int)OutputFunction::Motor1)); }
+
+private:
+	uORB::SubscriptionCallbackWorkItem _topic;
+	actuator_motors_s _data{};
+	const float &_thrust_factor;
+};
diff --git a/src/lib/mixer_module/functions/FunctionParachute.hpp b/src/lib/mixer_module/functions/FunctionParachute.hpp
new file mode 100644
index 0000000000..d81f623325
--- /dev/null
+++ b/src/lib/mixer_module/functions/FunctionParachute.hpp
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-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 "FunctionProviderBase.hpp"
+
+/**
+ * Functions: Parachute
+ */
+class FunctionParachute : public FunctionProviderBase
+{
+public:
+	FunctionParachute() = default;
+	static FunctionProviderBase *allocate(const Context &context) { return new FunctionParachute(); }
+
+	void update() override {}
+	float value(OutputFunction func) override { return -1.f; }
+	float defaultFailsafeValue(OutputFunction func) const override { return 1.f; }
+};
diff --git a/src/lib/mixer_module/functions/FunctionProviderBase.hpp b/src/lib/mixer_module/functions/FunctionProviderBase.hpp
new file mode 100644
index 0000000000..f3a648bb28
--- /dev/null
+++ b/src/lib/mixer_module/functions/FunctionProviderBase.hpp
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-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 <limits.h>
+
+#include <mixer_module/output_functions.hpp>
+
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+class FunctionProviderBase
+{
+public:
+	struct Context {
+		px4::WorkItem &work_item;
+		const float &thrust_factor;
+	};
+
+	FunctionProviderBase() = default;
+	virtual ~FunctionProviderBase() = default;
+
+	virtual void update() = 0;
+
+	/**
+	 * Get the current output value for a given function
+	 * @return NAN (=disarmed) or value in range [-1, 1]
+	 */
+	virtual float value(OutputFunction func) = 0;
+
+	virtual float defaultFailsafeValue(OutputFunction func) const { return NAN; }
+	virtual bool allowPrearmControl() const { return true; }
+
+	virtual uORB::SubscriptionCallbackWorkItem *subscriptionCallback() { return nullptr; }
+
+	virtual bool getLatestSampleTimestamp(hrt_abstime &t) const { return false; }
+
+	/**
+	 * Check whether the output (motor) is configured to be reversible
+	 */
+	virtual bool reversible(OutputFunction func) const { return false; }
+};
diff --git a/src/lib/mixer_module/functions/FunctionServos.hpp b/src/lib/mixer_module/functions/FunctionServos.hpp
new file mode 100644
index 0000000000..5b69d326d8
--- /dev/null
+++ b/src/lib/mixer_module/functions/FunctionServos.hpp
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2021-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 "FunctionProviderBase.hpp"
+
+#include <uORB/topics/actuator_servos.h>
+
+/**
+ * Functions: Servo1 ... ServoMax
+ */
+class FunctionServos : public FunctionProviderBase
+{
+public:
+	static_assert(actuator_servos_s::NUM_CONTROLS == (int)OutputFunction::ServoMax - (int)OutputFunction::Servo1 + 1,
+		      "Unexpected num servos");
+
+	FunctionServos(const Context &context) :
+		_topic(&context.work_item, ORB_ID(actuator_servos))
+	{
+		for (int i = 0; i < actuator_servos_s::NUM_CONTROLS; ++i) {
+			_data.control[i] = NAN;
+		}
+	}
+
+	static FunctionProviderBase *allocate(const Context &context) { return new FunctionServos(context); }
+
+	void update() override { _topic.update(&_data); }
+	float value(OutputFunction func) override { return _data.control[(int)func - (int)OutputFunction::Servo1]; }
+
+	uORB::SubscriptionCallbackWorkItem *subscriptionCallback() override { return &_topic; }
+
+	float defaultFailsafeValue(OutputFunction func) const override { return 0.f; }
+
+private:
+	uORB::SubscriptionCallbackWorkItem _topic;
+	actuator_servos_s _data{};
+};
diff --git a/src/lib/mixer_module/mixer_module.cpp b/src/lib/mixer_module/mixer_module.cpp
index 9ec0064392..dbe4344db0 100644
--- a/src/lib/mixer_module/mixer_module.cpp
+++ b/src/lib/mixer_module/mixer_module.cpp
@@ -62,6 +62,7 @@ static const FunctionProvider all_function_providers[] = {
 	{OutputFunction::Offboard_Actuator_Set1, OutputFunction::Offboard_Actuator_Set6, &FunctionActuatorSet::allocate},
 	{OutputFunction::Landing_Gear, &FunctionLandingGear::allocate},
 	{OutputFunction::Parachute, &FunctionParachute::allocate},
+	{OutputFunction::Gripper, &FunctionGripper::allocate},
 	{OutputFunction::RC_Roll, OutputFunction::RC_AUXMax, &FunctionManualRC::allocate},
 	{OutputFunction::Gimbal_Roll, OutputFunction::Gimbal_Yaw, &FunctionGimbal::allocate},
 };
diff --git a/src/lib/mixer_module/mixer_module.hpp b/src/lib/mixer_module/mixer_module.hpp
index b717855577..abbc7a19c8 100644
--- a/src/lib/mixer_module/mixer_module.hpp
+++ b/src/lib/mixer_module/mixer_module.hpp
@@ -34,7 +34,17 @@
 #pragma once
 
 #include "actuator_test.hpp"
-#include "functions.hpp"
+
+#include "functions/FunctionActuatorSet.hpp"
+#include "functions/FunctionConstantMax.hpp"
+#include "functions/FunctionConstantMin.hpp"
+#include "functions/FunctionGimbal.hpp"
+#include "functions/FunctionGripper.hpp"
+#include "functions/FunctionLandingGear.hpp"
+#include "functions/FunctionManualRC.hpp"
+#include "functions/FunctionMotors.hpp"
+#include "functions/FunctionParachute.hpp"
+#include "functions/FunctionServos.hpp"
 
 #include <board_config.h>
 #include <drivers/drv_pwm_output.h>
diff --git a/src/lib/mixer_module/output_functions.yaml b/src/lib/mixer_module/output_functions.yaml
index 223ff0241f..507ee5cf1f 100644
--- a/src/lib/mixer_module/output_functions.yaml
+++ b/src/lib/mixer_module/output_functions.yaml
@@ -34,6 +34,8 @@ functions:
     Gimbal_Pitch: 421
     Gimbal_Yaw: 422
 
+    Gripper: 430
+
     # Add your own here:
     #MyCustomFunction: 10000
 
diff --git a/src/lib/motion_planning/CMakeLists.txt b/src/lib/motion_planning/CMakeLists.txt
index f913c495d6..24c4c444d8 100644
--- a/src/lib/motion_planning/CMakeLists.txt
+++ b/src/lib/motion_planning/CMakeLists.txt
@@ -42,3 +42,5 @@ target_include_directories(motion_planning PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 
 px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS motion_planning)
 px4_add_unit_gtest(SRC PositionSmoothingTest.cpp LINKLIBS motion_planning)
+px4_add_unit_gtest(SRC TrajectoryConstraintsTest.cpp)
+px4_add_unit_gtest(SRC ManualVelocitySmoothingXYTest.cpp LINKLIBS FlightTaskUtility)
diff --git a/src/lib/npfg/npfg.cpp b/src/lib/npfg/npfg.cpp
index afb224388b..3f4da2df46 100644
--- a/src/lib/npfg/npfg.cpp
+++ b/src/lib/npfg/npfg.cpp
@@ -574,12 +574,14 @@ void NPFG::navigateWaypoints(const Vector2f &waypoint_A, const Vector2f &waypoin
 } // navigateWaypoints
 
 void NPFG::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle_pos,
-			  float radius, int8_t loiter_direction, const Vector2f &ground_vel, const Vector2f &wind_vel)
+			  float radius, bool loiter_direction_counter_clockwise, const Vector2f &ground_vel, const Vector2f &wind_vel)
 {
 	path_type_loiter_ = true;
 
 	radius = math::max(radius, MIN_RADIUS);
 
+	const float loiter_direction_multiplier = loiter_direction_counter_clockwise ? -1.f : 1.f;
+
 	Vector2f vector_center_to_vehicle = vehicle_pos - loiter_center;
 	const float dist_to_center = vector_center_to_vehicle.norm();
 
@@ -605,12 +607,12 @@ void NPFG::navigateLoiter(const Vector2f &loiter_center, const Vector2f &vehicle
 	}
 
 	// 90 deg clockwise rotation * loiter direction
-	unit_path_tangent_ = float(loiter_direction) * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
+	unit_path_tangent_ = loiter_direction_multiplier * Vector2f{-unit_vec_center_to_closest_pt(1), unit_vec_center_to_closest_pt(0)};
 
 	// positive in direction of path normal
-	signed_track_error_ = -loiter_direction * (dist_to_center - radius);
+	signed_track_error_ = -loiter_direction_multiplier * (dist_to_center - radius);
 
-	float path_curvature = float(loiter_direction) / radius;
+	float path_curvature = loiter_direction_multiplier / radius;
 
 	guideToPath(ground_vel, wind_vel, unit_path_tangent_, signed_track_error_, path_curvature);
 
diff --git a/src/lib/npfg/npfg.hpp b/src/lib/npfg/npfg.hpp
index 2e75164dc5..4d4a5a2772 100644
--- a/src/lib/npfg/npfg.hpp
+++ b/src/lib/npfg/npfg.hpp
@@ -240,12 +240,12 @@ public:
 	 * @param[in] loiter_center The position of the center of the loiter circle [m]
 	 * @param[in] vehicle_pos Vehicle position in WGS84 coordinates (lat,lon) [deg]
 	 * @param[in] radius Loiter radius [m]
-	 * @param[in] loiter_direction Loiter direction: -1=counter-clockwise, 1=clockwise
+	 * @param[in] loiter_direction_counter_clockwise Specifies loiter direction
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 */
 	void navigateLoiter(const matrix::Vector2f &loiter_center, const matrix::Vector2f &vehicle_pos,
-			    float radius, int8_t loiter_direction, const matrix::Vector2f &ground_vel,
+			    float radius, bool loiter_direction_counter_clockwise, const matrix::Vector2f &ground_vel,
 			    const matrix::Vector2f &wind_vel);
 
 	/*
diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp
index c8bf4c73e2..11e069eff3 100644
--- a/src/lib/parameters/param_translation.cpp
+++ b/src/lib/parameters/param_translation.cpp
@@ -212,5 +212,64 @@ bool param_modify_on_import(bson_node_t node)
 		}
 	}
 
+	// 2022-06-09: migrate EKF2_WIND_NOISE->EKF2_WIND_NSD
+	{
+		if (strcmp("EKF2_WIND_NOISE", node->name) == 0) {
+			node->d /= 10.0; // at 100Hz (EKF2 rate), NSD is sqrt(100) times smaller than std_dev
+			strcpy(node->name, "EKF2_WIND_NSD");
+			PX4_INFO("param migrating EKF2_WIND_NOISE (removed) -> EKF2_WIND_NSD: value=%.3f", node->d);
+			return true;
+		}
+	}
+
+	// 2022-06-09: translate ASPD_SC_P_NOISE->ASPD_SCALE_NSD and ASPD_W_P_NOISE->ASPD_WIND_NSD
+	{
+		if (strcmp("ASPD_SC_P_NOISE", node->name) == 0) {
+			strcpy(node->name, "ASPD_SCALE_NSD");
+			PX4_INFO("copying %s -> %s", "ASPD_SC_P_NOISE", "ASPD_SCALE_NSD");
+			return true;
+		}
+
+		if (strcmp("ASPD_W_P_NOISE", node->name) == 0) {
+			strcpy(node->name, "ASPD_WIND_NSD");
+			PX4_INFO("copying %s -> %s", "ASPD_W_P_NOISE", "ASPD_WIND_NSD");
+			return true;
+		}
+	}
+
+	// 2022-07-07: translate FW_THR_CRUISE->FW_THR_TRIM
+	{
+		if (strcmp("FW_THR_CRUISE", node->name) == 0) {
+			strcpy(node->name, "FW_THR_TRIM");
+			PX4_INFO("copying %s -> %s", "FW_THR_CRUISE", "FW_THR_TRIM");
+			return true;
+		}
+	}
+
+	// 2022-07-08: (Auterion only) migrate MPC_VEL_LAT_SC->MPC_VEL_MAN_SIDE and MPC_VEL_BACK_SC->MPC_VEL_MAN_BACK
+	{
+		if (strcmp("MPC_VEL_LAT_SC", node->name) == 0) {
+			if (node->d < 1.0) {
+				float vel_manual;
+				param_get(param_find("MPC_VEL_MANUAL"), &vel_manual);
+				node->d *= static_cast<double>(vel_manual); // convert the scaler relative to vel_manual to an absolute velocity
+				strcpy(node->name, "MPC_VEL_MAN_SIDE");
+				PX4_INFO("param migrating MPC_VEL_LAT_SC (removed) -> MPC_VEL_MAN_SIDE: value=%.3f", node->d);
+				return true;
+			}
+		}
+
+		if (strcmp("MPC_VEL_BACK_SC", node->name) == 0) {
+			if (node->d < 1.0) {
+				float vel_manual;
+				param_get(param_find("MPC_VEL_MANUAL"), &vel_manual);
+				node->d *= static_cast<double>(vel_manual); // convert the scaler relative to vel_manual to an absolute velocity
+				strcpy(node->name, "MPC_VEL_MAN_BACK");
+				PX4_INFO("param migrating MPC_VEL_BACK_SC (removed) -> MPC_VEL_MAN_BACK: value=%.3f", node->d);
+				return true;
+			}
+		}
+	}
+
 	return false;
 }
diff --git a/src/lib/parameters/px4params/srcparser.py b/src/lib/parameters/px4params/srcparser.py
index 922d53e38c..d3fb119757 100644
--- a/src/lib/parameters/px4params/srcparser.py
+++ b/src/lib/parameters/px4params/srcparser.py
@@ -358,7 +358,7 @@ class SourceParser(object):
                                 'deg', 'deg*1e7', 'deg/s',
                                 'celcius', 'gauss', 'gauss/s', 'gauss^2',
                                 'hPa', 'kg', 'kg/m^2', 'kg m^2',
-                                'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad',
+                                'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad',
                                 'Ohm', 'V', 'A',
                                 'us', 'ms', 's',
                                 'S', 'A/%', '(m/s^2)^2', 'm/m',  'tan(rad)^2', '(m/s)^2', 'm/rad',
diff --git a/src/lib/rc/sbus.cpp b/src/lib/rc/sbus.cpp
index 3a1f992ecf..04fd38d8fc 100644
--- a/src/lib/rc/sbus.cpp
+++ b/src/lib/rc/sbus.cpp
@@ -299,7 +299,7 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails
 	 * provides a degree of protection. Of course, it would be better
 	 * if we didn't drop bytes...
 	 */
-	if (now - last_rx_time > 3_ms) {
+	if (now - last_rx_time > 4_ms) {
 		if (partial_frame_count > 0) {
 			partial_frame_count = 0;
 			sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
@@ -309,13 +309,6 @@ sbus_input(int sbus_fd, uint16_t *values, uint16_t *num_values, bool *sbus_fails
 		}
 	}
 
-	if (partial_frame_count == 0 && buf[0] != SBUS_START_SYMBOL) {
-		/* don't bother going through the buffer if we don't get the
-		 * expected start symbol as a first byte */
-		sbus_decode_state = SBUS2_DECODE_STATE_DESYNC;
-		return false;
-	}
-
 #endif /* __PX4_NUTTX */
 
 	/*
@@ -372,17 +365,18 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
 		switch (sbus_decode_state) {
 		case SBUS2_DECODE_STATE_DESYNC:
 
+		/* fall through */
+		case SBUS2_DECODE_STATE_SBUS_START:
+
 			/* we are de-synced and only interested in the frame marker */
 			if (frame[d] == SBUS_START_SYMBOL) {
-				sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START;
+				sbus_decode_state = SBUS2_DECODE_STATE_SBUS2_SYNC;
 				partial_frame_count = 0;
 				sbus_frame[partial_frame_count++] = frame[d];
 			}
 
 			break;
 
-		/* fall through */
-		case SBUS2_DECODE_STATE_SBUS_START:
 		case SBUS2_DECODE_STATE_SBUS1_SYNC:
 
 		/* fall through */
@@ -438,6 +432,10 @@ sbus_parse(uint64_t now, uint8_t *frame, unsigned len, uint16_t *values,
 					partial_frame_count = 0;
 				}
 
+				if (decode_ret) {
+					sbus_decode_state = SBUS2_DECODE_STATE_SBUS_START;
+				}
+
 			}
 			break;
 
diff --git a/src/lib/systemlib/mavlink_log.h b/src/lib/systemlib/mavlink_log.h
index bc1ba32965..d944b6bc34 100644
--- a/src/lib/systemlib/mavlink_log.h
+++ b/src/lib/systemlib/mavlink_log.h
@@ -120,3 +120,15 @@ __EXPORT void mavlink_vasprintf(int severity, orb_advert_t *mavlink_log_pub, con
 		mavlink_vasprintf(_MSG_PRIO_CRITICAL, _pub, _text, ##__VA_ARGS__); \
 		PX4_WARN(_text, ##__VA_ARGS__); \
 	} while(0);
+
+/**
+ * TEMPORARY: Send a mavlink critical message and print to console as info.
+ * This can be removed after ther migration to events interface is fully in place.
+ * @param _pub		Pointer to the uORB advert;
+ * @param _text		The text to log;
+ */
+#define mavlink_log_gcs_critical(_pub, _text, ...) \
+	do { \
+		mavlink_vasprintf(_MSG_PRIO_CRITICAL, _pub, _text, ##__VA_ARGS__); \
+		PX4_INFO(_text, ##__VA_ARGS__); \
+	} while(0);
diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp
index e33250e226..ce0d9f0b8e 100644
--- a/src/lib/tecs/TECS.cpp
+++ b/src/lib/tecs/TECS.cpp
@@ -56,7 +56,7 @@ static constexpr float DT_MAX = 1.0f;	///< max value of _dt allowed before a fil
  * which is used by the airspeed complimentary filter.
  */
 void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward,
-		bool altitude_lock, bool in_air, float altitude, float vz)
+		bool altitude_lock, float altitude, float vz)
 {
 	// calculate the time lapsed since the last update
 	uint64_t now = hrt_absolute_time();
@@ -69,7 +69,7 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float
 		reset_altitude = true;
 	}
 
-	if (!altitude_lock || !in_air) {
+	if (!altitude_lock) {
 		reset_altitude = true;
 	}
 
@@ -80,8 +80,6 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float
 	_state_update_timestamp = now;
 	_EAS = equivalent_airspeed;
 
-	_in_air = in_air;
-
 	// Set the velocity and position state to the the INS data
 	_vert_vel_state = -vz;
 	_vert_pos_state = altitude;
@@ -97,11 +95,6 @@ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float
 		_tas_rate_raw = 0.0f;
 		_tas_rate_filtered = 0.0f;
 	}
-
-	if (!_in_air) {
-		_states_initialized = false;
-	}
-
 }
 
 void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equivalent_airspeed, float EAS2TAS)
@@ -111,21 +104,21 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
 	const float dt = constrain((now - _speed_update_timestamp) * 1.0e-6f, DT_MIN, DT_MAX);
 
 	// Convert equivalent airspeed quantities to true airspeed
-	_EAS_setpoint = equivalent_airspeed_setpoint;
+	_EAS_setpoint = PX4_ISFINITE(equivalent_airspeed_setpoint) ? equivalent_airspeed_setpoint : _equivalent_airspeed_trim;
 	_TAS_setpoint  = _EAS_setpoint * EAS2TAS;
 	_TAS_max   = _equivalent_airspeed_max * EAS2TAS;
 	_TAS_min   = _equivalent_airspeed_min * EAS2TAS;
 
-	// If airspeed measurements are not being used, fix the airspeed estimate to the nominal cruise airspeed
+	// If airspeed measurements are not being used, fix the airspeed estimate to the nominal trim airspeed
 	if (!PX4_ISFINITE(equivalent_airspeed) || !airspeed_sensor_enabled()) {
-		_EAS = _equivalent_airspeed_cruise;
+		_EAS = _equivalent_airspeed_trim;
 
 	} else {
 		_EAS = equivalent_airspeed;
 	}
 
-	// If first time through or not flying, reset airspeed states
-	if (_speed_update_timestamp == 0 || !_in_air) {
+	// If first time through reset airspeed states
+	if (_speed_update_timestamp == 0) {
 		_tas_rate_state = 0.0f;
 		_tas_state = (_EAS * EAS2TAS);
 	}
@@ -136,18 +129,21 @@ void TECS::_update_speed_states(float equivalent_airspeed_setpoint, float equiva
 	_tas_innov = (_EAS * EAS2TAS) - _tas_state;
 	float tas_rate_state_input = _tas_innov * _tas_estimate_freq * _tas_estimate_freq;
 
-	// limit integrator input to prevent windup
-	if (_tas_state < 3.1f) {
-		tas_rate_state_input = max(tas_rate_state_input, 0.0f);
-	}
-
 	// Update TAS state
 	_tas_rate_state = _tas_rate_state + tas_rate_state_input * dt;
 	float tas_state_input = _tas_rate_state + _tas_rate_raw + _tas_innov * _tas_estimate_freq * 1.4142f;
-	_tas_state = _tas_state + tas_state_input * dt;
+	const float new_tas_state = _tas_state + tas_state_input * dt;
+
+	if (new_tas_state < 0.0f) {
+		// clip TAS at zero, back calculate rate
+		tas_state_input = -_tas_state / dt;
+		_tas_rate_state = tas_state_input - _tas_rate_raw - _tas_innov * _tas_estimate_freq * 1.4142f;
+		_tas_state = 0.0f;
+
+	} else {
+		_tas_state = new_tas_state;
+	}
 
-	// Limit the TAS state to a minimum of 3 m/s
-	_tas_state = max(_tas_state, 3.0f);
 	_speed_update_timestamp = now;
 
 }
@@ -156,21 +152,36 @@ void TECS::_update_speed_setpoint()
 {
 	// Set the TAS demand to the minimum value if an underspeed or
 	// or a uncontrolled descent condition exists to maximise climb rate
-	if ((_uncommanded_descent_recovery) || (_underspeed_detected)) {
+	if (_uncommanded_descent_recovery) {
 		_TAS_setpoint = _TAS_min;
+
+	} else if (_percent_undersped > FLT_EPSILON) {
+		// TAS setpoint is reset from external setpoint every time tecs is called, so the interpolation is still
+		// between current setpoint and mininimum airspeed here (it's not feeding the newly adjusted setpoint
+		// from this line back into this method each time).
+		// TODO: WOULD BE GOOD to "functionalize" this library a bit and remove many of these internal states to
+		// avoid the fear of side effects in simple operations like this.
+		_TAS_setpoint = _TAS_min * _percent_undersped + (1.0f - _percent_undersped) * _TAS_setpoint;
 	}
 
 	_TAS_setpoint = constrain(_TAS_setpoint, _TAS_min, _TAS_max);
 
 	// Calculate limits for the demanded rate of change of speed based on physical performance limits
 	// with a 50% margin to allow the total energy controller to correct for errors.
-	float velRateMax = 0.5f * _STE_rate_max / _tas_state;
-	float velRateMin = 0.5f * _STE_rate_min / _tas_state;
+	const float max_tas_rate_sp = 0.5f * _STE_rate_max / math::max(_tas_state, FLT_EPSILON);
+	const float min_tas_rate_sp = 0.5f * _STE_rate_min / math::max(_tas_state, FLT_EPSILON);
 
 	_TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max);
 
 	// calculate the demanded true airspeed rate of change based on first order response of true airspeed error
-	_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, velRateMin, velRateMax);
+	// if airspeed measurement is not enabled then always set the rate setpoint to zero in order to avoid constant rate setpoints
+	if (airspeed_sensor_enabled()) {
+		_TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, min_tas_rate_sp,
+					       max_tas_rate_sp);
+
+	} else {
+		_TAS_rate_setpoint = 0.0f;
+	}
 
 }
 
@@ -201,18 +212,24 @@ void TECS::runAltitudeControllerSmoothVelocity(float alt_sp_amsl_m, float target
 void TECS::_detect_underspeed()
 {
 	if (!_detect_underspeed_enabled) {
-		_underspeed_detected = false;
+		_percent_undersped = 0.0f;
 		return;
 	}
 
-	if (((_tas_state < _TAS_min * 0.9f) && (_last_throttle_setpoint >= _throttle_setpoint_max * 0.95f))
-	    || ((_vert_pos_state < _hgt_setpoint) && _underspeed_detected)) {
+	// this is the expected (something like standard) deviation from the airspeed setpoint that we allow the airspeed
+	// to vary in before ramping in underspeed mitigation
+	const float tas_error_bound = kTASErrorPercentage * _equivalent_airspeed_trim;
 
-		_underspeed_detected = true;
+	// this is the soft boundary where underspeed mitigation is ramped in
+	// NOTE: it's currently the same as the error bound, but separated here to indicate these values do not in general
+	// need to be the same
+	const float tas_underspeed_soft_bound = kTASErrorPercentage * _equivalent_airspeed_trim;
 
-	} else {
-		_underspeed_detected = false;
-	}
+	const float tas_fully_undersped = math::max(_TAS_min - tas_error_bound - tas_underspeed_soft_bound, 0.0f);
+	const float tas_starting_to_underspeed = math::max(_TAS_min - tas_error_bound, tas_fully_undersped);
+
+	_percent_undersped = 1.0f - math::constrain((_tas_state - tas_fully_undersped) /
+			     math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f);
 }
 
 void TECS::_update_energy_estimates()
@@ -242,99 +259,95 @@ void TECS::_update_energy_estimates()
 	_SKE_rate = _tas_state * _tas_rate_filtered;// kinetic energy rate of change
 }
 
-void TECS::_update_throttle_setpoint(const float throttle_cruise)
+void TECS::_update_throttle_setpoint()
 {
 	// Calculate demanded rate of change of total energy, respecting vehicle limits.
 	// We will constrain the value below.
-	float STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint;
+	_STE_rate_setpoint = _SPE_rate_setpoint + _SKE_rate_setpoint;
 
 	// Calculate the total energy rate error, applying a first order IIR filter
 	// to reduce the effect of accelerometer noise
 	_STE_rate_error_filter.update(-_SPE_rate - _SKE_rate + _SPE_rate_setpoint + _SKE_rate_setpoint);
 	_STE_rate_error = _STE_rate_error_filter.getState();
 
-	float throttle_setpoint;
+	// Adjust the demanded total energy rate to compensate for induced drag rise in turns.
+	// Assume induced drag scales linearly with normal load factor.
+	// The additional normal load factor is given by (1/cos(bank angle) - 1)
+	_STE_rate_setpoint += _load_factor_correction * (_load_factor - 1.f);
 
-	// Calculate the throttle demand
-	if (_underspeed_detected) {
-		// always use full throttle to recover from an underspeed condition
-		throttle_setpoint = _throttle_setpoint_max;
+	_STE_rate_setpoint = constrain(_STE_rate_setpoint, _STE_rate_min, _STE_rate_max);
+
+	// Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle
+	// as the starting point. Assume:
+	// Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max
+	// Specific total energy rate = 0 at cruise throttle
+	// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
+	float throttle_predicted = 0.0f;
+
+	if (_STE_rate_setpoint >= 0) {
+		// throttle is between trim and maximum
+		throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max - _throttle_trim);
 
 	} else {
-		// Adjust the demanded total energy rate to compensate for induced drag rise in turns.
-		// Assume induced drag scales linearly with normal load factor.
-		// The additional normal load factor is given by (1/cos(bank angle) - 1)
-		STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (_load_factor - 1.f);
+		// throttle is between trim and minimum
+		throttle_predicted = _throttle_trim + _STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min - _throttle_trim);
 
-		STE_rate_setpoint = constrain(STE_rate_setpoint, _STE_rate_min, _STE_rate_max);
+	}
 
-		// Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle
-		// as the starting point. Assume:
-		// Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max
-		// Specific total energy rate = 0 at cruise throttle
-		// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
-		float throttle_predicted = 0.0f;
+	// Calculate gain scaler from specific energy rate error to throttle
+	const float STE_rate_to_throttle = 1.0f / (_STE_rate_max - _STE_rate_min);
 
-		if (STE_rate_setpoint >= 0) {
-			// throttle is between cruise and maximum
-			throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_max * (_throttle_setpoint_max - throttle_cruise);
+	// Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits
+	float throttle_setpoint = (_STE_rate_error * _throttle_damping_gain) * STE_rate_to_throttle + throttle_predicted;
+	throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_limit_max);
 
-		} else {
-			// throttle is between cruise and minimum
-			throttle_predicted = throttle_cruise + STE_rate_setpoint / _STE_rate_min * (_throttle_setpoint_min - throttle_cruise);
+	// Integral handling
+	if (airspeed_sensor_enabled()) {
+		if (_integrator_gain_throttle > 0.0f) {
+			float integ_state_max = _throttle_limit_max - throttle_setpoint;
+			float integ_state_min = _throttle_setpoint_min - throttle_setpoint;
 
-		}
+			// underspeed conditions zero out integration
+			float throttle_integ_input = (_STE_rate_error * _integrator_gain_throttle) * _dt *
+						     STE_rate_to_throttle * (1.0f - _percent_undersped);
 
-		// Calculate gain scaler from specific energy rate error to throttle
-		const float STE_rate_to_throttle = 1.0f / (_STE_rate_max - _STE_rate_min);
+			// only allow integrator propagation into direction which unsaturates throttle
+			if (_throttle_integ_state > integ_state_max) {
+				throttle_integ_input = math::min(0.f, throttle_integ_input);
 
-		// Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits
-		throttle_setpoint = (_STE_rate_error * _throttle_damping_gain) * STE_rate_to_throttle + throttle_predicted;
-		throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
-
-		if (airspeed_sensor_enabled()) {
-			if (_integrator_gain_throttle > 0.0f) {
-				float integ_state_max = _throttle_setpoint_max - throttle_setpoint;
-				float integ_state_min = _throttle_setpoint_min - throttle_setpoint;
-
-				float throttle_integ_input = (_STE_rate_error * _integrator_gain_throttle) * _dt *
-							     STE_rate_to_throttle;
-
-				// only allow integrator propagation into direction which unsaturates throttle
-				if (_throttle_integ_state > integ_state_max) {
-					throttle_integ_input = math::min(0.f, throttle_integ_input);
-
-				} else if (_throttle_integ_state < integ_state_min) {
-					throttle_integ_input = math::max(0.f, throttle_integ_input);
-				}
-
-				// Calculate a throttle demand from the integrated total energy rate error
-				// This will be added to the total throttle demand to compensate for steady state errors
-				_throttle_integ_state = _throttle_integ_state + throttle_integ_input;
-
-				if (_climbout_mode_active) {
-					// During climbout, set the integrator to maximum throttle to prevent transient throttle drop
-					// at end of climbout when we transition to closed loop throttle control
-					_throttle_integ_state = integ_state_max;
-				}
-
-			} else {
-				_throttle_integ_state = 0.0f;
+			} else if (_throttle_integ_state < integ_state_min) {
+				throttle_integ_input = math::max(0.f, throttle_integ_input);
 			}
 
-		}
+			// Calculate a throttle demand from the integrated total energy rate error
+			// This will be added to the total throttle demand to compensate for steady state errors
+			_throttle_integ_state = _throttle_integ_state + throttle_integ_input;
 
-		if (airspeed_sensor_enabled()) {
-			// Add the integrator feedback during closed loop operation with an airspeed sensor
-			throttle_setpoint += _throttle_integ_state;
+			if (_climbout_mode_active) {
+				// During climbout, set the integrator to maximum throttle to prevent transient throttle drop
+				// at end of climbout when we transition to closed loop throttle control
+				_throttle_integ_state = integ_state_max;
+			}
 
 		} else {
-			// when flying without an airspeed sensor, use the predicted throttle only
-			throttle_setpoint = throttle_predicted;
-
+			_throttle_integ_state = 0.0f;
 		}
+
 	}
 
+	if (airspeed_sensor_enabled()) {
+		// Add the integrator feedback during closed loop operation with an airspeed sensor
+		throttle_setpoint += _throttle_integ_state;
+
+	} else {
+		// when flying without an airspeed sensor, use the predicted throttle only
+		throttle_setpoint = throttle_predicted;
+
+	}
+
+	// ramp in max throttle setting with underspeediness value
+	throttle_setpoint = _percent_undersped * _throttle_setpoint_max + (1.0f - _percent_undersped) * throttle_setpoint;
+
 	// Rate limit the throttle demand
 	if (fabsf(_throttle_slewrate) > 0.01f) {
 		const float throttle_increment_limit = _dt * (_throttle_setpoint_max - _throttle_setpoint_min) * _throttle_slewrate;
@@ -342,7 +355,7 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise)
 					      _last_throttle_setpoint + throttle_increment_limit);
 	}
 
-	_last_throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max);
+	_last_throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_limit_max);
 }
 
 void TECS::_detect_uncommanded_descent()
@@ -356,13 +369,15 @@ void TECS::_detect_uncommanded_descent()
 	// Calculate rate of change of total specific energy
 	const float STE_rate = _SPE_rate + _SKE_rate;
 
+	const bool underspeed_detected = _percent_undersped > FLT_EPSILON;
+
 	// If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode
-	const bool enter_mode = !_uncommanded_descent_recovery && !_underspeed_detected && (_STE_error > 200.0f)
+	const bool enter_mode = !_uncommanded_descent_recovery && !underspeed_detected && (_STE_error > 200.0f)
 				&& (STE_rate < 0.0f)
 				&& (_last_throttle_setpoint >= _throttle_setpoint_max * 0.9f);
 
 	// If we enter an underspeed condition or recover the required total energy, then exit uncommanded descent recovery mode
-	const bool exit_mode = _uncommanded_descent_recovery && (_underspeed_detected || (_STE_error < 0.0f));
+	const bool exit_mode = _uncommanded_descent_recovery && (underspeed_detected || (_STE_error < 0.0f));
 
 	if (enter_mode) {
 		_uncommanded_descent_recovery = true;
@@ -434,9 +449,10 @@ void TECS::_update_pitch_setpoint()
 	float pitch_setpoint = constrain(_pitch_setpoint_unc, _pitch_setpoint_min, _pitch_setpoint_max);
 
 	// Comply with the specified vertical acceleration limit by applying a pitch rate limit
-	const float ptchRateIncr = _dt * _vert_accel_limit / _tas_state;
-	_last_pitch_setpoint = constrain(pitch_setpoint, _last_pitch_setpoint - ptchRateIncr,
-					 _last_pitch_setpoint + ptchRateIncr);
+	// NOTE: at zero airspeed, the pitch increment is unbounded
+	const float pitch_increment = _dt * _vert_accel_limit / _tas_state;
+	_last_pitch_setpoint = constrain(pitch_setpoint, _last_pitch_setpoint - pitch_increment,
+					 _last_pitch_setpoint + pitch_increment);
 }
 
 void TECS::_updateTrajectoryGenerationConstraints()
@@ -452,6 +468,38 @@ void TECS::_updateTrajectoryGenerationConstraints()
 	_velocity_control_traj_generator.setMaxVelDown(_max_climb_rate); // different convention for FW than for MC
 }
 
+void TECS::_updateFlightPhase(float altitude_sp_amsl, float height_rate_setpoint)
+{
+	const bool input_is_height_rate = PX4_ISFINITE(height_rate_setpoint);
+
+	// update flight phase state (are we flying level, climbing or descending)
+	if (input_is_height_rate) {
+
+		if (PX4_ISFINITE(_velocity_control_traj_generator.getCurrentPosition())) {
+			// we have a valid altitude setpoint which means that we are flying level
+			_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_LEVEL;
+
+		} else if (_velocity_control_traj_generator.getCurrentVelocity() > FLT_EPSILON) {
+			_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_CLIMB;
+
+		} else {
+			_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_DESCEND;
+		}
+
+	} else {
+		// stay in flight phase level if only small altitude changes are demanded
+		if (altitude_sp_amsl - _alt_control_traj_generator.getCurrentPosition() > _fw_eco_alt_err_o) {
+			_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_CLIMB;
+
+		} else if (altitude_sp_amsl - _alt_control_traj_generator.getCurrentPosition() < -_fw_eco_alt_err_u) {
+			_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_DESCEND;
+
+		} else {
+			_flight_phase = tecs_status_s::TECS_FLIGHT_PHASE_LEVEL;
+		}
+	}
+}
+
 void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate,
 					float target_sinkrate, float altitude_amsl)
 {
@@ -482,35 +530,32 @@ void TECS::_calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rat
 	}
 }
 
-void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_altitude, float pitch_min_climbout,
+void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altitude, float pitch_min_climbout,
 			      float EAS2TAS)
 {
-	if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_in_air || !_states_initialized) {
+	if (_pitch_update_timestamp == 0 || _dt > DT_MAX || !_states_initialized) {
 		// On first time through or when not using TECS of if there has been a large time slip,
 		// states must be reset to allow filters to a clean start
 		_vert_vel_state = 0.0f;
 		_vert_pos_state = baro_altitude;
 		_tas_rate_state = 0.0f;
 		_tas_state = _EAS * EAS2TAS;
-		_throttle_integ_state =  0.0f;
-		_pitch_integ_state = 0.0f;
-		_last_throttle_setpoint = (_in_air ? throttle_cruise : 0.0f);;
+		_last_throttle_setpoint = throttle_trim;
 		_last_pitch_setpoint = constrain(pitch, _pitch_setpoint_min, _pitch_setpoint_max);
 		_pitch_setpoint_unc = _last_pitch_setpoint;
 		_TAS_setpoint_last = _EAS * EAS2TAS;
 		_TAS_setpoint_adj = _TAS_setpoint_last;
-		_underspeed_detected = false;
 		_uncommanded_descent_recovery = false;
 		_STE_rate_error = 0.0f;
 		_hgt_setpoint = baro_altitude;
 
+		resetIntegrals();
+
 		if (_dt > DT_MAX || _dt < DT_MIN) {
 			_dt = DT_DEFAULT;
 		}
 
-		_alt_control_traj_generator.reset(0, 0, baro_altitude);
-		_velocity_control_traj_generator.reset(0.0f, 0.0f, baro_altitude);
-
+		resetTrajectoryGenerators(baro_altitude);
 
 	} else if (_climbout_mode_active) {
 		// During climbout use the lower pitch angle limit specified by the
@@ -526,8 +571,6 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt
 
 		_hgt_setpoint = baro_altitude;
 
-		// disable speed and decent error condition checks
-		_underspeed_detected = false;
 		_uncommanded_descent_recovery = false;
 	}
 
@@ -545,16 +588,18 @@ void TECS::_initialize_states(float pitch, float throttle_cruise, float baro_alt
 void TECS::_update_STE_rate_lim()
 {
 	// Calculate the specific total energy upper rate limits from the max throttle climb rate
-	_STE_rate_max = _max_climb_rate * CONSTANTS_ONE_G;
+	_STE_rate_max = math::max(_max_climb_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
 
 	// Calculate the specific total energy lower rate limits from the min throttle sink rate
-	_STE_rate_min = - _min_sink_rate * CONSTANTS_ONE_G;
+	_STE_rate_min = - math::max(_min_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
 }
 
 void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
 				 float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
-				 float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max,
-				 float target_climbrate, float target_sinkrate, float hgt_rate_sp)
+				 float throttle_min, float throttle_max, float throttle_trim,
+				 float throttle_lim_max,
+				 float pitch_limit_min, float pitch_limit_max,
+				 float target_climbrate, float target_sinkrate, float hgt_rate_sp, bool eco_mode_enabled)
 {
 	// Calculate the time since last update (seconds)
 	uint64_t now = hrt_absolute_time();
@@ -566,14 +611,11 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	_pitch_setpoint_max = pitch_limit_max;
 	_pitch_setpoint_min = pitch_limit_min;
 	_climbout_mode_active = climb_out_setpoint;
+	_throttle_trim = throttle_trim;
+	_throttle_limit_max = throttle_lim_max; // APX4 custom
 
 	// Initialize selected states and variables as required
-	_initialize_states(pitch, throttle_cruise, baro_altitude, pitch_min_climbout, eas_to_tas);
-
-	// Don't run TECS control algorithms when not in flight
-	if (!_in_air) {
-		return;
-	}
+	_initialize_states(pitch, throttle_trim, baro_altitude, pitch_min_climbout, eas_to_tas);
 
 	_updateTrajectoryGenerationConstraints();
 
@@ -591,16 +633,20 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	// Detect an uncommanded descent caused by an unachievable airspeed demand
 	_detect_uncommanded_descent();
 
+	_eco_mode_enabled = eco_mode_enabled;
+
 	// Calculate the demanded true airspeed
 	_update_speed_setpoint();
 
+	_updateFlightPhase(hgt_setpoint, hgt_rate_sp);
+
 	_calculateHeightRateSetpoint(hgt_setpoint, hgt_rate_sp, target_climbrate, target_sinkrate, baro_altitude);
 
 	// Calculate the specific energy values required by the control loop
 	_update_energy_estimates();
 
 	// Calculate the throttle demand
-	_update_throttle_setpoint(throttle_cruise);
+	_update_throttle_setpoint();
 
 	// Calculate the pitch demand
 	_update_pitch_setpoint();
@@ -609,7 +655,7 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	_pitch_update_timestamp = now;
 
 	// Set TECS mode for next frame
-	if (_underspeed_detected) {
+	if (_percent_undersped > FLT_EPSILON) {
 		_tecs_mode = ECL_TECS_MODE_UNDERSPEED;
 
 	} else if (_uncommanded_descent_recovery) {
@@ -618,6 +664,9 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 	} else if (_climbout_mode_active) {
 		_tecs_mode = ECL_TECS_MODE_CLIMBOUT;
 
+	} else if (_eco_mode_enabled) {
+		_tecs_mode = ECL_TECS_MODE_ECO;
+
 	} else {
 		// This is the default operation mode
 		_tecs_mode = ECL_TECS_MODE_NORMAL;
@@ -627,18 +676,27 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
 
 void TECS::_update_speed_height_weights()
 {
-	// Calculate the weight applied to control of specific kinetic energy error
-	_SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f);
+	if (_eco_mode_enabled) {
+		_pitch_speed_weight = _pitch_speed_weight_eco;
+		_height_error_gain = _height_error_gain_eco;
+	}
 
-	if ((_underspeed_detected || _climbout_mode_active) && airspeed_sensor_enabled()) {
-		_SKE_weighting = 2.0f;
+	// Calculate the weight applied to control of specific kinetic energy error
+	float pitch_speed_weight = constrain(_pitch_speed_weight, 0.0f, 2.0f);
+
+	if (_climbout_mode_active && airspeed_sensor_enabled()) {
+		pitch_speed_weight = 2.0f;
+
+	} else if (_percent_undersped > FLT_EPSILON && airspeed_sensor_enabled()) {
+		pitch_speed_weight = 2.0f * _percent_undersped + (1.0f - _percent_undersped) * pitch_speed_weight;
 
 	} else if (!airspeed_sensor_enabled()) {
-		_SKE_weighting = 0.0f;
+		pitch_speed_weight = 0.0f;
+
 	}
 
 	// don't allow any weight to be larger than one, as it has the same effect as reducing the control
 	// loop time constant and therefore can lead to a destabilization of that control loop
-	_SPE_weighting = constrain(2.0f - _SKE_weighting, 0.f, 1.f);
-	_SKE_weighting = constrain(_SKE_weighting, 0.f, 1.f);
+	_SPE_weighting = constrain(2.0f - pitch_speed_weight, 0.f, 1.f);
+	_SKE_weighting = constrain(pitch_speed_weight, 0.f, 1.f);
 }
diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp
index f95f483423..78ff170c17 100644
--- a/src/lib/tecs/TECS.hpp
+++ b/src/lib/tecs/TECS.hpp
@@ -81,7 +81,6 @@ public:
 	 * Must be called at 50Hz or greater
 	 */
 	void update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward, bool altitude_lock,
-					    bool in_air,
 					    float altitude, float vz);
 
 	/**
@@ -89,20 +88,41 @@ public:
 	 */
 	void update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
 				   float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
-				   float throttle_min, float throttle_setpoint_max, float throttle_cruise,
-				   float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN);
+				   float throttle_min, float throttle_setpoint_max, float throttle_trim,
+				   float throttle_lim_max,
+				   float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp,
+				   bool eco_mode_enabled);
 
 	float get_throttle_setpoint() { return _last_throttle_setpoint; }
 	float get_pitch_setpoint() { return _last_pitch_setpoint; }
 	float get_speed_weight() { return _pitch_speed_weight; }
+	float get_throttle_trim() { return _throttle_trim; }
 
 	void reset_state() { _states_initialized = false; }
 
+	void resetIntegrals()
+	{
+		_throttle_integ_state =  0.0f;
+		_pitch_integ_state = 0.0f;
+	}
+
+	/**
+	 * @brief Resets the altitude and height rate control trajectory generators to the input altitude
+	 *
+	 * @param altitude Vehicle altitude (AMSL) [m]
+	 */
+	void resetTrajectoryGenerators(const float altitude)
+	{
+		_alt_control_traj_generator.reset(0, 0, altitude);
+		_velocity_control_traj_generator.reset(0.0f, 0.0f, altitude);
+	}
+
 	enum ECL_TECS_MODE {
 		ECL_TECS_MODE_NORMAL = 0,
 		ECL_TECS_MODE_UNDERSPEED,
 		ECL_TECS_MODE_BAD_DESCENT,
-		ECL_TECS_MODE_CLIMBOUT
+		ECL_TECS_MODE_CLIMBOUT,
+		ECL_TECS_MODE_ECO
 	};
 
 	void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; }
@@ -120,7 +140,7 @@ public:
 
 	void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
 	void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
-	void set_equivalent_airspeed_cruise(float airspeed) { _equivalent_airspeed_cruise = airspeed; }
+	void set_equivalent_airspeed_trim(float airspeed) { _equivalent_airspeed_trim = airspeed; }
 
 	void set_pitch_damping(float damping) { _pitch_damping_gain = damping; }
 	void set_vertical_accel_limit(float limit) { _vert_accel_limit = limit; }
@@ -140,6 +160,11 @@ public:
 
 	void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; }
 
+	// eco mode settings
+	void set_speed_weight_eco(float weight_eco) { _pitch_speed_weight_eco = weight_eco; }
+	void set_height_error_time_constant_eco(float time_const_eco) { _height_error_gain_eco = 1.0f / math::max(time_const_eco, 0.1f); }
+	void set_fw_eco_alt_err_u(float fw_eco_alt_err_u) { _fw_eco_alt_err_u = fw_eco_alt_err_u; }
+	void set_fw_eco_alt_err_o(float fw_eco_alt_err_o) { _fw_eco_alt_err_o = fw_eco_alt_err_o; }
 
 	// TECS status
 	uint64_t timestamp() { return _pitch_update_timestamp; }
@@ -175,7 +200,7 @@ public:
 
 	float STE_rate() { return _SPE_rate + _SKE_rate; }
 
-	float STE_rate_setpoint() { return _SPE_rate_setpoint + _SKE_rate_setpoint; }
+	float STE_rate_setpoint() { return _STE_rate_setpoint; }
 
 	float SEB() { return _SPE_estimate * _SPE_weighting - _SKE_estimate * _SKE_weighting; }
 
@@ -185,6 +210,7 @@ public:
 
 	float SEB_rate_setpoint() { return _SPE_rate_setpoint * _SPE_weighting - _SKE_rate_setpoint * _SKE_weighting; }
 
+	int get_flight_phase() { return _flight_phase; }
 
 	/**
 	 * Handle the altitude reset
@@ -203,8 +229,11 @@ public:
 
 private:
 
+	// [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors
+	static constexpr float kTASErrorPercentage = 0.15;
+
 	static constexpr float _jerk_max =
-		1000.0f;	// maximum jerk for creating height rate trajectories, we want infinite jerk so set a high value
+		1000.0f;
 
 	enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL};
 
@@ -223,7 +252,7 @@ private:
 	float _integrator_gain_throttle{0.0f};				///< integrator gain used by the throttle demand calculation
 	float _integrator_gain_pitch{0.0f};				///< integrator gain used by the pitch demand calculation
 	float _vert_accel_limit{0.0f};					///< magnitude of the maximum vertical acceleration allowed (m/sec**2)
-	float _load_factor{0.0f};					///< additional normal load factor
+	float _load_factor{1.0f};					///< additional normal load factor
 	float _load_factor_correction{0.0f};				///< gain from normal load factor increase to total energy rate demand (m**2/sec**3)
 	float _pitch_speed_weight{1.0f};				///< speed control weighting used by pitch demand calculation
 	float _height_error_gain{0.2f};					///< height error inverse time constant [1/s]
@@ -231,12 +260,17 @@ private:
 	float _airspeed_error_gain{0.1f};				///< airspeed error inverse time constant [1/s]
 	float _equivalent_airspeed_min{3.0f};				///< equivalent airspeed demand lower limit (m/sec)
 	float _equivalent_airspeed_max{30.0f};				///< equivalent airspeed demand upper limit (m/sec)
-	float _equivalent_airspeed_cruise{15.0f};			///< equivalent cruise airspeed for airspeed less mode (m/sec)
+	float _equivalent_airspeed_trim{15.0f};			///< equivalent cruise airspeed for airspeed less mode (m/sec)
 	float _throttle_slewrate{0.0f};					///< throttle demand slew rate limit (1/sec)
 	float _STE_rate_time_const{0.1f};				///< filter time constant for specific total energy rate (damping path) (s)
 	float _speed_derivative_time_const{0.01f};			///< speed derivative filter time constant (s)
 	float _SEB_rate_ff{1.0f};
 
+	float _height_error_gain_eco{0.2f};				///< in eco mode: height error inverse time constant [1/s]
+	float _pitch_speed_weight_eco{1.0f};				///< in eco mode: speed control weighting used by pitch demand calculation
+	float _fw_eco_alt_err_u{10.f};					///< altitude difference low threshold for switching to flight mode descend
+	float _fw_eco_alt_err_o{20.f};					///< altitude difference high threshold for switching to flight mode climb
+
 	// complimentary filter states
 	float _vert_vel_state{0.0f};					///< complimentary filter state - height rate (m/sec)
 	float _vert_pos_state{0.0f};					///< complimentary filter state - height (m)
@@ -268,10 +302,12 @@ private:
 
 	// vehicle physical limits
 	float _pitch_setpoint_unc{0.0f};				///< pitch demand before limiting (rad)
-	float _STE_rate_max{0.0f};					///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3)
-	float _STE_rate_min{0.0f};					///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3)
+	float _STE_rate_max{FLT_EPSILON};				///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3)
+	float _STE_rate_min{-FLT_EPSILON};				///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3)
 	float _throttle_setpoint_max{0.0f};				///< normalised throttle upper limit
 	float _throttle_setpoint_min{0.0f};				///< normalised throttle lower limit
+	float _throttle_limit_max{0.0f};				///< APX4 custom extra throttle limit, can be changed in air without having an effect on STE_rate_max
+	float _throttle_trim{0.0f};					///< throttle required to fly level at _EAS_setpoint, compensated for air density and vehicle weight
 	float _pitch_setpoint_max{0.5f};				///< pitch demand upper limit (rad)
 	float _pitch_setpoint_min{-0.5f};				///< pitch demand lower limit (rad)
 
@@ -280,6 +316,7 @@ private:
 	float _SKE_setpoint{0.0f};					///< specific kinetic energy demand (m**2/sec**2)
 	float _SPE_rate_setpoint{0.0f};					///< specific potential energy rate demand (m**2/sec**3)
 	float _SKE_rate_setpoint{0.0f};					///< specific kinetic energy rate demand (m**2/sec**3)
+	float _STE_rate_setpoint{0.0f};					///< specific total energy rate demand (m**s/sec**3)
 	float _SPE_estimate{0.0f};					///< specific potential energy estimate (m**2/sec**2)
 	float _SKE_estimate{0.0f};					///< specific kinetic energy estimate (m**2/sec**2)
 	float _SPE_rate{0.0f};						///< specific potential energy rate estimate (m**2/sec**3)
@@ -300,13 +337,16 @@ private:
 	static constexpr float DT_DEFAULT = 0.02f;			///< default value for _dt (sec)
 
 	// controller mode logic
-	bool _underspeed_detected{false};				///< true when an underspeed condition has been detected
+	float _percent_undersped{0.0f};					///< a continuous representation of how "undersped" the TAS is [0,1]
 	bool _detect_underspeed_enabled{true};				///< true when underspeed detection is enabled
 	bool _uncommanded_descent_recovery{false};			///< true when a continuous descent caused by an unachievable airspeed demand has been detected
 	bool _climbout_mode_active{false};				///< true when in climbout mode
 	bool _airspeed_enabled{false};					///< true when airspeed use has been enabled
-	bool _states_initialized{false};					///< true when TECS states have been iniitalized
-	bool _in_air{false};						///< true when the vehicle is flying
+	bool _states_initialized{false};				///< true when TECS states have been iniitalized
+	bool _eco_mode_enabled{false};
+
+	// APX4 custom: flight phase
+	int _flight_phase{tecs_status_s::TECS_FLIGHT_PHASE_LEVEL};
 
 	/**
 	 * Update the airspeed internal state using a second order complementary filter
@@ -325,7 +365,7 @@ private:
 			float alt_amsl);
 
 	/**
-	 * Detect if the system is not capable of maintaining airspeed
+	 * Detect how undersped the aircraft is
 	 */
 	void _detect_underspeed();
 
@@ -337,7 +377,7 @@ private:
 	/**
 	 * Update throttle setpoint
 	 */
-	void _update_throttle_setpoint(float throttle_cruise);
+	void _update_throttle_setpoint();
 
 	/**
 	 * Detect an uncommanded descent
@@ -351,6 +391,8 @@ private:
 
 	void _updateTrajectoryGenerationConstraints();
 
+	void _updateFlightPhase(float altitude_sp_amsl, float height_rate_setpoint);
+
 	void _calculateHeightRateSetpoint(float altitude_sp_amsl, float height_rate_sp, float target_climbrate,
 					  float target_sinkrate, float altitude_amsl);
 
diff --git a/src/lib/terrain/CMakeLists.txt b/src/lib/terrain/CMakeLists.txt
new file mode 100644
index 0000000000..09b71d9782
--- /dev/null
+++ b/src/lib/terrain/CMakeLists.txt
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (c) 2020 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_library(terrain
+	util.cpp
+	terrain_provider.cpp
+)
+target_link_libraries(terrain PRIVATE geo)
diff --git a/src/lib/terrain/terrain_provider.cpp b/src/lib/terrain/terrain_provider.cpp
new file mode 100644
index 0000000000..0ece5ba968
--- /dev/null
+++ b/src/lib/terrain/terrain_provider.cpp
@@ -0,0 +1,305 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 "terrain_provider.h"
+
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/posix.h>
+#include <lib/mathlib/mathlib.h>
+
+#include <limits.h>
+
+using namespace terrain;
+
+
+TerrainProvider::TerrainProvider(int grid_spacing_m)
+	: _grid_spacing_m(grid_spacing_m)
+{
+	_cache_data_size = util::maxNumItemsPerFile(_grid_spacing_m);
+
+	// allocate on startup so we know if we have the memory available or not
+	for (int i = 0; i < cache_size; ++i) {
+		_cache[i].data = new int16_t[_cache_data_size];
+
+		if (!_cache[i].data) {
+			PX4_ERR("terrain alloc failed");
+		}
+	}
+
+	pthread_mutex_init(&_mtx, nullptr);
+	pthread_cond_init(&_cv, nullptr);
+	int ret = threadStart();
+
+	if (ret != 0) {
+		PX4_ERR("Failed to start terrain IO thread (%i)", ret);
+	}
+
+}
+
+TerrainProvider::~TerrainProvider()
+{
+	threadStop();
+	pthread_mutex_destroy(&_mtx);
+	pthread_cond_destroy(&_cv);
+}
+
+int TerrainProvider::threadStart()
+{
+	pthread_attr_t thr_attr;
+	pthread_attr_init(&thr_attr);
+
+	sched_param param;
+	/* low priority, as this is disk I/O */
+	param.sched_priority = SCHED_PRIORITY_DEFAULT - 41;
+	(void)pthread_attr_setschedparam(&thr_attr, &param);
+
+	pthread_attr_setstacksize(&thr_attr, PX4_STACK_ADJUSTED(1170));
+
+	int ret = pthread_create(&_thread, &thr_attr, &ioThreadEntry, this);
+	pthread_attr_destroy(&thr_attr);
+
+	return ret;
+}
+
+void TerrainProvider::threadStop()
+{
+	_should_exit.store(true);
+	lock();
+	wakeup();
+	unlock();
+
+	// wait for thread to complete
+	int ret = pthread_join(_thread, nullptr);
+
+	if (ret) {
+		PX4_ERR("join failed: %d", ret);
+	}
+}
+
+void *TerrainProvider::ioThreadEntry(void *context)
+{
+	px4_prctl(PR_SET_NAME, "terrain_io", px4_getpid());
+	static_cast<TerrainProvider *>(context)->runThread();
+	return nullptr;
+}
+
+void TerrainProvider::runThread()
+{
+	while (true) {
+		pthread_mutex_lock(&_mtx);
+		// check for requests
+		bool had_request = false;
+
+		do {
+			had_request = false;
+
+			for (int i = 0; i < cache_size; ++i) {
+				if (_cache[i].state == CacheState::Requested) {
+					pthread_mutex_unlock(&_mtx);
+					had_request = true;
+					bool success = loadCache(_cache[i]);
+					pthread_mutex_lock(&_mtx);
+
+					if (success) {
+						_cache[i].state = CacheState::Used;
+
+					} else {
+						_cache[i].reset();
+					}
+				}
+			}
+		} while (had_request);
+
+		if (_should_exit.load()) {
+			break;
+		}
+
+		pthread_cond_wait(&_cv, &_mtx);
+		pthread_mutex_unlock(&_mtx);
+	}
+
+	pthread_mutex_unlock(&_mtx);
+}
+
+bool TerrainProvider::loadCache(CacheItem &item)
+{
+	if (!item.data) {
+		return false;
+	}
+
+	char filename[64];
+	util::getFileName(item.tag_lat, item.tag_lon, filename, sizeof(filename));
+
+	int fd = open(filename, O_RDONLY, PX4_O_MODE_666);
+
+	if (fd == -1) {
+		PX4_DEBUG("failed to open file %s", filename);
+		return false;
+	}
+
+	FileHeader header;
+	uint16_t expected_magic = header.magic;
+	uint16_t expected_version = header.version;
+
+	bool success = false;
+
+	if (read(fd, &header, sizeof(header)) == sizeof(header)) {
+		if (header.magic == expected_magic && header.version == expected_version &&
+		    header.grid_spacing_m == _grid_spacing_m) {
+
+			item.x_count = header.num_points_x;
+			item.y_count = header.num_points_y;
+			item.ref.initReference(header.lat_sw, header.lon_sw);
+
+			if (item.x_count * item.y_count > _cache_data_size) {
+				PX4_ERR("File contains too many items (%i*%i > %i)", item.x_count, item.y_count, _cache_data_size);
+
+			} else {
+				int read_size = header.num_points_x * header.num_points_y * sizeof(int16_t);
+
+				if (read(fd, item.data, read_size) == read_size) {
+					success = true;
+				}
+			}
+		}
+	}
+
+	close(fd);
+
+	PX4_DEBUG("loaded terrain data to cache: lat: %.3f lon: %.3f", header.lat_sw, header.lon_sw);
+
+	item.used_tick = _current_tick++;
+	return success;
+}
+
+bool TerrainProvider::lookup(double lat, double lon, float &elevation_m)
+{
+	bool ret = false;
+	unsigned tag_lat = util::cacheTagFromLat(lat);
+	unsigned tag_lon = util::cacheTagFromLon(lon);
+
+	lock();
+	unsigned lowest_tick = UINT_MAX;
+	int lowest_tick_idx = -1;
+	int found_tag_idx = -1;
+
+	for (int i = 0; i < cache_size; ++i) {
+		if (_cache[i].state != CacheState::Unused) {
+			if (_cache[i].tag_lat == tag_lat && _cache[i].tag_lon == tag_lon) {
+				found_tag_idx = i;
+			}
+		}
+
+		if (_cache[i].state != CacheState::Requested) {
+			// keep track of the lowest used tick: this will be an unused item or the oldest one (LRU)
+			if (_cache[i].used_tick < lowest_tick) {
+				lowest_tick = _cache[i].used_tick;
+				lowest_tick_idx = i;
+			}
+		}
+	}
+
+	if (found_tag_idx == -1) {
+		if (lowest_tick_idx != -1) {
+			// request item for caching
+			CacheItem &item = _cache[lowest_tick_idx];
+			item.tag_lat = tag_lat;
+			item.tag_lon = tag_lon;
+			item.state = CacheState::Requested;
+			wakeup();
+		} // else: all items currently being requested for caching
+
+	} else {
+		if (_cache[found_tag_idx].state == CacheState::Used) {
+			ret = getElevationFromCache(_cache[found_tag_idx], lat, lon, elevation_m);
+		} // else: currently being requested for caching
+	}
+
+	unlock();
+	return ret;
+}
+
+bool TerrainProvider::getElevationFromCache(CacheItem &item, double lat, double lon, float &elevation_m)
+{
+	if (!item.ref.isInitialized()) {
+		return false;
+	}
+
+	float x, y;
+	item.ref.project(lat, lon, x, y);
+
+	x /= _grid_spacing_m;
+	y /= _grid_spacing_m;
+	int ix = x;
+	int iy = y;
+
+	if (ix < 0 || ix >= item.x_count - 1) {
+		// getting here is a bug, but we don't want to spam the console
+		double ref_lat = item.ref.getProjectionReferenceLat();
+		double ref_lon = item.ref.getProjectionReferenceLon();
+		PX4_DEBUG("x index out of range: %i, %i, x=%.3f, lat=%.3f, ref lat=%.3f, max lat=%.3f, lat/delta=%.3f",
+			  ix, item.x_count, (double)x, lat, math::degrees(ref_lat), math::degrees(ref_lat) + util::delta_lat_lon_deg,
+			  lat / util::delta_lat_lon_deg);
+		(void)ref_lat;
+		(void)ref_lon;
+		return false;
+	}
+
+	if (iy < 0 || iy >= item.y_count - 1) {
+		double ref_lat = item.ref.getProjectionReferenceLat();
+		double ref_lon = item.ref.getProjectionReferenceLon();
+		PX4_DEBUG("y index out of range: %i, %i, y=%.3f, lon=%.3f, ref lon=%.3f, max lon=%.3f, lon/delta=%.3f",
+			  iy, item.y_count, (double)y, lon, math::degrees(ref_lon), math::degrees(ref_lon) + util::delta_lat_lon_deg,
+			  lon / util::delta_lat_lon_deg);
+		(void)ref_lat;
+		(void)ref_lon;
+		return false;
+	}
+
+	// bilinear interpolation
+	float data_sw = item.data[iy + ix * item.y_count];
+	float data_se = item.data[(iy + 1) + ix * item.y_count];
+	float data_ne = item.data[(iy + 1) + (ix + 1) * item.y_count];
+	float data_nw = item.data[iy + (ix + 1) * item.y_count];
+
+	float x_scale = x - ix;
+	float y_scale = y - iy;
+
+	float data_min = y_scale * (data_se - data_sw) + data_sw;
+	float data_max = y_scale * (data_ne - data_nw) + data_nw;
+	elevation_m = x_scale * (data_max - data_min) + data_min;
+
+	item.used_tick = _current_tick++;
+
+	return true;
+}
diff --git a/src/lib/terrain/terrain_provider.h b/src/lib/terrain/terrain_provider.h
new file mode 100644
index 0000000000..ede54f764b
--- /dev/null
+++ b/src/lib/terrain/terrain_provider.h
@@ -0,0 +1,145 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 <stdint.h>
+#include <pthread.h>
+
+#include "util.h"
+
+#include <lib/geo/geo.h>
+#include <px4_platform_common/atomic.h>
+
+namespace terrain
+{
+
+/**
+ * @class TerrainProvider
+ * This provides terrain lookup from disk, with an LRU cache in RAM.
+ * If not in cache, data is loaded async with an IO thread.
+ * Make sure to only keep one instance as it is expensive on memory usage.
+ */
+class TerrainProvider
+{
+public:
+	TerrainProvider(int grid_spacing_m);
+	virtual ~TerrainProvider();
+
+	/**
+	 * lookup altitude data. This method returns immediately, triggering a caching request if data is not
+	 * cached already.
+	 * Note: this can be called concurrently from different threads.
+	 * @param lat lookup latitude in [-90, 90]
+	 * @param lon lookup longitude in [-180, 180]
+	 * @param alt_msl_m output altitude
+	 * @return true on success, false if not in cache / outside of data range
+	 */
+	virtual bool lookup(double lat, double lon, float &alt_msl_m);
+private:
+	static constexpr int cache_size =
+		3; ///< number of cached items. Item size depends on grid_spacing, generally several KB
+
+	enum class CacheState {
+		Unused = 0,
+		Requested,
+		Used
+	};
+
+	struct CacheItem {
+		~CacheItem() { delete[] data; }
+
+		void reset()
+		{
+			state = CacheState::Unused;
+			used_tick = 0;
+		}
+
+		CacheState state{CacheState::Unused};
+		unsigned tag_lat{};
+		unsigned tag_lon{};
+		unsigned used_tick{};
+		int x_count{};
+		int y_count{};
+		MapProjection ref{};
+		int16_t *data{nullptr};
+	};
+
+	int threadStart();
+	void threadStop();
+
+	static void *ioThreadEntry(void *);
+	void runThread();
+
+	void lock()
+	{
+		pthread_mutex_lock(&_mtx);
+	}
+
+	void unlock()
+	{
+		pthread_mutex_unlock(&_mtx);
+	}
+
+	/**
+	 * Wakeup the IO thread. Must be called in locked state to avoid lost wakeups.
+	 */
+	void wakeup()
+	{
+		pthread_cond_broadcast(&_cv);
+	}
+
+	/**
+	 * Load data from disk into cache.
+	 * Does not change the item's state.
+	 * Called from the IO thread, in unlocked state
+	 * @return true on success
+	 */
+	bool loadCache(CacheItem &item);
+
+	bool getElevationFromCache(CacheItem &item, double lat, double lon, float &elevation_m);
+
+	const int _grid_spacing_m;
+	int _cache_data_size;
+
+	CacheItem _cache[cache_size];
+	unsigned _current_tick{};
+
+	pthread_mutex_t _mtx;
+	pthread_cond_t _cv;
+	pthread_t _thread{0};
+
+	px4::atomic_bool _should_exit{false};
+};
+
+}
diff --git a/src/lib/terrain/util.cpp b/src/lib/terrain/util.cpp
new file mode 100644
index 0000000000..aba50a5ed9
--- /dev/null
+++ b/src/lib/terrain/util.cpp
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 "util.h"
+
+#include <lib/geo/geo.h>
+#include <lib/mathlib/mathlib.h>
+#include <px4_platform_common/log.h>
+
+using namespace terrain;
+
+namespace terrain
+{
+namespace util
+{
+
+void getFileName(unsigned tag_lat, unsigned tag_lon, char *buf, int len)
+{
+	int n = snprintf(buf, len, "%s/%05i_%05i.dat", TERRAIN_STORAGE_DIR, tag_lat, tag_lon);
+
+	if (n >= len) {
+		PX4_ERR("terrain path too long (%i %i)", n, len);
+	}
+
+	buf[len - 1] = '\0';
+}
+
+int maxNumItemsPerFile(int grid_spacing_m)
+{
+	// Given a delta angle (delta_lat_lon_deg), this computes the maximum area in local space
+	// over all possible lat/lon reference points. See map_projection_project().
+	// The maximum is at latitude=0, the longitude does not matter.
+	constexpr double r = CONSTANTS_RADIUS_OF_EARTH;
+	const double cos_d = cos(math::radians(delta_lat_lon_deg));
+	const double cos_d_sq = cos_d * cos_d;
+	const double sin_d = sin(math::radians(delta_lat_lon_deg));
+
+	double c = acos(cos_d_sq);
+	double x = c / sin(c) * r * sin_d;
+	double y = x * cos_d;
+	// account for 1 item overlap in both directions
+	return (int)((x + grid_spacing_m) / grid_spacing_m + 1) * (int)((y + grid_spacing_m) / grid_spacing_m + 1);
+}
+} // namespace util
+} // namespace terrain
diff --git a/src/lib/terrain/util.h b/src/lib/terrain/util.h
new file mode 100644
index 0000000000..22a25caeee
--- /dev/null
+++ b/src/lib/terrain/util.h
@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 <matrix/math.hpp>
+#include <px4_platform_common/defines.h>
+
+namespace terrain
+{
+
+#define TERRAIN_STORAGE_DIR PX4_STORAGEDIR "/terrain_data"
+
+namespace util
+{
+
+/// This defines the delta latitude/longitude angle stored in a single file.
+/// The larger this is the less files are generated, but the larger the files and thus required
+/// RAM buffer, as always whole files are loaded to RAM (see maxNumItemsPerFile()).
+/// Note: if this is changed, make sure to also increase the FileHeader::version
+static constexpr double delta_lat_lon_deg = 0.02;
+
+// 90/delta_lat_lon_deg must be int, the alignment in TerrainUploader::updateArea() (and other places) depend on that.
+static_assert(90. / delta_lat_lon_deg - (int)(90. / delta_lat_lon_deg) < 0.000001, "invalid delta_lat_lon_deg");
+
+// The following are given by the mavlink messages
+static constexpr int mavlink_num_blocks_x = 7; ///< 7 blocks in north direction
+static constexpr int mavlink_num_blocks_y = 8; ///< 8 blocks in east direction
+static constexpr int mavlink_terrain_data_size = 4; ///< 4x4 data points in TERRAIN_DATA.data
+
+/**
+ * Maximum number of items (x*y) in a file
+ */
+int maxNumItemsPerFile(int grid_spacing_m);
+
+/**
+ * Get a tag from a latitude, used to identify cached items or files on disk (this is used as part of the filename)
+ */
+static inline unsigned cacheTagFromLat(double lat)
+{
+	lat = matrix::wrap(lat, -90., 90.);
+	return (lat + 90.) / delta_lat_lon_deg;
+}
+
+/**
+ * Get a tag from a longitude, used to identify cached items or files on disk (this is used as part of the filename)
+ */
+static inline unsigned cacheTagFromLon(double lon)
+{
+	lon = matrix::wrap(lon, -180., 180.);
+	return (lon + 180.) / delta_lat_lon_deg;
+}
+
+void getFileName(unsigned tag_lat, unsigned tag_lon, char *buf, int len);
+
+static inline void getFileName(double lat, double lon, char *buf, int len)
+{
+	unsigned tag_lat = cacheTagFromLat(lat);
+	unsigned tag_lon = cacheTagFromLon(lon);
+	getFileName(tag_lat, tag_lon, buf, len);
+}
+
+
+static inline bool getTagsFromFileName(const char *filename, unsigned &tag_lat, unsigned &tag_lon)
+{
+	return sscanf(filename, "%d_%d.dat", &tag_lat, &tag_lon) == 2;
+}
+
+}
+
+#pragma pack(push, 1)
+struct FileHeader {
+	uint16_t magic{0xccbb};
+	uint16_t version{0};
+	uint16_t num_points_x;
+	uint16_t num_points_y;
+	int grid_spacing_m;
+	double lat_sw; ///< latitude of south-west corner
+	double lon_sw; ///< longitude of south-west corner
+};
+#pragma pack(pop)
+
+
+}
diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp
index 8d3f405ac7..c8876bedc4 100644
--- a/src/lib/wind_estimator/WindEstimator.cpp
+++ b/src/lib/wind_estimator/WindEstimator.cpp
@@ -39,55 +39,48 @@
 #include "WindEstimator.hpp"
 
 bool
-WindEstimator::initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas,
-			  const matrix::Quatf &q_att)
+WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_variance, const float heading_rad,
+			  const float tas_meas, const float tas_variance)
 {
-	// do no initialise if ground velocity is low
-	// this should prevent the filter from initialising on the ground
-	if (sqrtf(velI(0) * velI(0) + velI(1) * velI(1)) < 3.0f) {
-		return false;
+	if (PX4_ISFINITE(tas_meas) && PX4_ISFINITE(tas_variance)) {
+
+		const float cos_heading = cosf(heading_rad);
+		const float sin_heading = sinf(heading_rad);
+
+		// initialise wind states assuming zero side slip and horizontal flight
+		_state(INDEX_W_N) = velI(0) - tas_meas * cos_heading;
+		_state(INDEX_W_E) = velI(1) - tas_meas * sin_heading;
+		_state(INDEX_TAS_SCALE) = _scale_init;
+
+		constexpr float initial_sideslip_uncertainty = math::radians(INITIAL_BETA_ERROR_DEG);
+		const float initial_wind_var_body_y = sq(tas_meas * sinf(initial_sideslip_uncertainty));
+		constexpr float heading_variance = sq(math::radians(INITIAL_HEADING_ERROR_DEG));
+
+		// rotate wind velocity into earth frame aligned with vehicle yaw
+		const float Wx = _state(INDEX_W_N) * cos_heading + _state(INDEX_W_E) * sin_heading;
+		const float Wy = -_state(INDEX_W_N) * sin_heading + _state(INDEX_W_E) * cos_heading;
+
+		_P(INDEX_W_N, INDEX_W_N) = tas_variance * sq(cos_heading) + heading_variance * sq(-Wx * sin_heading - Wy * cos_heading)
+					   + initial_wind_var_body_y * sq(sin_heading);
+		_P(INDEX_W_N, INDEX_W_E) = tas_variance * sin_heading * cos_heading + heading_variance *
+					   (-Wx * sin_heading - Wy * cos_heading) * (Wx * cos_heading - Wy * sin_heading) -
+					   initial_wind_var_body_y * sin_heading * cos_heading;
+		_P(INDEX_W_E, INDEX_W_N) = _P(INDEX_W_N, INDEX_W_E);
+		_P(INDEX_W_E, INDEX_W_E) = tas_variance * sq(sin_heading) + heading_variance * sq(Wx * cos_heading - Wy * sin_heading) +
+					   initial_wind_var_body_y * sq(cos_heading);
+
+		// Now add the variance due to uncertainty in vehicle velocity that was used to calculate the initial wind speed
+		_P(INDEX_W_N, INDEX_W_N) += hor_vel_variance;
+		_P(INDEX_W_E, INDEX_W_E) += hor_vel_variance;
+
+	} else {
+		// no airspeed available
+		_state.setZero();
+		_state(INDEX_TAS_SCALE) = 1.0f;
+		_P.setZero();
+		_P(INDEX_W_N, INDEX_W_N) = _P(INDEX_W_E, INDEX_W_E) = sq(INITIAL_WIND_ERROR);
 	}
 
-	const float v_n = velI(0);
-	const float v_e = velI(1);
-
-	const float heading_est = matrix::Eulerf(q_att).psi();
-
-	// initilaise wind states assuming zero side slip and horizontal flight
-	_state(INDEX_W_N) = velI(INDEX_W_N) - tas_meas * cosf(heading_est);
-	_state(INDEX_W_E) = velI(INDEX_W_E) - tas_meas * sinf(heading_est);
-	_state(INDEX_TAS_SCALE) = _scale_init;
-
-	// compute jacobian of states wrt north/each earth velocity states and true airspeed measurement
-	float L0 = v_e * v_e;
-	float L1 = v_n * v_n;
-	float L2 = L0 + L1;
-	float L3 = tas_meas / (L2 * sqrtf(L2));
-	float L4 = L3 * v_e * v_n + 1.0f;
-	float L5 = 1.0f / sqrtf(L2);
-	float L6 = -L5 * tas_meas;
-
-	matrix::Matrix3f L;
-	L.setZero();
-	L(0, 0) = L4;
-	L(0, 1) = L0 * L3 + L6;
-	L(1, 0) = L1 * L3 + L6;
-	L(1, 1) = L4;
-	L(2, 2) = 1.0f;
-
-	// get an estimate of the state covariance matrix given the estimated variance of ground velocity
-	// and measured airspeed
-	_P.setZero();
-	_P(INDEX_W_N, INDEX_W_N) = velIvar(0);
-	_P(INDEX_W_E, INDEX_W_E) = velIvar(1);
-	_P(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = 0.0001f;
-
-	_P = L * _P * L.transpose();
-
-	// reset the timestamp for measurement rejection
-	_time_rejected_tas = 0;
-	_time_rejected_beta = 0;
-
 	_wind_estimator_reset = true;
 
 	return true;
@@ -113,28 +106,23 @@ WindEstimator::update(uint64_t time_now)
 	}
 
 	float dt = (float)(time_now - _time_last_update) * 1e-6f;
-	float dt2 = dt * dt;
 	_time_last_update = time_now;
 
-	float q_w = _wind_p_var;
-	float q_k_tas = _tas_scale_p_var;
-
 	matrix::Matrix3f Qk;
-	Qk(0, 0) = q_w * dt2;
+	Qk(0, 0) = _wind_psd * dt;
 	Qk(1, 1) = Qk(0, 0);
-	Qk(2, 2) = q_k_tas * dt2;
+	Qk(2, 2) = _tas_scale_psd * dt;
 	_P += Qk;
 }
 
 void
 WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const matrix::Vector3f &velI,
-			     const matrix::Vector2f &velIvar, const matrix::Quatf &q_att)
+			     const float hor_vel_variance, const matrix::Quatf &q_att)
 {
-	matrix::Vector2f velIvar_constrained = { math::max(0.01f, velIvar(0)), math::max(0.01f, velIvar(1)) };
 
 	if (!_initialised) {
 		// try to initialise
-		_initialised = initialise(velI, velIvar_constrained, true_airspeed, q_att);
+		_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var);
 		return;
 	}
 
@@ -177,20 +165,14 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 	// innovation variance
 	_tas_innov_var = S(0, 0);
 
-	bool reinit_filter = false;
-	bool meas_is_rejected = false;
+	const bool meas_is_rejected = check_if_meas_is_rejected(_tas_innov, _tas_innov_var, _tas_gate);
 
-	// note: _time_rejected_tas and reinit_filter are not used anymore as filter can't get reset due to tas rejection
-	meas_is_rejected = check_if_meas_is_rejected(time_now, _tas_innov, _tas_innov_var, _tas_gate, _time_rejected_tas,
-			   reinit_filter);
+	if (_tas_innov_var < 0.0f) {
+		// re init filter in case of a negative variance, and trigger early return to not fuse measurement
+		_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi(), true_airspeed, _tas_var);
+		return;
 
-	if (meas_is_rejected || _tas_innov_var < 0.f) {
-		// only reset filter if _tas_innov_var gets unfeasible
-		if (_tas_innov_var < 0.0f) {
-			_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), true_airspeed, q_att);
-		}
-
-		// we either did a filter reset or the current measurement was rejected so do not fuse
+	} else if (meas_is_rejected) {
 		return;
 	}
 
@@ -206,10 +188,11 @@ WindEstimator::fuse_airspeed(uint64_t time_now, const float true_airspeed, const
 }
 
 void
-WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att)
+WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const float hor_vel_variance,
+			 const matrix::Quatf &q_att)
 {
 	if (!_initialised) {
-		_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length(), q_att);
+		_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi());
 		return;
 	}
 
@@ -271,20 +254,14 @@ WindEstimator::fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const
 	_beta_innov = 0.0f - beta_pred;
 	_beta_innov_var = S(0, 0);
 
-	bool reinit_filter = false;
-	bool meas_is_rejected = false;
+	const bool meas_is_rejected = check_if_meas_is_rejected(_beta_innov, _beta_innov_var, _beta_gate);
 
-	meas_is_rejected = check_if_meas_is_rejected(time_now, _beta_innov, _beta_innov_var, _beta_gate, _time_rejected_beta,
-			   reinit_filter);
+	if (_beta_innov_var < 0.0f) {
+		// re init filter in case of a negative variance, and trigger early return to not fuse measurement
+		_initialised = initialise(velI, hor_vel_variance, matrix::Eulerf(q_att).psi());
+		return;
 
-	reinit_filter |= _beta_innov_var < 0.0f;
-
-	if (meas_is_rejected || reinit_filter) {
-		if (reinit_filter) {
-			_initialised = initialise(velI, matrix::Vector2f(0.1f, 0.1f), velI.length(), q_att);
-		}
-
-		// we either did a filter reset or the current measurement was rejected so do not fuse
+	} else if (meas_is_rejected) {
 		return;
 	}
 
@@ -334,17 +311,7 @@ WindEstimator::run_sanity_checks()
 }
 
 bool
-WindEstimator::check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size,
-		uint64_t &time_meas_rejected, bool &reinit_filter)
+WindEstimator::check_if_meas_is_rejected(float innov, float innov_var, uint8_t gate_size)
 {
-	if (innov * innov > gate_size * gate_size * innov_var) {
-		time_meas_rejected = time_meas_rejected == 0 ? time_now : time_meas_rejected;
-
-	} else {
-		time_meas_rejected = 0;
-	}
-
-	reinit_filter = time_now - time_meas_rejected > 5_s && time_meas_rejected != 0;
-
-	return time_meas_rejected != 0;
+	return (innov * innov > gate_size * gate_size * innov_var);
 }
diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp
index b89f501dab..f07cabb9a8 100644
--- a/src/lib/wind_estimator/WindEstimator.hpp
+++ b/src/lib/wind_estimator/WindEstimator.hpp
@@ -58,13 +58,13 @@ public:
 	void update(uint64_t time_now);
 
 	void fuse_airspeed(uint64_t time_now, float true_airspeed, const matrix::Vector3f &velI,
-			   const matrix::Vector2f &velIvar, const matrix::Quatf &q_att);
-	void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const matrix::Quatf &q_att);
+			   const float hor_vel_variance, const matrix::Quatf &q_att);
+	void fuse_beta(uint64_t time_now, const matrix::Vector3f &velI, const float hor_vel_variance,
+		       const matrix::Quatf &q_att);
 
 	bool is_estimate_valid() { return _initialised; }
 
-	bool check_if_meas_is_rejected(uint64_t time_now, float innov, float innov_var, uint8_t gate_size,
-				       uint64_t &time_meas_rejected, bool &reinit_filter);
+	bool check_if_meas_is_rejected(float innov, float innov_var, uint8_t gate_size);
 
 	matrix::Vector2f get_wind() { return matrix::Vector2f{_state(INDEX_W_N), _state(INDEX_W_E)}; }
 
@@ -78,8 +78,10 @@ public:
 	matrix::Vector2f  get_wind_var() { return matrix::Vector2f{_P(0, 0), _P(1, 1)}; }
 	bool get_wind_estimator_reset() { return _wind_estimator_reset; }
 
-	void set_wind_p_noise(float wind_sigma) { _wind_p_var = wind_sigma * wind_sigma; }
-	void set_tas_scale_p_noise(float tas_scale_sigma) { _tas_scale_p_var = tas_scale_sigma * tas_scale_sigma; }
+	// unaided, the state uncertainty (diagonal of sqrt(P)) grows by the process noise spectral density every second
+	void set_wind_process_noise_spectral_density(float wind_nsd) { _wind_psd = wind_nsd * wind_nsd; }
+	void set_tas_scale_process_noise_spectral_density(float tas_scale_nsd) { _tas_scale_psd = tas_scale_nsd * tas_scale_nsd; }
+
 	void set_tas_noise(float tas_sigma) { _tas_var = tas_sigma * tas_sigma; }
 	void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; }
 	void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; }
@@ -93,6 +95,13 @@ private:
 		INDEX_TAS_SCALE
 	};	///< enum which can be used to access state.
 
+	static constexpr float INITIAL_WIND_ERROR =
+		2.5f; // initial uncertainty of each wind state when filter is initialised without airspeed [m/s]
+	static constexpr float INITIAL_BETA_ERROR_DEG =
+		15.0f;	// sidelip uncertainty used to initialise the filter with a true airspeed measurement [deg]
+	static constexpr float INITIAL_HEADING_ERROR_DEG =
+		10.0f; // heading uncertainty used to initialise the filter with a true airspeed measurement [deg]
+
 	matrix::Vector3f _state{0.f, 0.f, 1.f};
 	matrix::Matrix3f _P;		///< state covariance matrix
 
@@ -104,8 +113,8 @@ private:
 
 	bool _initialised{false};	///< True: filter has been initialised
 
-	float _wind_p_var{0.1f};	///< wind process noise variance
-	float _tas_scale_p_var{0.0001f};	///< true airspeed scale process noise variance
+	float _wind_psd{0.1f};	///< wind process noise power spectral density (m^2/s^4/Hz)
+	float _tas_scale_psd{0.0001f};	///< true airspeed process noise power spectral density (1/s^2/Hz)
 	float _tas_var{1.4f};		///< true airspeed measurement noise variance
 	float _beta_var{0.5f};	///< sideslip measurement noise variance
 	uint8_t _tas_gate{3};	///< airspeed fusion gate size
@@ -116,15 +125,15 @@ private:
 	uint64_t _time_last_airspeed_fuse = 0;	///< timestamp of last airspeed fusion
 	uint64_t _time_last_beta_fuse = 0;	///< timestamp of last sideslip fusion
 	uint64_t _time_last_update = 0;		///< timestamp of last covariance prediction
-	uint64_t _time_rejected_beta = 0;	///< timestamp of when sideslip measurements have consistently started to be rejected
-	uint64_t _time_rejected_tas =
-		0;	///< timestamp of when true airspeed measurements have consistently started to be rejected
 
 	bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle
 
 	// initialise state and state covariance matrix
-	bool initialise(const matrix::Vector3f &velI, const matrix::Vector2f &velIvar, const float tas_meas,
-			const matrix::Quatf &q_att);
+	bool initialise(const matrix::Vector3f &velI, const float hor_vel_variance, const float heading_rad,
+			const float tas_meas = NAN, const float tas_variance = NAN);
 
 	void run_sanity_checks();
+
+	// return the square of two floating point numbers
+	static constexpr float sq(float var) { return var * var; }
 };
diff --git a/src/modules/airspeed_selector/AirspeedValidator.cpp b/src/modules/airspeed_selector/AirspeedValidator.cpp
index 6bc9e2870c..4c64f615be 100644
--- a/src/modules/airspeed_selector/AirspeedValidator.cpp
+++ b/src/modules/airspeed_selector/AirspeedValidator.cpp
@@ -57,6 +57,7 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
 			      input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
 	update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
 	check_airspeed_data_stuck(input_data.timestamp);
+	check_airspeed_data_variation(input_data.timestamp);
 	check_load_factor(input_data.accel_z);
 	check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio,
 				  input_data.ground_velocity);
@@ -81,11 +82,11 @@ AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float air
 		Quatf q(att_q);
 
 		// airspeed fusion (with raw TAS)
-		const Vector3f vel_var{Dcmf(q) *Vector3f{lpos_evh, lpos_evh, lpos_evv}};
-		_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, Vector2f{vel_var(0), vel_var(1)}, q);
+		const float hor_vel_variance =  lpos_evh * lpos_evh;
+		_wind_estimator.fuse_airspeed(time_now_usec, airspeed_true_raw, vI, hor_vel_variance, q);
 
 		// sideslip fusion
-		_wind_estimator.fuse_beta(time_now_usec, vI, q);
+		_wind_estimator.fuse_beta(time_now_usec, vI, hor_vel_variance, q);
 	}
 }
 
@@ -145,9 +146,9 @@ AirspeedValidator::update_CAS_scale_validated(bool lpos_valid, const matrix::Vec
 		// check passes if the average airspeed with the scale applied is closer to groundspeed than without
 		if (fabsf(TAS_to_grounspeed_error_new) < fabsf(TAS_to_grounspeed_error_current)) {
 
-			// constrain the scale update to max 0.01 at a time
-			const float new_scale_constrained = math::constrain(_wind_estimator.get_tas_scale(), _CAS_scale_validated - 0.01f,
-							    _CAS_scale_validated + 0.01f);
+			// constrain the scale update to max 0.05 at a time
+			const float new_scale_constrained = math::constrain(_wind_estimator.get_tas_scale(), _CAS_scale_validated - 0.05f,
+							    _CAS_scale_validated + 0.05f);
 
 			_CAS_scale_validated = new_scale_constrained;
 		}
@@ -196,7 +197,9 @@ AirspeedValidator::update_CAS_TAS(float air_pressure_pa, float air_temperature_c
 void
 AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now)
 {
-	// data stuck test: trigger when IAS is not changing for DATA_STUCK_TIMEOUT (2s)
+	// Data stuck test: trigger when IAS is not changing for DATA_STUCK_TIMEOUT (2s) when in fixed-wing flight.
+	// Only consider fixed-wing flight as some airspeed sensors have a very low resolution around 0m/s and
+	// can output the exact same value for several seconds then.
 
 	if (!_data_stuck_check_enabled) {
 		_data_stuck_test_failed = false;
@@ -208,7 +211,37 @@ AirspeedValidator::check_airspeed_data_stuck(uint64_t time_now)
 		_IAS_prev = _IAS;
 	}
 
-	_data_stuck_test_failed = hrt_elapsed_time(&_time_last_unequal_data) > DATA_STUCK_TIMEOUT;
+	_data_stuck_test_failed = hrt_elapsed_time(&_time_last_unequal_data) > DATA_STUCK_TIMEOUT && _in_fixed_wing_flight;
+}
+
+void
+AirspeedValidator::check_airspeed_data_variation(uint64_t time_now)
+{
+	// Data variation after boot test: trigger when IAS is not changing for within VARIATION_CHECK_TIMEOUT
+	// after the first data is received.
+
+	if (!_data_variation_check_enabled) {
+		_data_variation_test_failed = false;
+		return;
+	}
+
+	if (_time_first_data == 0) {
+		// init
+		_time_first_data = time_now;
+		_data_variation_check_ias_prev = _IAS;
+	}
+
+	if (!_variation_detected && (time_now - _time_first_data < VARIATION_CHECK_TIMEOUT)) {
+		if (fabsf(_IAS - _data_variation_check_ias_prev) > FLT_EPSILON) {
+			_variation_detected = true;
+		}
+
+		_data_variation_check_ias_prev = _IAS;
+
+	} else {
+		// only update the test_failed flag once the timeout since first data is over
+		_data_variation_test_failed = !_variation_detected;
+	}
 }
 
 void
@@ -289,12 +322,13 @@ AirspeedValidator::check_load_factor(float accel_z)
 void
 AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
 {
-	if (_data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed) {
-		// at least one check (data stuck, innovation or load factor) failed, so record timestamp
+	if (_data_variation_test_failed || _data_stuck_test_failed || _innovations_check_failed || _load_factor_check_failed) {
+		// at least one check (variation, data stuck, innovation or load factor) failed, so record timestamp
 		_time_checks_failed = timestamp;
 
-	} else if (! _data_stuck_test_failed && !_innovations_check_failed && !_load_factor_check_failed) {
-		// all checks(data stuck, innovation and load factor) must pass to declare airspeed good
+	} else if (!_data_variation_test_failed && ! _data_stuck_test_failed && !_innovations_check_failed
+		   && !_load_factor_check_failed) {
+		// all checks(variation, data stuck, innovation and load factor) must pass to declare airspeed good
 		_time_checks_passed = timestamp;
 	}
 
@@ -307,13 +341,13 @@ AirspeedValidator::update_airspeed_valid_status(const uint64_t timestamp)
 		// a timeout period is applied.
 		const bool single_check_fail_timeout = (timestamp - _time_checks_passed) > _checks_fail_delay * 1_s;
 
-		if (both_checks_failed || single_check_fail_timeout || _data_stuck_test_failed) {
+		if (both_checks_failed || single_check_fail_timeout || _data_variation_test_failed || _data_stuck_test_failed) {
 
 			_airspeed_valid = false;
 		}
 
 	} else if (_checks_clear_delay > 0.f && (timestamp - _time_checks_failed) > _checks_clear_delay * 1_s) {
-		// disable the re-enabling if the clear delay is negative
+		// re-enabling is only possible if the clear delay is positive
 		_airspeed_valid = true;
 	}
 }
diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp
index 33429eca2b..f15f0ac27f 100644
--- a/src/modules/airspeed_selector/AirspeedValidator.hpp
+++ b/src/modules/airspeed_selector/AirspeedValidator.hpp
@@ -87,8 +87,8 @@ public:
 	airspeed_wind_s get_wind_estimator_states(uint64_t timestamp);
 
 	// setters wind estimator parameters
-	void set_wind_estimator_wind_p_noise(float wind_sigma) { _wind_estimator.set_wind_p_noise(wind_sigma); }
-	void set_wind_estimator_tas_scale_p_noise(float tas_scale_sigma) { _wind_estimator.set_tas_scale_p_noise(tas_scale_sigma); }
+	void set_wind_estimator_wind_process_noise_spectral_density(float wind_nsd) { _wind_estimator.set_wind_process_noise_spectral_density(wind_nsd); }
+	void set_wind_estimator_tas_scale_process_noise_spectral_density(float tas_scale_nsd) { _wind_estimator.set_tas_scale_process_noise_spectral_density(tas_scale_nsd); }
 	void set_wind_estimator_tas_scale_init(float tas_scale_init)
 	{
 		_tas_scale_init = tas_scale_init;
@@ -118,6 +118,7 @@ public:
 	void set_enable_data_stuck_check(bool enable) { _data_stuck_check_enabled = enable; }
 	void set_enable_innovation_check(bool enable) { _innovation_check_enabled = enable; }
 	void set_enable_load_factor_check(bool enable) { _load_factor_check_enabled = enable; }
+	void set_enable_data_variation_check(bool enable) { _data_variation_check_enabled = enable; }
 
 private:
 
@@ -127,6 +128,7 @@ private:
 	bool _data_stuck_check_enabled{false};
 	bool _innovation_check_enabled{false};
 	bool _load_factor_check_enabled{false};
+	bool _data_variation_check_enabled{false};
 
 	// airspeed scale validity check
 	static constexpr int SCALE_CHECK_SAMPLES = 12; ///< take samples from 12 segments (every 360/12=30°)
@@ -145,6 +147,13 @@ private:
 	float _IAS_prev{0.f};
 	static constexpr uint64_t DATA_STUCK_TIMEOUT{2_s}; ///< timeout after which data stuck check triggers when data is flat
 
+	// variation check
+	bool _data_variation_test_failed{false};
+	static constexpr uint64_t VARIATION_CHECK_TIMEOUT{10_s}; ///< timeout to check for data variation after first data is received
+	hrt_abstime _time_first_data{0};
+	float _data_variation_check_ias_prev{0.f};
+	bool _variation_detected{false};
+
 	// states of innovation check
 	bool _innovations_check_failed{false};  ///< true when airspeed innovations have failed consistency checks
 	float _tas_innov_threshold{1.0}; ///< innovation error threshold for triggering innovation check failure
@@ -184,6 +193,7 @@ private:
 	void update_CAS_scale_applied();
 	void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius);
 	void check_airspeed_data_stuck(uint64_t timestamp);
+	void check_airspeed_data_variation(uint64_t timestamp);
 	void check_airspeed_innovation(uint64_t timestamp, float estimator_status_vel_test_ratio,
 				       float estimator_status_mag_test_ratio, const matrix::Vector3f &vI);
 	void check_load_factor(float accel_z);
diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp
index d641e8011c..6beac7a17a 100644
--- a/src/modules/airspeed_selector/airspeed_selector_main.cpp
+++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp
@@ -147,8 +147,8 @@ private:
 	bool _initialized{false}; /**< module initialized*/
 	bool _vehicle_local_position_valid{false}; /**< local position (from GPS) valid */
 	bool _in_takeoff_situation{true}; /**< in takeoff situation (defined as not yet stall speed reached) */
-	float _ground_minus_wind_TAS{0.0f}; /**< true airspeed from groundspeed minus windspeed */
-	float _ground_minus_wind_CAS{0.0f}; /**< calibrated airspeed from groundspeed minus windspeed */
+	float _ground_minus_wind_TAS{NAN}; /**< true airspeed from groundspeed minus windspeed */
+	float _ground_minus_wind_CAS{NAN}; /**< calibrated airspeed from groundspeed minus windspeed */
 	bool _armed_prev{false};
 
 	hrt_abstime _time_last_airspeed_update[MAX_NUM_AIRSPEED_SENSORS] {};
@@ -161,12 +161,13 @@ private:
 		CHECK_TYPE_ONLY_DATA_MISSING_BIT = (1 << 0),
 		CHECK_TYPE_DATA_STUCK_BIT = (1 << 1),
 		CHECK_TYPE_INNOVATION_BIT = (1 << 2),
-		CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3)
+		CHECK_TYPE_LOAD_FACTOR_BIT = (1 << 3),
+		CHECK_TYPE_DATA_VARIATION_BIT = (1 << 4)
 	};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::ASPD_W_P_NOISE>) _param_west_w_p_noise,
-		(ParamFloat<px4::params::ASPD_SC_P_NOISE>) _param_west_sc_p_noise,
+		(ParamFloat<px4::params::ASPD_WIND_NSD>) _param_aspd_wind_nsd,
+		(ParamFloat<px4::params::ASPD_SCALE_NSD>) _param_aspd_scale_nsd,
 		(ParamFloat<px4::params::ASPD_TAS_NOISE>) _param_west_tas_noise,
 		(ParamFloat<px4::params::ASPD_BETA_NOISE>) _param_west_beta_noise,
 		(ParamInt<px4::params::ASPD_TAS_GATE>) _param_west_tas_gate,
@@ -184,7 +185,11 @@ private:
 		(ParamInt<px4::params::ASPD_FS_T_STOP>) _checks_fail_delay, /**< delay to declare airspeed invalid */
 		(ParamInt<px4::params::ASPD_FS_T_START>) _checks_clear_delay, /**<  delay to declare airspeed valid again */
 
-		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall
+		(ParamFloat<px4::params::ASPD_WERR_THR>) _param_wind_sigma_max_synth_tas,
+
+		// external parameters
+		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
+		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim
 	)
 
 	void 		init(); 	/**< initialization of the airspeed validator instances */
@@ -373,7 +378,7 @@ AirspeedModule::Run()
 				// takeoff situation is active from start till one of the sensors' IAS or groundspeed_CAS is above stall speed
 				if (_in_takeoff_situation &&
 				    (airspeed_raw.indicated_airspeed_m_s > _param_fw_airspd_stall.get() ||
-				     _ground_minus_wind_CAS > _param_fw_airspd_stall.get())) {
+				     (PX4_ISFINITE(_ground_minus_wind_CAS) && _ground_minus_wind_CAS > _param_fw_airspd_stall.get()))) {
 					_in_takeoff_situation = false;
 				}
 
@@ -382,7 +387,7 @@ AirspeedModule::Run()
 					_in_takeoff_situation = true;
 				}
 
-				input_data.in_fixed_wing_flight = (armed && in_air_fixed_wing && !_in_takeoff_situation);
+				input_data.in_fixed_wing_flight = (in_air_fixed_wing && !_in_takeoff_situation);
 
 				// push input data into airspeed validator
 				_airspeed_validator[i].update_airspeed_validator(input_data);
@@ -447,16 +452,16 @@ void AirspeedModule::update_params()
 	_param_airspeed_scale[1] = _param_airspeed_scale_2.get();
 	_param_airspeed_scale[2] = _param_airspeed_scale_3.get();
 
-	_wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get());
-	_wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get());
+	_wind_estimator_sideslip.set_wind_process_noise_spectral_density(_param_aspd_wind_nsd.get());
+	_wind_estimator_sideslip.set_tas_scale_process_noise_spectral_density(_param_aspd_scale_nsd.get());
 	_wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get());
 	_wind_estimator_sideslip.set_beta_noise(_param_west_beta_noise.get());
 	_wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get());
 	_wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get());
 
 	for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
-		_airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get());
-		_airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get());
+		_airspeed_validator[i].set_wind_estimator_wind_process_noise_spectral_density(_param_aspd_wind_nsd.get());
+		_airspeed_validator[i].set_wind_estimator_tas_scale_process_noise_spectral_density(_param_aspd_scale_nsd.get());
 		_airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get());
 		_airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get());
 		_airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get());
@@ -477,6 +482,8 @@ void AirspeedModule::update_params()
 				CheckTypeBits::CHECK_TYPE_INNOVATION_BIT);
 		_airspeed_validator[i].set_enable_load_factor_check(_param_airspeed_checks_on.get() &
 				CheckTypeBits::CHECK_TYPE_LOAD_FACTOR_BIT);
+		_airspeed_validator[i].set_enable_data_variation_check(_param_airspeed_checks_on.get() &
+				CheckTypeBits::CHECK_TYPE_DATA_VARIATION_BIT);
 	}
 }
 
@@ -505,7 +512,9 @@ void AirspeedModule::poll_topics()
 
 
 	_vehicle_local_position_valid = (_time_now_usec - _vehicle_local_position.timestamp < 1_s)
-					&& (_vehicle_local_position.timestamp > 0) && _vehicle_local_position.v_xy_valid;
+					&& (_vehicle_local_position.timestamp > 0)
+					&& _vehicle_local_position.v_xy_valid
+					&& !_vehicle_local_position.dead_reckoning;
 }
 
 void AirspeedModule::update_wind_estimator_sideslip()
@@ -514,11 +523,14 @@ void AirspeedModule::update_wind_estimator_sideslip()
 	_wind_estimator_sideslip.update(_time_now_usec);
 
 	if (_vehicle_local_position_valid
-	    && _vtol_vehicle_status.vehicle_vtol_state == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW) {
+	    && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
+	    && !_vehicle_land_detected.landed) {
 		Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
 		Quatf q(_vehicle_attitude.q);
 
-		_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, q);
+		const float hor_vel_variance =  _vehicle_local_position.evh * _vehicle_local_position.evh;
+
+		_wind_estimator_sideslip.fuse_beta(_time_now_usec, vI, hor_vel_variance, q);
 	}
 
 	_wind_estimate_sideslip.timestamp = _time_now_usec;
@@ -538,13 +550,19 @@ void AirspeedModule::update_wind_estimator_sideslip()
 
 void AirspeedModule::update_ground_minus_wind_airspeed()
 {
-	// calculate airspeed estimate based on groundspeed-windspeed to use as fallback
-	const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
-	const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
-	const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
-	_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
-	_ground_minus_wind_CAS = calc_CAS_from_TAS(_ground_minus_wind_TAS, _vehicle_air_data.baro_pressure_pa,
-				 _vehicle_air_data.baro_temp_celcius);
+	const float wind_uncertainty = sqrtf(_wind_estimate_sideslip.variance_north + _wind_estimate_sideslip.variance_east);
+
+	if (_wind_estimator_sideslip.is_estimate_valid() && wind_uncertainty < _param_wind_sigma_max_synth_tas.get()) {
+		// calculate airspeed estimate based on groundspeed-windspeed
+		const float TAS_north = _vehicle_local_position.vx - _wind_estimate_sideslip.windspeed_north;
+		const float TAS_east = _vehicle_local_position.vy - _wind_estimate_sideslip.windspeed_east;
+		const float TAS_down = _vehicle_local_position.vz; // no wind estimate in z
+		_ground_minus_wind_TAS = sqrtf(TAS_north * TAS_north + TAS_east * TAS_east + TAS_down * TAS_down);
+		_ground_minus_wind_CAS = calc_calibrated_from_true_airspeed(_ground_minus_wind_TAS, _vehicle_air_data.rho);
+
+	} else {
+		_ground_minus_wind_TAS = _ground_minus_wind_CAS = NAN;
+	}
 }
 
 
@@ -641,6 +659,9 @@ void AirspeedModule::select_airspeed_and_publish()
 	airspeed_validated.true_airspeed_m_s = NAN;
 	airspeed_validated.airspeed_sensor_measurement_valid = false;
 	airspeed_validated.selected_airspeed_index = _valid_airspeed_index;
+	airspeed_validated.calibrated_airspeed_trim_m_s = _param_fw_airspd_trim.get();
+	airspeed_validated.true_airspeed_trim_m_s = calc_true_from_calibrated_airspeed(_param_fw_airspd_trim.get(),
+			_vehicle_air_data.rho);
 
 	switch (_valid_airspeed_index) {
 	case airspeed_index::DISABLED_INDEX:
diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c
index 4e00817deb..d40bef557d 100644
--- a/src/modules/airspeed_selector/airspeed_selector_params.c
+++ b/src/modules/airspeed_selector/airspeed_selector_params.c
@@ -1,29 +1,31 @@
 
 /**
- * Airspeed Selector: Wind estimator wind process noise
+ * Airspeed Selector: Wind estimator wind process noise noise spectral density
  *
  * Wind process noise of the internal wind estimator(s) of the airspeed selector.
+ * When unaided, the wind estimate uncertainty (1-sigma, in m/s) increases by this amount every second.
  *
  * @min 0
  * @max 1
- * @unit m/s^2
+ * @unit m/s^2/sqrt(Hz)
  * @decimal 2
  * @group Airspeed Validator
  */
-PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f);
+PARAM_DEFINE_FLOAT(ASPD_WIND_NSD, 1.e-2f);
 
 /**
- * Airspeed Selector: Wind estimator true airspeed scale process noise
+ * Airspeed Selector: Wind estimator true airspeed scale process noise spectral density
  *
  * Airspeed scale process noise of the internal wind estimator(s) of the airspeed selector.
+ * When unaided, the scale uncertainty (1-sigma, unitless) increases by this amount every second.
  *
  * @min 0
  * @max 0.1
- * @unit Hz
+ * @unit 1/s/sqrt(Hz)
  * @decimal 5
  * @group Airspeed Validator
  */
-PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001f);
+PARAM_DEFINE_FLOAT(ASPD_SCALE_NSD, 0.0001f);
 
 /**
  * Airspeed Selector: Wind estimator true airspeed measurement noise
@@ -149,14 +151,15 @@ PARAM_DEFINE_INT32(ASPD_PRIMARY, 1);
  * Note that the data missing check is enabled if any of the options is set.
  *
  * @min 0
- * @max 15
+ * @max 31
  * @bit 0 Only data missing check (triggers if more than 1s no data)
- * @bit 1 Data stuck (triggers if data is exactly constant for 2s)
+ * @bit 1 Data stuck (triggers if data is exactly constant for 2s in FW mode)
  * @bit 2 Innovation check (see ASPD_FS_INNOV)
  * @bit 3 Load factor check (triggers if measurement is below stall speed)
+ * @bit 4 Check for data variation in first 10s after sensor connection
  * @group Airspeed Validator
  */
-PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 7);
+PARAM_DEFINE_INT32(ASPD_DO_CHECKS, 23);
 
 /**
  * Enable fallback to sensor-less airspeed estimation
@@ -225,3 +228,17 @@ PARAM_DEFINE_INT32(ASPD_FS_T_STOP, 2);
  * @max 1000
  */
 PARAM_DEFINE_INT32(ASPD_FS_T_START, -1);
+
+/**
+ * Horizontal wind uncertainty threshold for synthetic airspeed.
+ *
+ * The synthetic airspeed estimate (from groundspeed and heading) will be declared valid
+ * as soon and as long the horizontal wind uncertainty drops below this value.
+ *
+ * @unit m/s
+ * @min 0.001
+ * @max 5
+ * @decimal 3
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_FLOAT(ASPD_WERR_THR, 0.55f);
diff --git a/src/modules/camera_feedback/CameraFeedback.cpp b/src/modules/camera_feedback/CameraFeedback.cpp
index 2eb1a88f74..35ac09296d 100644
--- a/src/modules/camera_feedback/CameraFeedback.cpp
+++ b/src/modules/camera_feedback/CameraFeedback.cpp
@@ -33,6 +33,8 @@
 
 #include "CameraFeedback.hpp"
 
+using namespace time_literals;
+
 CameraFeedback::CameraFeedback() :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::hp_default)
@@ -112,11 +114,39 @@ CameraFeedback::Run()
 		}
 
 		// Fill attitude data
-		// TODO : this needs to be rotated by camera orientation or set to gimbal orientation when available
-		capture.q[0] = att.q[0];
-		capture.q[1] = att.q[1];
-		capture.q[2] = att.q[2];
-		capture.q[3] = att.q[3];
+		gimbal_device_attitude_status_s gimbal{};
+
+		if (_gimbal_sub.copy(&gimbal) && (hrt_elapsed_time(&gimbal.timestamp) < 1_s)) {
+			if (gimbal.device_flags & gimbal_device_attitude_status_s::DEVICE_FLAGS_YAW_LOCK) {
+				// Gimbal yaw angle is absolute angle relative to North
+				capture.q[0] = gimbal.q[0];
+				capture.q[1] = gimbal.q[1];
+				capture.q[2] = gimbal.q[2];
+				capture.q[3] = gimbal.q[3];
+
+			} else {
+				// Gimbal quaternion frame is in the Earth frame rotated so that the x-axis is pointing
+				// forward (yaw relative to vehicle). Get heading from the vehicle attitude and combine it
+				// with the gimbal orientation.
+				const matrix::Eulerf euler_vehicle(matrix::Quatf(att.q));
+				const matrix::Quatf q_heading(matrix::Eulerf(0.0f, 0.0f, euler_vehicle(2)));
+				matrix::Quatf q_gimbal(gimbal.q);
+				q_gimbal = q_heading * q_gimbal;
+
+				capture.q[0] = q_gimbal(0);
+				capture.q[1] = q_gimbal(1);
+				capture.q[2] = q_gimbal(2);
+				capture.q[3] = q_gimbal(3);
+			}
+
+		} else {
+			// No gimbal orientation, use vehicle attitude
+			capture.q[0] = att.q[0];
+			capture.q[1] = att.q[1];
+			capture.q[2] = att.q[2];
+			capture.q[3] = att.q[3];
+		}
+
 		capture.result = 1;
 
 		_capture_pub.publish(capture);
diff --git a/src/modules/camera_feedback/CameraFeedback.hpp b/src/modules/camera_feedback/CameraFeedback.hpp
index b60368ef42..8b86e6eaef 100644
--- a/src/modules/camera_feedback/CameraFeedback.hpp
+++ b/src/modules/camera_feedback/CameraFeedback.hpp
@@ -55,6 +55,7 @@
 #include <uORB/topics/camera_trigger.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/gimbal_device_attitude_status.h>
 
 class CameraFeedback : public ModuleBase<CameraFeedback>, public ModuleParams, public px4::WorkItem
 {
@@ -81,6 +82,7 @@ private:
 
 	uORB::Subscription	_gpos_sub{ORB_ID(vehicle_global_position)};
 	uORB::Subscription	_att_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription	_gimbal_sub{ORB_ID(gimbal_device_attitude_status)};
 
 	uORB::Publication<camera_capture_s>	_capture_pub{ORB_ID(camera_capture)};
 
diff --git a/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp
index 6541569be5..bfcff4a3ae 100644
--- a/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp
+++ b/src/modules/commander/Arming/ArmAuthorization/ArmAuthorization.cpp
@@ -110,10 +110,10 @@ static uint8_t _auth_method_arm_req_check()
 		break;
 
 	case ARM_AUTH_MISSION_APPROVED:
-		return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 	default:
-		return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 	}
 
 	/* handling ARM_AUTH_IDLE */
@@ -148,7 +148,7 @@ static uint8_t _auth_method_arm_req_check()
 	}
 
 	return state == ARM_AUTH_MISSION_APPROVED ?
-	       vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+	       vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 }
 
 static uint8_t _auth_method_two_arm_check()
@@ -159,14 +159,14 @@ static uint8_t _auth_method_two_arm_check()
 		break;
 
 	case ARM_AUTH_MISSION_APPROVED:
-		return vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 	case ARM_AUTH_WAITING_AUTH:
 	case ARM_AUTH_WAITING_AUTH_WITH_ACK:
-		return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 
 	default:
-		return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 	}
 
 	/* handling ARM_AUTH_IDLE */
@@ -179,7 +179,7 @@ static uint8_t _auth_method_two_arm_check()
 
 	mavlink_log_info(mavlink_log_pub, "Arm auth: Requesting authorization...");
 
-	return vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
+	return vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 }
 
 uint8_t arm_auth_check()
@@ -188,7 +188,7 @@ uint8_t arm_auth_check()
 		return arm_check_method[_param_com_arm_auth_method]();
 	}
 
-	return vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+	return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 }
 
 void arm_auth_update(hrt_abstime now, bool param_update)
@@ -232,11 +232,11 @@ void arm_auth_update(hrt_abstime now, bool param_update)
 	    && command_ack.target_system == *system_id
 	    && command_ack.timestamp > auth_req_time) {
 		switch (command_ack.result) {
-		case vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS:
+		case vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS:
 			state = ARM_AUTH_WAITING_AUTH_WITH_ACK;
 			break;
 
-		case vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED:
+		case vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED:
 			mavlink_log_info(mavlink_log_pub,
 					 "Arm auth: Authorized for the next %" PRId32 " seconds",
 					 command_ack.result_param2);
@@ -244,12 +244,12 @@ void arm_auth_update(hrt_abstime now, bool param_update)
 			auth_timeout = command_ack.timestamp + (command_ack.result_param2 * 1000000);
 			return;
 
-		case vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED:
+		case vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
 			mavlink_log_critical(mavlink_log_pub, "Arm auth: Temporarily rejected");
 			state = ARM_AUTH_IDLE;
 			return;
 
-		case vehicle_command_ack_s::VEHICLE_RESULT_DENIED:
+		case vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED:
 		default:
 			switch (command_ack.result_param1) {
 			case vehicle_command_ack_s::ARM_AUTH_DENIED_REASON_NONE:
diff --git a/src/modules/commander/Arming/HealthFlags/HealthFlags.h b/src/modules/commander/Arming/HealthFlags/HealthFlags.h
index 04e5db1c38..950c207d24 100644
--- a/src/modules/commander/Arming/HealthFlags/HealthFlags.h
+++ b/src/modules/commander/Arming/HealthFlags/HealthFlags.h
@@ -77,6 +77,7 @@ struct subsystem_info_s {
 	static constexpr uint64_t SUBSYSTEM_TYPE_PREARM_CHECK = 1 << 28;
 	static constexpr uint64_t SUBSYSTEM_TYPE_OBSTACLE_AVOIDANCE = 1 << 29;
 	static constexpr uint64_t SUBSYSTEM_TYPE_PARACHUTE = 1ULL << 32;
+	static constexpr uint64_t SUBSYSTEM_TYPE_OPEN_DRONE_ID = 1ULL << 33;
 };
 
 void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status);
diff --git a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt b/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt
index edec715b23..cd99e6c2dc 100644
--- a/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt
+++ b/src/modules/commander/Arming/PreFlightCheck/CMakeLists.txt
@@ -50,8 +50,10 @@ px4_add_library(PreFlightCheck
 	checks/manualControlCheck.cpp
 	checks/modeCheck.cpp
 	checks/cpuResourceCheck.cpp
+	checks/externalUpdateCheck.cpp
 	checks/sdcardCheck.cpp
 	checks/parachuteCheck.cpp
+	checks/openDroneIDCheck.cpp
 )
 target_include_directories(PreFlightCheck PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
 target_link_libraries(PreFlightCheck PUBLIC ArmAuthorization HealthFlags sensor_calibration)
diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
index a36d1b7581..d697acf11a 100644
--- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
+++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.cpp
@@ -59,6 +59,7 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 
 	bool failed = false;
 
+	failed = failed || !externalUpdateCheck(mavlink_log_pub, report_failures);
 	failed = failed || !airframeCheck(mavlink_log_pub, status);
 	failed = failed || !sdcardCheck(mavlink_log_pub, status_flags.sd_card_detected_once, report_failures);
 
@@ -100,8 +101,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		param_get(param_find("SYS_HAS_BARO"), &sys_has_baro);
 
 		if (sys_has_baro == 1) {
-			static_cast<void>(sensorAvailabilityCheck(report_failures, max_mandatory_baro_count,
-					  mavlink_log_pub, status, baroCheck));
+			failed |= !sensorAvailabilityCheck(report_failures, max_mandatory_baro_count,
+							   mavlink_log_pub, status, baroCheck);
 		}
 	}
 
@@ -119,8 +120,8 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		param_get(param_find("SYS_HAS_NUM_DIST"), &sys_has_num_dist_sens);
 
 		if (sys_has_num_dist_sens > 0) {
-			static_cast<void>(sensorAvailabilityCheck(report_failures, sys_has_num_dist_sens,
-					  mavlink_log_pub, status, distSensCheck));
+			failed |= !sensorAvailabilityCheck(report_failures, sys_has_num_dist_sens,
+							   mavlink_log_pub, status, distSensCheck);
 		}
 
 	}
@@ -223,11 +224,23 @@ bool PreFlightCheck::preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_statu
 		failed = true;
 	}
 
+	if (status.failsafe) {
+		if (report_failures) {
+			mavlink_log_critical(mavlink_log_pub, "Arming Denied: vehicle is in failsafe mode. Switch flight mode first");
+		}
+
+		failed = true;
+	}
+
 	failed = failed || !manualControlCheck(mavlink_log_pub, report_failures);
 	failed = failed || !modeCheck(mavlink_log_pub, report_failures, status);
 	failed = failed || !cpuResourceCheck(mavlink_log_pub, report_failures);
 	failed = failed || !parachuteCheck(mavlink_log_pub, report_failures, status_flags);
 
+	if (prearm) {
+		failed = failed || !openDroneIDCheck(mavlink_log_pub, report_failures, status_flags);
+	}
+
 	/* Report status */
 	return !failed;
 }
diff --git a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
index 58432d425b..8b3142f5d7 100644
--- a/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
+++ b/src/modules/commander/Arming/PreFlightCheck/PreFlightCheck.hpp
@@ -135,7 +135,10 @@ private:
 	static bool modeCheck(orb_advert_t *mavlink_log_pub, const bool report_fail, const vehicle_status_s &status);
 	static bool airframeCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status);
 	static bool cpuResourceCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
+	static bool externalUpdateCheck(orb_advert_t *mavlink_log_pub, const bool report_fail);
 	static bool sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_detected_once, const bool report_fail);
 	static bool parachuteCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
 				   const vehicle_status_flags_s &status_flags);
+	static bool openDroneIDCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
+				     const vehicle_status_flags_s &status_flags);
 };
diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
index ff8b56acda..d7ed9265f2 100644
--- a/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
+++ b/src/modules/commander/Arming/PreFlightCheck/checks/ekf2Check.cpp
@@ -164,7 +164,7 @@ bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &
 	}
 
 	// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
-	if (sys_has_gps == 1) {
+	if (sys_has_gps == 1 && !arm_without_gps) {
 		const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
 		const bool ekf_gps_check_fail = status.gps_check_fail_flags > 0;
 
diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/externalUpdateCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/externalUpdateCheck.cpp
new file mode 100644
index 0000000000..82a2670678
--- /dev/null
+++ b/src/modules/commander/Arming/PreFlightCheck/checks/externalUpdateCheck.cpp
@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 "../PreFlightCheck.hpp"
+#include <lib/parameters/param.h>
+#include <systemlib/mavlink_log.h>
+#include <stdio.h>
+#include <string.h>
+#ifdef __PX4_DARWIN
+#include <sys/param.h>
+#include <sys/mount.h>
+#else
+#include <sys/statfs.h>
+#endif
+
+/* Error codes
+ *
+ * -1 = File does not exists
+ * -2 = FMU card missing
+ *  0 = Parsing failure
+ *
+ */
+
+static constexpr char ext_component_path[] = PX4_STORAGEDIR "/ext_component_updated";
+
+bool PreFlightCheck::externalUpdateCheck(orb_advert_t *mavlink_log_pub, const bool report_fail)
+{
+	static bool update_succeeded = false;
+	static bool already_executed = false;
+	static int error_code = -1;
+	int32_t enable_check = -1;
+	struct statfs statfs_buf;
+	uint64_t total_bytes = 0;
+	param_get(param_find("COM_EXT_COMP_EN"), &enable_check);
+
+	if (enable_check == 0) {
+		return true;
+	}
+
+	if (!already_executed) {
+		already_executed = true;
+
+		if (statfs(ext_component_path, &statfs_buf) == 0) {
+			total_bytes = (uint64_t)statfs_buf.f_blocks * statfs_buf.f_bsize;
+		}
+
+		if (total_bytes == 0) { // on NuttX we get 0 total bytes if no SD card is inserted
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Update Check Fail: Missing FMU SD Card");
+			}
+
+			error_code = -2;
+			return true; // Still allow arming when the SD card is missing
+		}
+
+		FILE *fp = fopen(ext_component_path, "r");
+
+		if (fp == nullptr) {
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Update Fail: External update file not found");
+			}
+
+			return false;
+		}
+
+		char reported_status[64];
+		int reported_code = 0;
+
+		if (fscanf(fp, "%s %d", reported_status, &reported_code) == 2) {
+
+			if (!strcmp(reported_status, "SUCCESS")) {
+				update_succeeded = true;
+
+			} else if (!strcmp(reported_status, "FAIL")) {
+				error_code = reported_code;
+			}
+
+		} else {
+			error_code = 0;
+		}
+	}
+
+	bool result = update_succeeded;
+
+	if (report_fail && !update_succeeded) {
+		if (error_code == -2) {
+			mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Software update check failed.");
+			result = true; // Still allow arming when the SD card is missing
+
+		} else {
+			mavlink_log_critical(mavlink_log_pub, "Update Fail: Software update failed, error code %d.",
+					     error_code);
+		}
+	}
+
+	return result;
+}
diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/modeCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/modeCheck.cpp
index fa69e77692..9997c2bc90 100644
--- a/src/modules/commander/Arming/PreFlightCheck/checks/modeCheck.cpp
+++ b/src/modules/commander/Arming/PreFlightCheck/checks/modeCheck.cpp
@@ -53,6 +53,9 @@ bool PreFlightCheck::modeCheck(orb_advert_t *mavlink_log_pub, const bool report_
 	case vehicle_status_s::NAVIGATION_STATE_STAB:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
+
+	// TODO: disallow init mode when workflow changed from first arming to first switching mode
+	case vehicle_status_s::NAVIGATION_STATE_INIT:
 		break;
 
 	default:
diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/openDroneIDCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/openDroneIDCheck.cpp
new file mode 100644
index 0000000000..195ee879bb
--- /dev/null
+++ b/src/modules/commander/Arming/PreFlightCheck/checks/openDroneIDCheck.cpp
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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 "../PreFlightCheck.hpp"
+
+#include <lib/parameters/param.h>
+#include <lib/systemlib/mavlink_log.h>
+#include <px4_platform_common/events.h>
+
+bool PreFlightCheck::openDroneIDCheck(orb_advert_t *mavlink_log_pub, const bool report_fail,
+				      const vehicle_status_flags_s &status_flags)
+{
+	bool success = true;
+
+	int32_t param_com_prearm_odid = false;
+	param_get(param_find("COM_PREARM_ODID"), &param_com_prearm_odid);
+	const bool open_drone_id_required = param_com_prearm_odid != 0;
+
+	if (open_drone_id_required) {
+		if (!status_flags.open_drone_id_system_present) {
+			success = false;
+
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Fail: OpenDroneID system missing");
+				events::send(events::ID("commander_open_drone_id_missing"), events::Log::Critical, "OpenDroneID system missing");
+			}
+
+		} else if (!status_flags.open_drone_id_system_healthy) {
+			success = false;
+
+			if (report_fail) {
+				mavlink_log_critical(mavlink_log_pub, "Fail: OpenDroneID system not ready");
+				events::send(events::ID("commander_open_drone_id_system_not_ready"), events::Log::Critical,
+					     "OpenDroneID system not ready");
+			}
+		}
+	}
+
+	return success;
+}
diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp
index df7eea0d1a..b08e29e4ef 100644
--- a/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp
+++ b/src/modules/commander/Arming/PreFlightCheck/checks/preArmCheck.cpp
@@ -190,6 +190,15 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
 		}
 	}
 
+	if (status_flags.onboard_logging_system_required && !status_flags.onboard_logging_system_valid) {
+		if (prearm_ok) {
+			if (report_fail) { mavlink_log_critical(mavlink_log_pub, "Arming denied! Onboard logging system not ready"); }
+
+		}
+
+		prearm_ok = false;
+	}
+
 	if (status.is_vtol) {
 
 		if (status.in_transition_mode) {
@@ -221,7 +230,7 @@ bool PreFlightCheck::preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_st
 	// Arm Requirements: authorization
 	// check last, and only if everything else has passed
 	if (arm_requirements.arm_authorization && prearm_ok) {
-		if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) {
+		if (arm_auth_check() != vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
 			// feedback provided in arm_auth_check
 			prearm_ok = false;
 		}
diff --git a/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp b/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp
index bfcd4d5d34..3cefd2dc2f 100644
--- a/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp
+++ b/src/modules/commander/Arming/PreFlightCheck/checks/sdcardCheck.cpp
@@ -34,6 +34,8 @@
 #include "../PreFlightCheck.hpp"
 #include <lib/parameters/param.h>
 #include <systemlib/mavlink_log.h>
+#include <dirent.h>
+#include <string.h>
 
 #ifdef __PX4_DARWIN
 #include <sys/param.h>
@@ -70,5 +72,47 @@ bool PreFlightCheck::sdcardCheck(orb_advert_t *mavlink_log_pub, bool &sd_card_de
 		}
 	}
 
+#ifdef __PX4_NUTTX
+	// Check for hardfault files
+	static bool hardfault_checked_once = false; // TODO: make this a class member when porting to newer PX4
+	static bool hardfault_file_present = false;
+
+	if (!hardfault_checked_once) {
+		hardfault_checked_once = true;
+
+		int32_t param_com_arm_hardfault_check{0};
+		param_get(param_find("COM_ARM_HFLT_CHK"), &param_com_arm_hardfault_check);
+
+		if (param_com_arm_hardfault_check > 0) {
+
+			DIR *dp = opendir(PX4_STORAGEDIR);
+
+			if (dp != nullptr) {
+
+				struct dirent *result;
+
+				while ((result = readdir(dp)) && !hardfault_file_present) {
+
+					// Check for pattern fault_*.log
+					if (strncmp("fault_", result->d_name, 6) == 0 && strcmp(result->d_name + strlen(result->d_name) - 4, ".log") == 0) {
+						hardfault_file_present = true;
+					}
+				}
+
+				closedir(dp);
+			}
+		}
+	}
+
+	if (hardfault_file_present) {
+		if (report_fail) {
+			mavlink_log_critical(mavlink_log_pub, "Crash dumps present on SD, vehicle needs service");
+		}
+
+		success = false;
+	}
+
+#endif
+
 	return success;
 }
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index d51ea0aeb2..ce9bd471d2 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -653,9 +653,11 @@ static inline navigation_mode_t navigation_mode(uint8_t main_state)
 	case commander_state_s::MAIN_STATE_ORBIT: return navigation_mode_t::orbit;
 
 	case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff;
+
+	case commander_state_s::MAIN_STATE_INIT: return navigation_mode_t::init;
 	}
 
-	static_assert(commander_state_s::MAIN_STATE_MAX - 1 == (int)navigation_mode_t::auto_vtol_takeoff,
+	static_assert(commander_state_s::MAIN_STATE_MAX - 1 == (int)navigation_mode_t::init,
 		      "enum definition mismatch");
 
 	return navigation_mode_t::unknown;
@@ -730,10 +732,10 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
 		} else if (calling_reason == arm_disarm_reason_t::rc_stick
 			   || calling_reason == arm_disarm_reason_t::rc_switch
 			   || calling_reason == arm_disarm_reason_t::rc_button) {
-			mavlink_log_critical(&_mavlink_log_pub, "Arming denied: switch to manual mode first\t");
-			events::send(events::ID("commander_arm_denied_not_manual"),
+			mavlink_log_critical(&_mavlink_log_pub, "Arming denied: switch flight mode\t");
+			events::send(events::ID("commander_arm_denied_flight_mode"),
 			{events::Log::Critical, events::LogInternal::Info},
-			"Arming denied: switch to manual mode first");
+			"Arming denied: switch flight mode");
 			tune_negative(true);
 			return TRANSITION_DENIED;
 		}
@@ -781,7 +783,7 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 					     || (calling_reason == arm_disarm_reason_t::rc_switch)
 					     || (calling_reason == arm_disarm_reason_t::rc_button);
 
-		if (!landed && !(mc_manual_thrust_mode && commanded_by_rc)) {
+		if (!landed && !(mc_manual_thrust_mode && commanded_by_rc && _param_com_disarm_man.get())) {
 			if (calling_reason != arm_disarm_reason_t::rc_stick) {
 				mavlink_log_critical(&_mavlink_log_pub, "Disarming denied! Not landed\t");
 				events::send(events::ID("commander_disarming_denied_not_landed"),
@@ -810,6 +812,10 @@ transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason, bool f
 
 		_status_changed = true;
 
+		// reset flight time and wind exceeded flags to false upon disarm
+		_vehicle_status_flags.max_flight_time_exceeded = false;
+		_vehicle_status_flags.max_wind_speed_exceeded = false;
+
 	} else if (arming_res == TRANSITION_DENIED) {
 		tune_negative(true);
 	}
@@ -821,6 +827,8 @@ Commander::Commander() :
 	ModuleParams(nullptr),
 	_failure_detector(this)
 {
+	_commander_state.main_state = commander_state_s::MAIN_STATE_INIT;
+
 	_vehicle_land_detected.landed = true;
 
 	_vehicle_status.system_id = 1;
@@ -843,12 +851,13 @@ Commander::Commander() :
 	_vehicle_status_flags.offboard_control_signal_lost = true;
 
 	_vehicle_status_flags.power_input_valid = true;
+	_vehicle_status_flags.onboard_logging_system_valid = false;
 
 	// default for vtol is rotary wing
 	_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
 
 	_vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME);
-	_vehicle_gps_position_valid.set_hysteresis_time_from(true, GPS_VALID_TIME);
+	_vehicle_gps_position_valid.set_hysteresis_time_from(true, 0);
 
 	_param_mav_comp_id = param_find("MAV_COMP_ID");
 	_param_mav_sys_id = param_find("MAV_SYS_ID");
@@ -874,7 +883,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	}
 
 	/* result of the command */
-	unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+	unsigned cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 
 	/* request to set different system mode */
 	switch (cmd.command) {
@@ -891,10 +900,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 							       _vehicle_status_flags, _commander_state);
 
 				if ((main_ret != TRANSITION_DENIED)) {
-					cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+					cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
-					cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 					mavlink_log_critical(&_mavlink_log_pub, "Reposition command rejected\t");
 					/* EVENT
 					 * @description Check for a valid position estimate
@@ -905,7 +914,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				}
 
 			} else {
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 			}
 		}
 		break;
@@ -1006,10 +1015,11 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			}
 
 			if (main_ret != TRANSITION_DENIED) {
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+				_vehicle_status.failsafe_but_user_took_over = true;
 
 			} else {
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 		}
 		break;
@@ -1053,10 +1063,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				}
 
 				if (arming_res == TRANSITION_DENIED) {
-					cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+					cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 
 				} else {
-					cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+					cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 					/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
 					if ((arming_action == vehicle_command_s::ARMING_ACTION_ARM) && (arming_res == TRANSITION_CHANGED)
@@ -1097,7 +1107,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				PX4_WARN("disabling failsafe and lockdown");
 			}
 
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 		}
 		break;
 
@@ -1108,10 +1118,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				if (use_current) {
 					/* use current position */
 					if (set_home_position()) {
-						cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+						cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 					} else {
-						cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+						cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 					}
 
 				} else {
@@ -1144,20 +1154,20 @@ Commander::handle_command(const vehicle_command_s &cmd)
 							/* mark home position as set */
 							_vehicle_status_flags.home_position_valid = _home_position_pub.update(home);
 
-							cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+							cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 						} else {
-							cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+							cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 						}
 
 					} else {
-						cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+						cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 					}
 				}
 
 			} else {
 				// COM_HOME_EN disabled
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 			}
 		}
 		break;
@@ -1169,7 +1179,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 					_commander_state)) {
 				mavlink_log_info(&_mavlink_log_pub, "Returning to launch\t");
 				events::send(events::ID("commander_rtl"), events::Log::Info, "Returning to launch");
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
 				mavlink_log_critical(&_mavlink_log_pub, "Return to launch denied\t");
@@ -1178,7 +1188,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				 */
 				events::send(events::ID("commander_rtl_denied"), {events::Log::Critical, events::LogInternal::Info},
 					     "Return to launch denied");
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 		}
 		break;
@@ -1188,10 +1198,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			if (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_TAKEOFF,
 					_vehicle_status_flags,
 					_commander_state)) {
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else if (_commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF) {
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
 				mavlink_log_critical(&_mavlink_log_pub, "Takeoff denied! Please disarm and retry\t");
@@ -1200,7 +1210,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				 */
 				events::send(events::ID("commander_takeoff_denied"), {events::Log::Critical, events::LogInternal::Info},
 					     "Takeoff denied! Please disarm and retry");
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 		}
 		break;
@@ -1211,14 +1221,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		if (TRANSITION_CHANGED == main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF,
 				_vehicle_status_flags,
 				_commander_state)) {
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 		} else if (_commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF) {
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 		} else {
 			mavlink_log_critical(&_mavlink_log_pub, "VTOL Takeoff denied! Please disarm and retry");
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 		}
 
 		break;
@@ -1230,7 +1240,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				mavlink_log_info(&_mavlink_log_pub, "Landing at current position\t");
 				events::send(events::ID("commander_landing_current_pos"), events::Log::Info,
 					     "Landing at current position");
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
 				mavlink_log_critical(&_mavlink_log_pub, "Landing denied! Please land manually\t");
@@ -1239,7 +1249,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				 */
 				events::send(events::ID("commander_landing_current_pos_denied"), {events::Log::Critical, events::LogInternal::Info},
 					     "Landing denied! Please land manually");
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 		}
 		break;
@@ -1251,7 +1261,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				mavlink_log_info(&_mavlink_log_pub, "Precision landing\t");
 				events::send(events::ID("commander_landing_prec_land"), events::Log::Info,
 					     "Landing using precision landing");
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			} else {
 				mavlink_log_critical(&_mavlink_log_pub, "Precision landing denied! Please land manually\t");
@@ -1260,14 +1270,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
 				 */
 				events::send(events::ID("commander_landing_prec_land_denied"), {events::Log::Critical, events::LogInternal::Info},
 					     "Precision landing denied! Please land manually");
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 			}
 		}
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_MISSION_START: {
 
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+			cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 
 			// check if current mission and first item are valid
 			if (_vehicle_status_flags.auto_mission_available) {
@@ -1281,10 +1291,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 							_commander_state))
 					    && (TRANSITION_DENIED != arm(arm_disarm_reason_t::mission_start))) {
 
-						cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+						cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 					} else {
-						cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+						cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 						mavlink_log_critical(&_mavlink_log_pub, "Mission start denied\t");
 						/* EVENT
 						 * @description Check for a valid position estimate
@@ -1305,7 +1315,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_CONTROL_HIGH_LATENCY: {
 			// if no high latency telemetry exists send a failed acknowledge
 			if (_high_latency_datalink_heartbeat > _boot_timestamp) {
-				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
 				mavlink_log_critical(&_mavlink_log_pub, "Control high latency failed! Telemetry unavailable\t");
 				events::send(events::ID("commander_ctrl_high_latency_failed"), {events::Log::Critical, events::LogInternal::Info},
 					     "Control high latency failed! Telemetry unavailable");
@@ -1313,32 +1323,58 @@ Commander::handle_command(const vehicle_command_s &cmd)
 		}
 		break;
 
-	case vehicle_command_s::VEHICLE_CMD_DO_ORBIT:
+	case vehicle_command_s::VEHICLE_CMD_DO_ORBIT: {
 
-		transition_result_t main_ret;
+			transition_result_t main_ret;
 
-		if (_vehicle_status.in_transition_mode) {
-			main_ret = TRANSITION_DENIED;
+			if (_vehicle_status.in_transition_mode) {
+				main_ret = TRANSITION_DENIED;
 
-		} else if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-			// for fixed wings the behavior of orbit is the same as loiter
-			main_ret = main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER,
-							 _vehicle_status_flags, _commander_state);
+			} else if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+				// // for fixed wings the behavior of orbit is the same as loiter
+				main_ret = main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER,
+								 _vehicle_status_flags, _commander_state);
 
-		} else {
-			// Switch to orbit state and let the orbit task handle the command further
-			main_ret = main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_ORBIT, _vehicle_status_flags,
-							 _commander_state);
+			} else {
+				// Switch to orbit state and let the orbit task handle the command further
+				main_ret = main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_ORBIT, _vehicle_status_flags,
+								 _commander_state);
+			}
+
+			if ((main_ret != TRANSITION_DENIED)) {
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				mavlink_log_critical(&_mavlink_log_pub, "Orbit command rejected");
+			}
 		}
+		break;
 
-		if ((main_ret != TRANSITION_DENIED)) {
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+	case vehicle_command_s::VEHICLE_CMD_DO_FIGUREEIGHT: {
 
-		} else {
-			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-			mavlink_log_critical(&_mavlink_log_pub, "Orbit command rejected");
+			if (!((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) || (_vehicle_status.is_vtol))) {
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+				mavlink_log_critical(&_mavlink_log_pub, "Figure 8 command only available for fixed wing and vtol vehicles.");
+				break;
+			}
+
+			transition_result_t main_ret = TRANSITION_DENIED;
+
+			if ((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) &&
+			    (!_vehicle_status.in_transition_mode)) {
+				main_ret = main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER,
+								 _vehicle_status_flags, _commander_state);
+			}
+
+			if (main_ret != TRANSITION_DENIED) {
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
+
+			} else {
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				mavlink_log_critical(&_mavlink_log_pub, "Figure 8 command rejected, Only available in fixed wing mode.");
+			}
 		}
-
 		break;
 
 	case vehicle_command_s::VEHICLE_CMD_DO_MOTOR_TEST:
@@ -1355,13 +1391,13 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 			if (param1 == 0) {
 				// 0: Do nothing for autopilot
-				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 #if defined(CONFIG_BOARDCTL_RESET)
 
 			} else if ((param1 == 1) && shutdown_if_allowed() && (px4_reboot_request(false, 400_ms) == 0)) {
 				// 1: Reboot autopilot
-				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 				while (1) { px4_usleep(1); }
 
@@ -1371,7 +1407,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 			} else if ((param1 == 2) && shutdown_if_allowed() && (px4_shutdown_request(400_ms) == 0)) {
 				// 2: Shutdown autopilot
-				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 				while (1) { px4_usleep(1); }
 
@@ -1381,14 +1417,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 			} else if ((param1 == 3) && shutdown_if_allowed() && (px4_reboot_request(true, 400_ms) == 0)) {
 				// 3: Reboot autopilot and keep it in the bootloader until upgraded.
-				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 				while (1) { px4_usleep(1); }
 
 #endif // CONFIG_BOARDCTL_RESET
 
 			} else {
-				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
 			}
 		}
 
@@ -1399,7 +1435,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
-				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
 
 			} else {
 
@@ -1413,14 +1449,14 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						(cmd.from_external ? arm_disarm_reason_t::command_external : arm_disarm_reason_t::command_internal))
 				   ) {
 
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
 					break;
 
 				}
 
 				if ((int)(cmd.param1) == 1) {
 					/* gyro calibration */
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status_flags.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::GyroCalibration);
 
@@ -1432,19 +1468,19 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 				} else if ((int)(cmd.param2) == 1) {
 					/* magnetometer calibration */
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status_flags.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::MagCalibration);
 
 				} else if ((int)(cmd.param3) == 1) {
 					/* baro calibration */
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status_flags.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::BaroCalibration);
 
 				} else if ((int)(cmd.param4) == 1) {
 					/* RC calibration */
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					/* disable RC control input completely */
 					_vehicle_status_flags.rc_calibration_in_progress = true;
 					mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input\t");
@@ -1453,39 +1489,39 @@ Commander::handle_command(const vehicle_command_s &cmd)
 
 				} else if ((int)(cmd.param4) == 2) {
 					/* RC trim calibration */
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status_flags.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::RCTrimCalibration);
 
 				} else if ((int)(cmd.param5) == 1) {
 					/* accelerometer calibration */
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status_flags.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::AccelCalibration);
 
 				} else if ((int)(cmd.param5) == 2) {
 					// board offset calibration
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status_flags.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::LevelCalibration);
 
 				} else if ((int)(cmd.param5) == 4) {
 					// accelerometer quick calibration
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status_flags.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::AccelCalibrationQuick);
 
 				} else if ((int)(cmd.param6) == 1 || (int)(cmd.param6) == 2) {
 					// TODO: param6 == 1 is deprecated, but we still accept it for a while (feb 2017)
 					/* airspeed calibration */
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_vehicle_status_flags.calibration_enabled = true;
 					_worker_thread.startTask(WorkerThread::Request::AirspeedCalibration);
 
 				} else if ((int)(cmd.param7) == 1) {
 					/* do esc calibration */
 					if (check_battery_disconnected(&_mavlink_log_pub)) {
-						answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+						answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 						if (_safety.isButtonAvailable() && !_safety.isSafetyOff()) {
 							mavlink_log_critical(&_mavlink_log_pub, "ESC calibration denied! Press safety button first\t");
@@ -1499,7 +1535,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
 						}
 
 					} else {
-						answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
+						answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
 					}
 
 				} else if ((int)(cmd.param4) == 0) {
@@ -1512,10 +1548,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 							     "Calibration: Restoring RC input");
 					}
 
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 				} else {
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
 				}
 			}
 
@@ -1527,10 +1563,10 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
-				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
 
 			} else {
-				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 				// parameter 1: Heading   (degrees)
 				// parameter 3: Latitude  (degrees)
 				// parameter 4: Longitude (degrees)
@@ -1562,28 +1598,28 @@ Commander::handle_command(const vehicle_command_s &cmd)
 			if (_arm_state_machine.isArmed() || _arm_state_machine.isShutdown() || _worker_thread.isBusy()) {
 
 				// reject if armed or shutting down
-				answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
 
 			} else {
 
 				if (((int)(cmd.param1)) == 0) {
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamLoadDefault);
 
 				} else if (((int)(cmd.param1)) == 1) {
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamSaveDefault);
 
 				} else if (((int)(cmd.param1)) == 2) {
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamResetAllConfig);
 
 				} else if (((int)(cmd.param1)) == 3) {
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamResetSensorFactory);
 
 				} else if (((int)(cmd.param1)) == 4) {
-					answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 					_worker_thread.startTask(WorkerThread::Request::ParamResetAll);
 				}
 			}
@@ -1629,17 +1665,19 @@ Commander::handle_command(const vehicle_command_s &cmd)
 	case vehicle_command_s::VEHICLE_CMD_CONFIGURE_ACTUATOR:
 	case vehicle_command_s::VEHICLE_CMD_DO_SET_ACTUATOR:
 	case vehicle_command_s::VEHICLE_CMD_REQUEST_MESSAGE:
+	case vehicle_command_s::VEHICLE_CMD_DO_WINCH:
+	case vehicle_command_s::VEHICLE_CMD_DO_GRIPPER:
 		/* ignore commands that are handled by other parts of the system */
 		break;
 
 	default:
 		/* Warn about unsupported commands, this makes sense because only commands
 		 * to this component ID (or all) are passed by mavlink. */
-		answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
+		answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
 		break;
 	}
 
-	if (cmd_result != vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
+	if (cmd_result != vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* already warned about unsupported commands in "default" case */
 		answer_command(cmd, cmd_result);
 	}
@@ -1651,11 +1689,11 @@ unsigned
 Commander::handle_command_motor_test(const vehicle_command_s &cmd)
 {
 	if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
-		return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 	}
 
 	if (_param_com_mot_test_en.get() != 1) {
-		return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 	}
 
 	test_motor_s test_motor{};
@@ -1665,13 +1703,13 @@ Commander::handle_command_motor_test(const vehicle_command_s &cmd)
 	int throttle_type = (int)(cmd.param2 + 0.5f);
 
 	if (throttle_type != 0) { // 0: MOTOR_TEST_THROTTLE_PERCENT
-		return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 	}
 
 	int motor_count = (int)(cmd.param5 + 0.5);
 
 	if (motor_count > 1) {
-		return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 	}
 
 	test_motor.action = test_motor_s::ACTION_RUN;
@@ -1692,18 +1730,18 @@ Commander::handle_command_motor_test(const vehicle_command_s &cmd)
 	test_motor.driver_instance = 0; // the mavlink command does not allow to specify the instance, so set to 0 for now
 	_test_motor_pub.publish(test_motor);
 
-	return vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+	return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 }
 
 unsigned
 Commander::handle_command_actuator_test(const vehicle_command_s &cmd)
 {
 	if (_arm_state_machine.isArmed() || (_safety.isButtonAvailable() && !_safety.isSafetyOff())) {
-		return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 	}
 
 	if (_param_com_mot_test_en.get() != 1) {
-		return vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+		return vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 	}
 
 	actuator_test_s actuator_test{};
@@ -1723,7 +1761,7 @@ Commander::handle_command_actuator_test(const vehicle_command_s &cmd)
 			actuator_test.function = actuator_test.function - first_servo_function + actuator_test_s::FUNCTION_SERVO1;
 
 		} else {
-			return vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
+			return vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 		}
 
 	} else {
@@ -1748,7 +1786,7 @@ Commander::handle_command_actuator_test(const vehicle_command_s &cmd)
 	}
 
 	_actuator_test_pub.publish(actuator_test);
-	return vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+	return vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 }
 
 void Commander::executeActionRequest(const action_request_s &action_request)
@@ -1788,7 +1826,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 		break;
 
 	case action_request_s::ACTION_UNKILL:
-		if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && _actuator_armed.manual_lockdown) {
+		if (_actuator_armed.manual_lockdown) {
 			mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged\t");
 			events::send(events::ID("commander_kill_sw_disengaged"), events::Log::Info, "Kill-switch disengaged");
 			_status_changed = true;
@@ -1798,7 +1836,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 		break;
 
 	case action_request_s::ACTION_KILL:
-		if (arm_disarm_reason == arm_disarm_reason_t::rc_switch && !_actuator_armed.manual_lockdown) {
+		if (!_actuator_armed.manual_lockdown) {
 			const char kill_switch_string[] = "Kill-switch engaged\t";
 			events::LogLevels log_levels{events::Log::Info};
 
@@ -1823,6 +1861,7 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 
 		// if there's never been a mode change force RC switch as initial state
 		if (action_request.source == action_request_s::SOURCE_RC_MODE_SLOT
+		    && (_param_rc_map_fltm_btn.get() == 0)
 		    && !_arm_state_machine.isArmed() && (_commander_state.main_state_changes == 0)
 		    && (action_request.mode == commander_state_s::MAIN_STATE_ALTCTL
 			|| action_request.mode == commander_state_s::MAIN_STATE_POSCTL)) {
@@ -1830,6 +1869,8 @@ void Commander::executeActionRequest(const action_request_s &action_request)
 			_commander_state.main_state_changes++;
 		}
 
+		_vehicle_status.failsafe_but_user_took_over = true;
+
 		int ret = main_state_transition(_vehicle_status, action_request.mode, _vehicle_status_flags, _commander_state);
 
 		if (ret == transition_result_t::TRANSITION_DENIED) {
@@ -2105,6 +2146,7 @@ void Commander::updateParameters()
 
 
 	_vehicle_status_flags.avoidance_system_required = _param_com_obs_avoid.get();
+	_vehicle_status_flags.onboard_logging_system_required = _param_com_arm_wo_ob_logger.get();
 
 	_arm_requirements.arm_authorization = _param_arm_auth_required.get();
 	_arm_requirements.esc_check = _param_escs_checks_required.get();
@@ -2215,12 +2257,41 @@ Commander::run()
 			if (!_arm_state_machine.isArmed()) {
 				updateParameters();
 
+#ifdef PX4_EXPORT_CONTROLLED_BUILD
+
+				// If export control build flag is set, restrict COM_FLT_TIME_MAX to range (1s, EXPORT_RESTRICTED_MAX_FLIGHT_TIME)
+				if (_param_com_flt_time_max.get() > EXPORT_RESTRICTED_MAX_FLIGHT_TIME || _param_com_flt_time_max.get() < 1) {
+					_param_com_flt_time_max.set(EXPORT_RESTRICTED_MAX_FLIGHT_TIME);
+					_param_com_flt_time_max.commit();
+					mavlink_log_critical(&_mavlink_log_pub, "COM_FLT_TIME_MAX constrained to range (1s, 3540s)\t");
+					/* EVENT
+					* @description <param>COM_FLT_TIME_MAX</param> is set to {1:.0}.
+					*/
+					events::send<uint32_t>(events::ID("com_export_restriced_flight_time_limit"), events::Log::Warning,
+							       "COM_FLT_TIME_MAX constrained to range (1s, {1}s)", EXPORT_RESTRICTED_MAX_FLIGHT_TIME);
+				}
+
+				// If export control build flag is set, restrict max wind to range (0.1m/s, EXPORT_RESTRICTED_MAX_WIND)
+				if (_param_com_wind_max.get() > EXPORT_RESTRICTED_MAX_WIND || _param_com_wind_max.get() < 0.1f) {
+					_param_com_wind_max.set(EXPORT_RESTRICTED_MAX_WIND);
+					_param_com_wind_max.commit();
+					mavlink_log_critical(&_mavlink_log_pub, "COM_WIND_MAX constrained to range (0.1m/s, %.1fm/s)\t",
+							     (double)EXPORT_RESTRICTED_MAX_WIND);
+					/* EVENT
+					* @description <param>COM_WIND_MAX</param> is set to {1:.0}.
+					*/
+					events::send<float>(events::ID("com_export_restriced_wind_limit"), events::Log::Warning,
+							    "COM_WIND_MAX constrained to range (0.1m/s, {1:.1}m/s)", EXPORT_RESTRICTED_MAX_WIND);
+				}
+
+#endif
 				_status_changed = true;
 			}
 		}
 
 		/* Update OA parameter */
 		_vehicle_status_flags.avoidance_system_required = _param_com_obs_avoid.get();
+		_vehicle_status_flags.onboard_logging_system_required = _param_com_arm_wo_ob_logger.get();
 
 #if defined(BOARD_HAS_POWER_CONTROL)
 
@@ -2519,18 +2590,23 @@ Commander::run()
 				}
 			}
 
-			// Transition main state to loiter or auto-mission after takeoff is completed.
 			if (_arm_state_machine.isArmed() && !_vehicle_land_detected.landed
-			    && (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
-				_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
-			    && (mission_result.timestamp >= _vehicle_status.nav_state_timestamp)
-			    && mission_result.finished) {
+			    && (mission_result.timestamp >= _vehicle_status.nav_state_timestamp) && mission_result.finished) {
 
-				if ((_param_takeoff_finished_action.get() == 1) && _vehicle_status_flags.auto_mission_available) {
-					main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _vehicle_status_flags,
-							      _commander_state);
+				if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
+				    || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
+					// Transition mode to loiter or auto-mission after takeoff is completed.
+					if ((_param_takeoff_finished_action.get() == 1) && _vehicle_status_flags.auto_mission_available) {
+						main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _vehicle_status_flags,
+								      _commander_state);
 
-				} else {
+					} else {
+						main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _vehicle_status_flags,
+								      _commander_state);
+					}
+
+				} else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
+					// Transition to loiter when the mission is cleared and/or finished, and we are still in mission mode.
 					main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _vehicle_status_flags,
 							      _commander_state);
 				}
@@ -2543,7 +2619,8 @@ Commander::run()
 			_vehicle_status.geofence_violated = _geofence_result.geofence_violated;
 		}
 
-		const bool in_low_battery_failsafe_delay = _battery_failsafe_timestamp != 0;
+		const bool in_low_battery_failsafe_delay = (_battery_failsafe_timestamp != 0)
+				|| (_remaining_flighttime_failsafe_timestamp != 0);
 
 		// Geofence actions
 		if (_arm_state_machine.isArmed()
@@ -2732,15 +2809,26 @@ Commander::run()
 			_status_changed = true;
 
 			if (_arm_state_machine.isArmed()) {
+				const hrt_abstime time_at_arm = _vehicle_status.armed_time;
+
 				if (fd_status_flags.arm_escs) {
-					// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
-					if (hrt_elapsed_time(&_vehicle_status.armed_time) < 500_ms) {
+					// Check within the motor spool up time before the takeoff
+					if (hrt_elapsed_time(&time_at_arm) < _param_com_spoolup_time.get() * 1_s) {
 						disarm(arm_disarm_reason_t::failure_detector);
 						mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request\t");
 						events::send(events::ID("commander_fd_escs_not_arming"), events::Log::Critical, "ESCs did not respond to arm request");
 					}
 				}
 
+				if (fd_status_flags.battery) {
+					// Check within the motor spool up time before the takeoff. Has no effect if battery_status
+					// is published at a lower rate than the spoolup time demands
+					if (hrt_elapsed_time(&_vehicle_status.armed_time) < _param_com_spoolup_time.get() * 1_s) {
+						disarm(arm_disarm_reason_t::failure_detector);
+						mavlink_log_critical(&_mavlink_log_pub, "One or more batteries reported a problem");
+					}
+				}
+
 				if (fd_status_flags.roll || fd_status_flags.pitch || fd_status_flags.alt || fd_status_flags.ext) {
 					const bool is_right_after_takeoff = hrt_elapsed_time(&_vehicle_status.takeoff_time) <
 									    (1_s * _param_com_lkdown_tko.get());
@@ -2854,35 +2942,17 @@ Commander::run()
 			}
 		}
 
-		// Check wind speed actions if either high wind warning or high wind RTL is enabled
-		if ((_param_com_wind_warn.get() > FLT_EPSILON || _param_com_wind_max.get() > FLT_EPSILON)
-		    && !_vehicle_land_detected.landed) {
-			checkWindSpeedThresholds();
-		}
+		// Check wind speed actions
+		checkWindSpeedThresholds();
+
+		// Check for flight time maximum
+		checkFlightTimeThresholds();
 
 		_vehicle_status_flags.flight_terminated = _actuator_armed.force_failsafe || _actuator_armed.manual_lockdown;
 
 		/* Get current timestamp */
 		const hrt_abstime now = hrt_absolute_time();
 
-		// Trigger RTL if flight time is larger than max flight time specified in COM_FLT_TIME_MAX.
-		// The user is not able to override once above threshold, except for triggering Land.
-		if (!_vehicle_land_detected.landed
-		    && _param_com_flt_time_max.get() > FLT_EPSILON
-		    && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
-		    && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND
-		    && (now - _vehicle_status.takeoff_time) > (1_s * _param_com_flt_time_max.get())) {
-			main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, _commander_state);
-			_status_changed = true;
-			mavlink_log_critical(&_mavlink_log_pub, "Maximum flight time reached, abort operation and RTL");
-			/* EVENT
-			* @description
-			* Maximal flight time reached, return to launch.
-			*/
-			events::send(events::ID("commander_max_flight_time_rtl"), {events::Log::Critical, events::LogInternal::Warning},
-				     "Maximum flight time reached, abort operation and RTL");
-		}
-
 		// automatically set or update home position
 		if (_param_com_home_en.get() && !_home_position_pub.get().manual_home) {
 			if (!_arm_state_machine.isArmed() && _vehicle_land_detected.landed) {
@@ -3485,6 +3555,13 @@ Commander::update_control_mode()
 		_vehicle_control_mode.flag_control_velocity_enabled = true;
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_INIT:
+		// TODO: this needs to be fixed, if rate controller is disabled PX4 will hang because something is waiting or busy looping on actuator_controls!
+		// When MulticopterRateControl is publishing actuator_controls_s everything works fine, for now we just keep rate controller enabled.
+		// For the proper fix the output initialization needs to be handled as well.
+		_vehicle_control_mode.flag_control_rates_enabled = true;
+		break;
+
 	default:
 		break;
 	}
@@ -3530,22 +3607,22 @@ Commander::print_reject_mode(uint8_t main_state)
 void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
 {
 	switch (result) {
-	case vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED:
+	case vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED:
 		break;
 
-	case vehicle_command_s::VEHICLE_CMD_RESULT_DENIED:
+	case vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED:
 		tune_negative(true);
 		break;
 
-	case vehicle_command_s::VEHICLE_CMD_RESULT_FAILED:
+	case vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED:
 		tune_negative(true);
 		break;
 
-	case vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
+	case vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
 		tune_negative(true);
 		break;
 
-	case vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED:
+	case vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED:
 		tune_negative(true);
 		break;
 
@@ -3695,6 +3772,24 @@ void Commander::data_link_check()
 				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, true, true, healthy, _vehicle_status);
 			}
 
+			if (telemetry.heartbeat_type_open_drone_id) {
+				if (_open_drone_id_system_lost) {
+					_open_drone_id_system_lost = false;
+
+					if (_datalink_last_heartbeat_open_drone_id_system != 0) {
+						mavlink_log_info(&_mavlink_log_pub, "OpenDroneID system regained\t");
+						events::send(events::ID("commander_open_drone_id_regained"), events::Log::Info, "OpenDroneID system regained");
+					}
+				}
+
+				bool healthy = telemetry.open_drone_id_system_healthy;
+
+				_datalink_last_heartbeat_open_drone_id_system = telemetry.timestamp;
+				_vehicle_status_flags.open_drone_id_system_present = true;
+				_vehicle_status_flags.open_drone_id_system_healthy = healthy;
+				set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OPEN_DRONE_ID, true, true, healthy, _vehicle_status);
+			}
+
 			if (telemetry.heartbeat_component_obstacle_avoidance) {
 				if (_avoidance_system_lost) {
 					_avoidance_system_lost = false;
@@ -3735,6 +3830,17 @@ void Commander::data_link_check()
 		_status_changed = true;
 	}
 
+	// logging system
+	if (_vehicle_status_flags.onboard_logging_system_required && _datalink_last_heartbeat_logging_system > 0
+	    && (hrt_elapsed_time(&_datalink_last_heartbeat_logging_system) > 5_s)
+	    && !_logging_system_lost) {
+
+		mavlink_log_critical(&_mavlink_log_pub, "Logging System lost");
+		_vehicle_status_flags.onboard_logging_system_valid = false;
+		_logging_system_lost = true;
+		_status_changed = true;
+	}
+
 	// Parachute system
 	if ((hrt_elapsed_time(&_datalink_last_heartbeat_parachute_system) > 3_s)
 	    && !_parachute_system_lost) {
@@ -3746,6 +3852,18 @@ void Commander::data_link_check()
 		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_PARACHUTE, false, true, false, _vehicle_status);
 	}
 
+	// OpenDroneID system
+	if ((hrt_elapsed_time(&_datalink_last_heartbeat_open_drone_id_system) > 3_s)
+	    && !_open_drone_id_system_lost) {
+		mavlink_log_critical(&_mavlink_log_pub, "OpenDroneID system lost");
+		events::send(events::ID("commander_open_drone_id_lost"), events::Log::Critical, "OpenDroneID system lost");
+		_vehicle_status_flags.open_drone_id_system_present = false;
+		_vehicle_status_flags.open_drone_id_system_healthy = false;
+		_open_drone_id_system_lost = true;
+		_status_changed = true;
+		set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_OPEN_DRONE_ID, false, true, false, _vehicle_status);
+	}
+
 	// AVOIDANCE SYSTEM state check (only if it is enabled)
 	if (_vehicle_status_flags.avoidance_system_required && !_onboard_controller_lost) {
 		// if heartbeats stop
@@ -3913,16 +4031,24 @@ void Commander::battery_status_check()
 	    && PX4_ISFINITE(worst_battery_time_s)
 	    && rtl_time_estimate.safe_time_estimate >= worst_battery_time_s
 	    && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
-	    && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
+	    && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND
+	    && (_remaining_flighttime_failsafe_timestamp == 0)) {
 		// Try to trigger RTL
-		if (main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags,
+		main_state_t failsafe_commander_state{commander_state_s::MAIN_STATE_AUTO_RTL};
+
+		if (_param_com_bat_act_t.get() > 0.f) {
+			failsafe_commander_state = commander_state_s::MAIN_STATE_AUTO_LOITER;
+			_remaining_flighttime_failsafe_timestamp = hrt_absolute_time();
+		}
+
+		if (main_state_transition(_vehicle_status, failsafe_commander_state, _vehicle_status_flags,
 					  _commander_state) == TRANSITION_CHANGED) {
-			mavlink_log_emergency(&_mavlink_log_pub, "Remaining flight time low, returning to land\t");
+			mavlink_log_gcs_critical(&_mavlink_log_pub, "Remaining flight time low, returning to land\t");
 			events::send(events::ID("commander_remaining_flight_time_rtl"), {events::Log::Critical, events::LogInternal::Info},
 				     "Remaining flight time low, returning to land");
 
 		} else {
-			mavlink_log_emergency(&_mavlink_log_pub, "Remaining flight time low, land now!\t");
+			mavlink_log_gcs_critical(&_mavlink_log_pub, "Remaining flight time low, land now!\t");
 			events::send(events::ID("commander_remaining_flight_time_land"), {events::Log::Critical, events::LogInternal::Info},
 				     "Remaining flight time low, land now!");
 		}
@@ -3930,6 +4056,20 @@ void Commander::battery_status_check()
 		_rtl_time_actions_done = true;
 	}
 
+	if ((_remaining_flighttime_failsafe_timestamp != 0)
+	    && (hrt_elapsed_time(&_remaining_flighttime_failsafe_timestamp) > _param_com_bat_act_t.get() * 1_s)) {
+		_remaining_flighttime_failsafe_timestamp = 0;
+
+		if ((_vehicle_control_mode.flag_control_auto_enabled)
+		    && _arm_state_machine.isArmed()
+		    && !_vehicle_land_detected.ground_contact
+		    && (_commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL)
+		    && (_commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND)) {
+			// Go to RTL, we made the pilot aware before the delay so no need here.
+			main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, _commander_state);
+		}
+	}
+
 	bool battery_warning_level_increased_while_armed = false;
 	bool update_internal_battery_state = false;
 
@@ -3955,7 +4095,7 @@ void Commander::battery_status_check()
 		// There is at least one connected battery (in any slot)
 		&& (_last_connected_batteries.count() >= battery_required_count)
 		// No currently-connected batteries have any warning
-		&& (_battery_warning == battery_status_s::BATTERY_WARNING_NONE)
+		&& (_battery_warning < battery_status_s::BATTERY_WARNING_CRITICAL)
 		// No currently-connected batteries have any fault
 		&& (!battery_has_fault);
 
@@ -4071,6 +4211,18 @@ void Commander::estimator_check()
 
 		// Check for a magnetomer fault and notify the user
 		if (!mag_fault_prev && estimator_status_flags.cs_mag_fault) {
+			if (_vehicle_control_mode.flag_control_velocity_enabled) {
+				// If RTL denied, do land
+				if (main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags,
+							  _commander_state) == TRANSITION_DENIED) {
+					if (_vehicle_status_flags.global_position_valid || (_vehicle_status_flags.local_position_valid
+							&& _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) {
+						main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_LAND, _vehicle_status_flags,
+								      _commander_state);
+					}
+				}
+			}
+
 			mavlink_log_critical(&_mavlink_log_pub, "Compass needs calibration - Land now!\t");
 			events::send(events::ID("commander_stopping_mag_use"), events::Log::Critical,
 				     "Stopping compass use! Land now and calibrate the compass");
@@ -4195,6 +4347,28 @@ void Commander::estimator_check()
 		_vehicle_status_flags.local_velocity_valid =
 			check_posvel_validity(v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
 					      _last_lvel_fail_time_us, _vehicle_status_flags.local_velocity_valid);
+
+		// trigger RTL when dead-reckoning and horizontal position accurancy falls below defined threshold (COM_DR_EPH)
+		if (!_rtl_after_gnss_outtage_triggered
+		    && _param_com_dr_eph.get() > 0
+		    && lpos.eph > _param_com_dr_eph.get()
+		    && _vehicle_status_flags.global_position_valid
+		    && _vehicle_status_flags.dead_reckoning
+		    && !_vehicle_land_detected.landed
+		    && (_commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER
+			|| _commander_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION)) {
+
+			main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, _commander_state);
+			_status_changed = true;
+			mavlink_log_critical(&_mavlink_log_pub, "Large position error while dead-reckoning: Returning to Launch\t");
+			events::send(events::ID("commander_dead_reckoning_rtl"), {events::Log::Critical, events::LogInternal::Info},
+				     "Large position error while dead-reckoning: Returning to Launch");
+			_rtl_after_gnss_outtage_triggered = true;
+
+		} else if (!_vehicle_status_flags.dead_reckoning) {
+			// reset if no longer dead-reckoning
+			_rtl_after_gnss_outtage_triggered = false;
+		}
 	}
 
 
@@ -4261,11 +4435,20 @@ void Commander::estimator_check()
 			bool eph = vehicle_gps_position.eph < _param_com_pos_fs_eph.get();
 			bool epv = vehicle_gps_position.epv < _param_com_pos_fs_epv.get();
 			bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get();
+			bool no_jamming = (vehicle_gps_position.jamming_state != vehicle_gps_position_s::JAMMING_STATE_CRITICAL);
 
-			_vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh, hrt_absolute_time());
+			_vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh && no_jamming, hrt_absolute_time());
 			_vehicle_status_flags.gps_position_valid = _vehicle_gps_position_valid.get_state();
 
 			_vehicle_gps_position_timestamp_last = vehicle_gps_position.timestamp;
+
+			if (condition_gps_position_was_valid) {
+				if (vehicle_gps_position.jamming_state == vehicle_gps_position_s::JAMMING_STATE_CRITICAL) {
+					mavlink_log_warning(&_mavlink_log_pub, "GPS jamming state critical\t");
+					events::send(events::ID("commander_gps_jamming_critical"), {events::Log::Critical, events::LogInternal::Info},
+						     "GPS jamming state critical");
+				}
+			}
 		}
 
 	} else {
@@ -4279,6 +4462,23 @@ void Commander::estimator_check()
 
 	if (condition_gps_position_was_valid && !_vehicle_status_flags.gps_position_valid) {
 		PX4_DEBUG("GPS no longer valid");
+
+		// report GPS failure if flying and the global position estimate is still valid
+		if (!_vehicle_land_detected.landed && _vehicle_status_flags.global_position_valid
+		    && _vehicle_status_flags.dead_reckoning) {
+			mavlink_log_warning(&_mavlink_log_pub, "GPS no longer valid, switching to dead-reckoning\t");
+			events::send(events::ID("commander_gps_lost"), {events::Log::Critical, events::LogInternal::Info},
+				     "GPS no longer valid, switching to dead-reckoning");
+		}
+
+	} else if (!condition_gps_position_was_valid && _vehicle_status_flags.gps_position_valid) {
+
+		// report GPS valid (again) if flying and global position is (still) valid
+		if (!_vehicle_land_detected.landed && _vehicle_status_flags.global_position_valid) {
+			mavlink_log_warning(&_mavlink_log_pub, "GPS valid\t");
+			events::send(events::ID("commander_gps_valid_in_flight"), {events::Log::Info, events::LogInternal::Info},
+				     "GPS valid");
+		}
 	}
 }
 
@@ -4311,6 +4511,8 @@ void Commander::manual_control_check()
 		_is_throttle_low = (manual_control_setpoint.z < 0.1f);
 
 		const bool is_mavlink = (manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC);
+		const bool position_is_valid = _vehicle_status_flags.global_position_valid
+					       || _vehicle_status_flags.local_position_valid;
 
 		if (is_mavlink) {
 			set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true, _vehicle_status);
@@ -4342,7 +4544,8 @@ void Commander::manual_control_check()
 					}
 				}
 
-				const bool in_low_battery_failsafe_delay = (_battery_failsafe_timestamp != 0);
+				const bool in_low_battery_failsafe_delay = (_battery_failsafe_timestamp != 0)
+						|| (_remaining_flighttime_failsafe_timestamp != 0);
 
 				if (override_enabled && !in_low_battery_failsafe_delay && !_geofence_warning_action_on) {
 
@@ -4375,7 +4578,7 @@ void Commander::manual_control_check()
 		} else {
 			// disarmed
 			// if there's never been a mode change force position control as initial state
-			if (_commander_state.main_state_changes == 0) {
+			if ((_commander_state.main_state == commander_state_s::MAIN_STATE_INIT) && position_is_valid) {
 				if (is_mavlink || !_mode_switch_mapped) {
 					_commander_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
 					_commander_state.main_state_changes++;
@@ -4563,40 +4766,108 @@ void Commander::send_parachute_command()
 
 void Commander::checkWindSpeedThresholds()
 {
-	wind_s wind_estimate;
+	if ((_param_com_wind_warn.get() > FLT_EPSILON || _param_com_wind_max.get() > FLT_EPSILON
+	     || _vehicle_status_flags.max_wind_speed_exceeded)
+	    && !_vehicle_land_detected.landed) {
 
-	if (_wind_sub.update(&wind_estimate)) {
-		const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east);
+		wind_s wind_estimate;
+
+		if (_wind_sub.update(&wind_estimate)) {
+			const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east);
+
+			// publish a warning if it's the first since in air or 60s have passed since the last warning
+			const bool warning_timeout_passed = _last_wind_warning == 0 || hrt_elapsed_time(&_last_wind_warning) > 60_s;
+
+			// reset wind flag if param is set to <0 or current wind is below threshold
+			if (_vehicle_status_flags.max_wind_speed_exceeded
+			    && (_param_com_wind_max.get() < FLT_EPSILON || !wind.longerThan(_param_com_wind_max.get()))) {
+				_vehicle_status_flags.max_wind_speed_exceeded = false;
+
+				mavlink_log_critical(&_mavlink_log_pub, "Measured wind speed below limit again, flight can be continued (%.1f m/s)\t",
+						     (double)wind.norm());
+				events::send<float>(events::ID("commander_high_wind_cleared"),
+				{events::Log::Warning, events::LogInternal::Info},
+				"Measured wind speed below limit again, flight can be continued ({1:.1m/s})", wind.norm());
+			}
+
+			if (!_vehicle_status_flags.max_wind_speed_exceeded
+			    && _param_com_wind_max.get() > FLT_EPSILON
+			    && wind.longerThan(_param_com_wind_max.get())
+			    && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
+			    && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
+
+				main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, _commander_state);
+				_status_changed = true;
+				mavlink_log_critical(&_mavlink_log_pub, "Measured wind speed above limit, abort operation and RTL (%.1f m/s)\t",
+						     (double)wind.norm());
+
+				events::send<float>(events::ID("commander_high_wind_rtl"),
+				{events::Log::Warning, events::LogInternal::Info},
+				"Measured wind speed above limit, abort operation and RTL ({1:.1m/s})", wind.norm());
+				_vehicle_status_flags.max_wind_speed_exceeded = true;
+
+			} else if (_param_com_wind_warn.get() > FLT_EPSILON
+				   && wind.longerThan(_param_com_wind_warn.get())
+				   && warning_timeout_passed
+				   && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
+				   && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
+
+				mavlink_log_critical(&_mavlink_log_pub, "High wind speed detected (%.1f m/s), landing advised\t", (double)wind.norm());
+
+				events::send<float>(events::ID("commander_high_wind_warning"),
+				{events::Log::Warning, events::LogInternal::Info},
+				"High wind speed detected ({1:.1m/s}), landing advised", wind.norm());
+				_last_wind_warning = hrt_absolute_time();
+			}
+		}
+	}
+}
+
+void Commander::checkFlightTimeThresholds()
+{
+	// Trigger RTL if flight time is larger than max flight time specified in COM_FLT_TIME_MAX.
+	// Only trigger it once per flight. Mode switches to Auto modes (beside RTL and Land) are disabled.
+
+	if (!_vehicle_status_flags.max_flight_time_exceeded
+	    && !_vehicle_land_detected.landed
+	    && _param_com_flt_time_max.get() > 0
+	    && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
+	    && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
 
 		// publish a warning if it's the first since in air or 60s have passed since the last warning
-		const bool warning_timeout_passed = _last_wind_warning == 0 || hrt_elapsed_time(&_last_wind_warning) > 60_s;
+		const bool warning_timeout_passed = _last_flight_time_warning == 0
+						    || hrt_elapsed_time(&_last_flight_time_warning) > 60_s;
 
-		if (_param_com_wind_max.get() > FLT_EPSILON
-		    && wind.longerThan(_param_com_wind_max.get())
-		    && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
-		    && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
+		const float flight_time_percentage_reached = (_vehicle_status.takeoff_time < hrt_absolute_time()) ? (float)(
+					hrt_absolute_time() - _vehicle_status.takeoff_time) / 1_s /
+				_param_com_flt_time_max.get() : 0.f;
 
+		if (flight_time_percentage_reached > 1.f) {
 			main_state_transition(_vehicle_status, commander_state_s::MAIN_STATE_AUTO_RTL, _vehicle_status_flags, _commander_state);
 			_status_changed = true;
-			mavlink_log_critical(&_mavlink_log_pub, "Wind speeds above limit, abort operation and RTL (%.1f m/s)\t",
-					     (double)wind.norm());
+			mavlink_log_critical(&_mavlink_log_pub, "Maximum flight time reached, abort operation and RTL");
+			/* EVENT
+			* @description
+			* Maximal flight time reached, return to launch.
+			*/
+			events::send(events::ID("commander_max_flight_time_rtl"), {events::Log::Critical, events::LogInternal::Warning},
+				     "Maximum flight time reached, abort operation and RTL");
+			_vehicle_status_flags.max_flight_time_exceeded = true;
 
-			events::send<float>(events::ID("commander_high_wind_rtl"),
-			{events::Log::Warning, events::LogInternal::Info},
-			"Wind speeds above limit, abort operation and RTL ({1:.1m/s})", wind.norm());
+		} else if (flight_time_percentage_reached > 0.9f && warning_timeout_passed) {
+			// send warning every 1 minute with updated remaining time till RTL once passed 90% threshold until RTL is triggered
 
-		} else if (_param_com_wind_warn.get() > FLT_EPSILON
-			   && wind.longerThan(_param_com_wind_warn.get())
-			   && warning_timeout_passed
-			   && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_RTL
-			   && _commander_state.main_state != commander_state_s::MAIN_STATE_AUTO_LAND) {
+			const int remaining_flight_time = (int)((1.f - flight_time_percentage_reached) * _param_com_flt_time_max.get());
+			mavlink_log_warning(&_mavlink_log_pub, "Approaching max flight time (system will RTL in %i seconds)",
+					    remaining_flight_time);
+			/* EVENT
+			* @description
+			* Maximal flight time warning
+			*/
+			events::send<float>(events::ID("commander_max_flight_time_warning"), events::Log::Warning,
+					    "Approaching max flight time (system will RTL in {1} seconds)", remaining_flight_time);
 
-			mavlink_log_critical(&_mavlink_log_pub, "High wind speed detected (%.1f m/s), landing advised\t", (double)wind.norm());
-
-			events::send<float>(events::ID("commander_high_wind_warning"),
-			{events::Log::Warning, events::LogInternal::Info},
-			"High wind speed detected ({1:.1m/s}), landing advised", wind.norm());
-			_last_wind_warning = hrt_absolute_time();
+			_last_flight_time_warning = hrt_absolute_time();
 		}
 	}
 }
diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index 4278c0aa01..8311aafccb 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -153,6 +153,14 @@ private:
 
 	void manual_control_check();
 
+	/**
+	 * @brief Handle incoming vehicle command relavant to Commander
+	 *
+	 * It ignores irrelevant vehicle commands defined inside the switch case statement
+	 * in the function.
+	 *
+	 * @param cmd 		Vehicle command to handle
+	 */
 	bool handle_command(const vehicle_command_s &cmd);
 
 	unsigned handle_command_motor_test(const vehicle_command_s &cmd);
@@ -185,6 +193,8 @@ private:
 
 	void checkWindSpeedThresholds();
 
+	void checkFlightTimeThresholds();
+
 	void updateParameters();
 
 	DEFINE_PARAMETERS(
@@ -226,6 +236,9 @@ private:
 
 		(ParamFloat<px4::params::COM_WIND_WARN>) _param_com_wind_warn,
 
+		(ParamInt<px4::params::RC_MAP_MODE_SW>) _param_rc_map_mode_sw,
+		(ParamInt<px4::params::RC_MAP_FLTM_BTN>) _param_rc_map_fltm_btn,
+
 		// Quadchute
 		(ParamInt<px4::params::COM_QC_ACT>) _param_com_qc_act,
 
@@ -237,6 +250,7 @@ private:
 		(ParamInt<px4::params::COM_PREARM_MODE>) _param_com_prearm_mode,
 		(ParamBool<px4::params::COM_FORCE_SAFETY>) _param_com_force_safety,
 		(ParamBool<px4::params::COM_MOT_TEST_EN>) _param_com_mot_test_en,
+		(ParamInt<px4::params::COM_ARM_WO_OBLOG>) _param_com_arm_wo_ob_logger,
 
 		(ParamFloat<px4::params::COM_KILL_DISARM>) _param_com_kill_disarm,
 		(ParamFloat<px4::params::COM_LKDOWN_TKO>) _param_com_lkdown_tko,
@@ -248,6 +262,7 @@ private:
 		(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_arm_mission_required,
 		(ParamBool<px4::params::COM_ARM_AUTH_REQ>) _param_arm_auth_required,
 		(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_escs_checks_required,
+		(ParamBool<px4::params::COM_DISARM_MAN>) _param_com_disarm_man,
 
 		(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
 		(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
@@ -260,8 +275,12 @@ private:
 		(ParamInt<px4::params::CBRK_VELPOSERR>) _param_cbrk_velposerr,
 		(ParamInt<px4::params::CBRK_VTOLARMING>) _param_cbrk_vtolarming,
 
+		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time,
+
 		(ParamInt<px4::params::COM_FLT_TIME_MAX>) _param_com_flt_time_max,
-		(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max
+		(ParamFloat<px4::params::COM_WIND_MAX>) _param_com_wind_max,
+
+		(ParamInt<px4::params::COM_DR_EPH>) _param_com_dr_eph
 	)
 
 	// optional parameters
@@ -296,6 +315,10 @@ private:
 	static constexpr uint64_t HOTPLUG_SENS_TIMEOUT{8_s};	/**< wait for hotplug sensors to come online for upto 8 seconds */
 	static constexpr uint64_t INAIR_RESTART_HOLDOFF_INTERVAL{500_ms};
 
+	// export compliance parameter limits (only active if PX4_EXPORT_CONTROLLED_BUILD)
+	static constexpr int EXPORT_RESTRICTED_MAX_FLIGHT_TIME{3540}; //59 min
+	static constexpr float EXPORT_RESTRICTED_MAX_WIND{12.f}; //12 m/s
+
 	ArmStateMachine _arm_state_machine{};
 	PreFlightCheck::arm_requirements_t	_arm_requirements{};
 
@@ -334,10 +357,14 @@ private:
 	hrt_abstime	_datalink_last_heartbeat_gcs{0};
 	hrt_abstime	_datalink_last_heartbeat_avoidance_system{0};
 	hrt_abstime	_datalink_last_heartbeat_onboard_controller{0};
+	hrt_abstime	_datalink_last_heartbeat_logging_system{0};
 	hrt_abstime	_datalink_last_heartbeat_parachute_system{0};
+	hrt_abstime	_datalink_last_heartbeat_open_drone_id_system{0};
 	bool		_onboard_controller_lost{false};
 	bool		_avoidance_system_lost{false};
+	bool		_logging_system_lost{false};
 	bool		_parachute_system_lost{true};
+	bool		_open_drone_id_system_lost{true};
 
 	hrt_abstime	_high_latency_datalink_heartbeat{0};
 	hrt_abstime	_high_latency_datalink_lost{0};
@@ -348,6 +375,7 @@ private:
 
 	uint8_t		_battery_warning{battery_status_s::BATTERY_WARNING_NONE};
 	hrt_abstime	_battery_failsafe_timestamp{0};
+	hrt_abstime	_remaining_flighttime_failsafe_timestamp{0};
 	px4::Bitset<battery_status_s::MAX_INSTANCES> _last_connected_batteries;
 	uint32_t	_last_battery_custom_fault[battery_status_s::MAX_INSTANCES] {};
 	uint16_t	_last_battery_fault[battery_status_s::MAX_INSTANCES] {};
@@ -395,6 +423,9 @@ private:
 	vtol_vehicle_status_s	_vtol_vehicle_status{};
 
 	hrt_abstime _last_wind_warning{0};
+	hrt_abstime _last_flight_time_warning{0};
+
+	bool _rtl_after_gnss_outtage_triggered{false};
 
 	// commander publications
 	actuator_armed_s        _actuator_armed{};
diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp
index 76d373a664..3c4d5ee2cb 100644
--- a/src/modules/commander/calibration_routines.cpp
+++ b/src/modules/commander/calibration_routines.cpp
@@ -339,13 +339,13 @@ bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &ca
 				    (int)cmd.param5 == 0 &&
 				    (int)cmd.param6 == 0) {
 
-					command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+					command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 					mavlink_log_critical(mavlink_log_pub, CAL_QGC_CANCELLED_MSG);
 					tune_positive(true);
 					ret = true;
 
 				} else {
-					command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
+					command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 					mavlink_log_critical(mavlink_log_pub, "command denied during calibration: %" PRIu32 "\t", cmd.command);
 					events::send<uint32_t>(events::ID("commander_cal_cmd_denied"), {events::Log::Error, events::LogInternal::Info},
 							       "Command denied during calibration: {1}", cmd.command);
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index eb4a5a125c..d186bdd82d 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -79,6 +79,8 @@
 #define VEHICLE_TYPE_VTOL_TILTROTOR 21
 #define VEHICLE_TYPE_VTOL_FIXEDROTOR 22 // VTOL standard
 #define VEHICLE_TYPE_VTOL_TAILSITTER 23
+#define VEHICLE_TYPE_VTOL_RESERVED4 24
+#define VEHICLE_TYPE_VTOL_RESERVED5 25
 
 #define BLINK_MSG_TIME	700000	// 3 fast blinks (in us)
 
@@ -103,7 +105,9 @@ bool is_vtol(const vehicle_status_s &current_status)
 		current_status.system_type == VEHICLE_TYPE_VTOL_TAILSITTER_QUADROTOR ||
 		current_status.system_type == VEHICLE_TYPE_VTOL_TILTROTOR ||
 		current_status.system_type == VEHICLE_TYPE_VTOL_FIXEDROTOR ||
-		current_status.system_type == VEHICLE_TYPE_VTOL_TAILSITTER);
+		current_status.system_type == VEHICLE_TYPE_VTOL_TAILSITTER ||
+		current_status.system_type == VEHICLE_TYPE_VTOL_RESERVED4 ||
+		current_status.system_type == VEHICLE_TYPE_VTOL_RESERVED5);
 }
 
 bool is_vtol_tailsitter(const vehicle_status_s &current_status)
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index c22c489987..63552c5762 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -223,7 +223,8 @@ PARAM_DEFINE_INT32(COM_RC_ARM_HYST, 1000);
  *
  * @group Commander
  * @unit s
- * @decimal 2
+ * @decimal 1
+ * @increment 0.1
  */
 
 PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f);
@@ -239,7 +240,8 @@ PARAM_DEFINE_FLOAT(COM_DISARM_LAND, 2.0f);
  *
  * @group Commander
  * @unit s
- * @decimal 2
+ * @decimal 1
+ * @increment 0.1
  */
 PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f);
 
@@ -252,7 +254,7 @@ PARAM_DEFINE_FLOAT(COM_DISARM_PRFLT, 10.0f);
  * @value 0 Require GPS lock to arm
  * @value 1 Allow arming without GPS
  */
-PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);
+PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 0);
 
 /**
  * Arm switch is a momentary button
@@ -266,6 +268,19 @@ PARAM_DEFINE_INT32(COM_ARM_WO_GPS, 1);
  */
 PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0);
 
+/**
+ * Allow disarming via switch/stick/button on multicopters in manual thrust modes
+ *
+ * 0: Disallow disarming when not landed
+ * 1: Allow disarming in multicopter flight in modes where
+ * the thrust is directly controlled by thr throttle stick
+ * e.g. Stabilized, Acro
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_DISARM_MAN, 1);
+
 /**
  * Battery failsafe mode
  *
@@ -276,8 +291,6 @@ PARAM_DEFINE_INT32(COM_ARM_SWISBTN, 0);
  * @value 0 Warning
  * @value 2 Land mode
  * @value 3 Return at critical level, land at emergency level
- * @decimal 0
- * @increment 1
  */
 PARAM_DEFINE_INT32(COM_LOW_BAT_ACT, 0);
 
@@ -310,7 +323,6 @@ PARAM_DEFINE_FLOAT(COM_BAT_ACT_T, 5.f);
  * @value 0 Warning
  * @value 1 Return
  * @value 2 Land
- * @decimal 0
  * @increment 1
  */
 PARAM_DEFINE_INT32(COM_IMB_PROP_ACT, 0);
@@ -656,7 +668,7 @@ PARAM_DEFINE_INT32(COM_RC_OVERRIDE, 1);
  * @decimal 0
  * @increment 0.05
  */
-PARAM_DEFINE_FLOAT(COM_RC_STICK_OV, 30.0f);
+PARAM_DEFINE_FLOAT(COM_RC_STICK_OV, 12.0f);
 
 /**
  * Require valid mission to arm
@@ -745,32 +757,47 @@ PARAM_DEFINE_INT32(COM_POS_FS_DELAY, 1);
 /**
  * Horizontal position error threshold.
  *
- * This is the horizontal position error (EPH) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ * This is the horizontal position error (EPH) threshold that will trigger a failsafe.
+ * The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ * If the previous position error was below this threshold, there is an additional
+ * factor of 2.5 applied (threshold for invalidation 2.5 times the one for validation).
  *
  * @unit m
+ * @min 0
+ * @decimal 1
  * @group Commander
  */
-PARAM_DEFINE_FLOAT(COM_POS_FS_EPH, 5);
+PARAM_DEFINE_FLOAT(COM_POS_FS_EPH, 5.f);
 
 /**
  * Vertical position error threshold.
  *
- * This is the vertical position error (EPV) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ * This is the vertical position error (EPV) threshold that will trigger a failsafe.
+ * The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ * If the previous position error was below this threshold, there is an additional
+ * factor of 2.5 applied (threshold for invalidation 2.5 times the one for validation).
  *
  * @unit m
+ * @min 0
+ * @decimal 1
  * @group Commander
  */
-PARAM_DEFINE_FLOAT(COM_POS_FS_EPV, 10);
+PARAM_DEFINE_FLOAT(COM_POS_FS_EPV, 10.f);
 
 /**
  * Horizontal velocity error threshold.
  *
- * This is the horizontal velocity error (EVH) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ * This is the horizontal velocity error (EVH) threshold that will trigger a failsafe.
+ * The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ * If the previous velocity error was below this threshold, there is an additional
+ * factor of 2.5 applied (threshold for invalidation 2.5 times the one for validation).
  *
  * @unit m/s
+ * @min 0
+ * @decimal 1
  * @group Commander
  */
-PARAM_DEFINE_FLOAT(COM_VEL_FS_EVH, 1);
+PARAM_DEFINE_FLOAT(COM_VEL_FS_EVH, 1.f);
 
 /**
  * Next flight UUID
@@ -924,6 +951,23 @@ PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 0);
  */
 PARAM_DEFINE_INT32(COM_PREARM_MODE, 0);
 
+/**
+ * Enable pre-arm check on OpenDroneID.
+ *
+ * If this parameter is set, the pre-arm mechanism will check for the OpenDroneID system and prevent arming if it is not present.
+ *
+ * @group Commander
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_PREARM_ODID, 0);
+
+/*
+ * Allow arming without onboard logger being enabled and in ready state
+ *
+ * The default allows the vehicle to arm without the onboard logger reporting as ready via it's heartbeat
+ */
+PARAM_DEFINE_INT32(COM_ARM_WO_OBLOG, 0);
+
 /**
  * Enable force safety
  *
@@ -997,6 +1041,22 @@ PARAM_DEFINE_INT32(COM_POWER_COUNT, 1);
  */
 PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f);
 
+/**
+ * Enable external components update
+ *
+ * If enabled, the check will verify that the external components (e.g. ESCs, Smartbattery,...) have been successfully updated.
+ * The check looks for the "ext_component_updated" file on the SD card and parses it.
+ * The file can contain a "SUCCESS <return_code>" or "FAIL <error_code>", depending on the parsed result the system will
+ * allow/deny arming.
+ *
+ * @group Commander
+ * @value 0 Disabled
+ * @value 1 Enabled
+ * @value 2 Trigger fake success (update_checker)
+ * @value 3 Trigger fake fail (update_checker)
+ */
+PARAM_DEFINE_INT32(COM_EXT_COMP_EN, 0);
+
 /**
 * Enable preflight check for maximal allowed airspeed when arming.
 *
@@ -1023,16 +1083,44 @@ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1);
  */
 PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1);
 
+/**
+ * Enable FMU SD card hardfault detection check
+ *
+ * This check detects if there are hardfault files present on the
+ * SD card. If so, and the parameter is enabled, arming is prevented.
+ *
+ * @group Commander
+ * @reboot_required true
+ * @boolean
+ */
+PARAM_DEFINE_INT32(COM_ARM_HFLT_CHK, 1);
+
+/**
+ * Enforced delay between arming and takeoff
+ *
+ * The minimal time from arming the motors until a takeoff is possible is COM_SPOOLUP_TIME seconds.
+ * Goal:
+ * - Motors and propellers spool up to idle speed before getting commanded to spin faster
+ * - ESCs do failure checks e.g. for stuck rotors before the vehicle is off the ground
+ *
+ * @group Commander
+ * @min 0
+ * @max 30
+ * @decimal 1
+ * @increment 0.1
+ * @unit s
+ */
+PARAM_DEFINE_FLOAT(COM_SPOOLUP_TIME, 1.0f);
+
 /**
  * Wind speed warning threshold
  *
  * A warning is triggered if the currently estimated wind speed is above this value.
  * Warning is sent periodically (every 1min).
  *
- * A negative value disables the feature.
+ * Set to -1 to disable.
  *
  * @min -1
- * @max 30
  * @decimal 1
  * @increment 0.1
  * @group Commander
@@ -1045,32 +1133,50 @@ PARAM_DEFINE_FLOAT(COM_WIND_WARN, -1.f);
  *
  * The vehicle aborts the current operation and returns to launch when
  * the time since takeoff is above this value. It is not possible to resume the
- * mission or switch to any mode other than RTL or Land.
+ * mission or switch to any auto mode other than RTL or Land. Taking over in any manual
+ * mode is still possible.
  *
- * Set a nagative value to disable.
+ * Starting from 90% of the maximum flight time, a warning message will be sent
+ * every 1 minute with the remaining time until automatic RTL.
  *
+ * Set to -1 to disable.
  *
  * @unit s
  * @min -1
- * @max 10000
- * @value 0 Disable
  * @group Commander
  */
-PARAM_DEFINE_INT32(COM_FLT_TIME_MAX, -1);
+PARAM_DEFINE_INT32(COM_FLT_TIME_MAX, 3540);
 
 /**
- * Wind speed RLT threshold
+ * Wind speed RTL threshold
  *
- * Wind speed threshold above which an automatic return to launch is triggered
- * and enforced as long as the threshold is exceeded.
+ * Wind speed threshold above which an automatic return to launch is triggered.
+ * It is not possible to resume the mission or switch to any auto mode other than
+ * RTL or Land if this threshold is exceeded. Taking over in any manual
+ * mode is still possible.
  *
- * A negative value disables the feature.
+ * Set to -1 to disable.
  *
  * @min -1
- * @max 30
  * @decimal 1
  * @increment 0.1
  * @group Commander
  * @unit m/s
  */
-PARAM_DEFINE_FLOAT(COM_WIND_MAX, -1.f);
+PARAM_DEFINE_FLOAT(COM_WIND_MAX, 12.f);
+
+/**
+ * EPH threshold for RTL when (wind-compensated) dead reckoning
+ *
+ * Even after the loss of GNSS sensoring capabilities on the vehicle, its position can still be estimated
+ * by relying on the previously estimated wind speed and current airspeed. As the quality of the estimate
+ * in that case though deteriorates with time, an automatic RTL can be configured through this parameter
+ * to have the vehicle (try to) reach Home before the position estimate gets invalidated (see COM_POS_FS_EPH).
+ * Only applies to Mission or Loiter flight mode.
+ * Set to -1 to disable.
+ *
+ * @group Commander
+ * @min -1
+ * @unit m
+ */
+PARAM_DEFINE_INT32(COM_DR_EPH, 50);
diff --git a/src/modules/commander/failure_detector/FailureDetector.cpp b/src/modules/commander/failure_detector/FailureDetector.cpp
index 19f2996e2b..c920a59f5b 100644
--- a/src/modules/commander/failure_detector/FailureDetector.cpp
+++ b/src/modules/commander/failure_detector/FailureDetector.cpp
@@ -124,8 +124,8 @@ void FailureInjector::update()
 			ack.command = vehicle_command.command;
 			ack.from_external = false;
 			ack.result = supported ?
-				     vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED :
-				     vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
+				     vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED :
+				     vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 			ack.timestamp = hrt_absolute_time();
 			_command_ack_pub.publish(ack);
 		}
@@ -184,6 +184,10 @@ bool FailureDetector::update(const vehicle_status_s &vehicle_status, const vehic
 		_status.flags.ext = false;
 	}
 
+	if (_param_fd_bat_en.get()) {
+		updateBatteryStatus(vehicle_status);
+	}
+
 	// esc_status subscriber is shared between subroutines
 	esc_status_s esc_status;
 
@@ -257,6 +261,40 @@ void FailureDetector::updateExternalAtsStatus()
 	}
 }
 
+void FailureDetector::updateBatteryStatus(const vehicle_status_s &vehicle_status)
+{
+	const hrt_abstime time_now = hrt_absolute_time();
+
+	if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+		bool battery_failure = false;
+
+		for (auto &battery_sub : _battery_status_subs) {
+			battery_status_s battery_status;
+
+			if (battery_sub.copy(&battery_status)) {
+				// One faulty battery is enough to trigger fault
+				battery_failure = battery_failure || (battery_status.mode != battery_status_s::BATTERY_MODE_UNKNOWN
+								      || battery_status.faults > 0);
+
+				if (battery_failure) {
+					break;
+				}
+			}
+		}
+
+		_battery_failure_hysteresis.set_hysteresis_time_from(false, 300_ms);
+		_battery_failure_hysteresis.set_state_and_update(battery_failure, time_now);
+
+		if (_battery_failure_hysteresis.get_state()) {
+			_status.flags.battery = true;
+		}
+
+	} else {
+		_battery_failure_hysteresis.set_state_and_update(false, time_now);
+		_status.flags.battery = false;
+	}
+}
+
 void FailureDetector::updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status)
 {
 	hrt_abstime time_now = hrt_absolute_time();
diff --git a/src/modules/commander/failure_detector/FailureDetector.hpp b/src/modules/commander/failure_detector/FailureDetector.hpp
index ba66c44e3a..ead1dbd1ca 100644
--- a/src/modules/commander/failure_detector/FailureDetector.hpp
+++ b/src/modules/commander/failure_detector/FailureDetector.hpp
@@ -51,8 +51,10 @@
 
 // subscriptions
 #include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/topics/actuator_motors.h>
+#include <uORB/topics/battery_status.h>
 #include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -109,6 +111,7 @@ public:
 private:
 	void updateAttitudeStatus();
 	void updateExternalAtsStatus();
+	void updateBatteryStatus(const vehicle_status_s &vehicle_status);
 	void updateEscsStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
 	void updateMotorStatus(const vehicle_status_s &vehicle_status, const esc_status_s &esc_status);
 	void updateImbalancedPropStatus();
@@ -119,6 +122,9 @@ private:
 	systemlib::Hysteresis _pitch_failure_hysteresis{false};
 	systemlib::Hysteresis _ext_ats_failure_hysteresis{false};
 	systemlib::Hysteresis _esc_failure_hysteresis{false};
+	systemlib::Hysteresis _battery_failure_hysteresis{false};
+
+	uORB::SubscriptionMultiArray<battery_status_s> _battery_status_subs{ORB_ID::battery_status};
 
 	static constexpr float _imbalanced_prop_lpf_time_constant{5.f};
 	AlphaFilter<float> _imbalanced_prop_lpf{};
@@ -149,6 +155,7 @@ private:
 		(ParamBool<px4::params::FD_EXT_ATS_EN>) _param_fd_ext_ats_en,
 		(ParamInt<px4::params::FD_EXT_ATS_TRIG>) _param_fd_ext_ats_trig,
 		(ParamInt<px4::params::FD_ESCS_EN>) _param_escs_en,
+		(ParamInt<px4::params::FD_BAT_EN>) _param_fd_bat_en,
 		(ParamInt<px4::params::FD_IMB_PROP_THR>) _param_fd_imb_prop_thr,
 
 		// Actuator failure
diff --git a/src/modules/commander/failure_detector/failure_detector_params.c b/src/modules/commander/failure_detector/failure_detector_params.c
index e472c304b8..edc4b2b1a7 100644
--- a/src/modules/commander/failure_detector/failure_detector_params.c
+++ b/src/modules/commander/failure_detector/failure_detector_params.c
@@ -142,6 +142,19 @@ PARAM_DEFINE_INT32(FD_EXT_ATS_TRIG, 1900);
  */
 PARAM_DEFINE_INT32(FD_ESCS_EN, 1);
 
+/**
+ * Enable checks on smart batteries that report readiness or faults.
+ *
+ * If enabled, failure detector will verify that all the batteries are in nominal mode and no faults were detected by the batteries.
+ * Timeout for receiving a nominal status report from the batteries after arming is 0.5s, if no feedback is received the failure detector will auto disarm the vehicle.
+ *
+ * @boolean
+ * @reboot_required true
+ *
+ * @group Failure Detector
+ */
+PARAM_DEFINE_INT32(FD_BAT_EN, 1);
+
 /**
  * Imbalanced propeller check threshold
  *
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index 4b1353e139..d417d45560 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -51,7 +51,8 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
 	PX4_CUSTOM_MAIN_MODE_STABILIZED,
 	PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY,
-	PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */
+	PX4_CUSTOM_MAIN_MODE_SIMPLE, /* unused, but reserved for future use */
+	PX4_CUSTOM_MAIN_MODE_INIT
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 002e71d6db..7abb487d23 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -150,8 +150,21 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
 
 	case commander_state_s::MAIN_STATE_AUTO_LOITER:
 
-		/* need global position estimate */
-		if (status_flags.global_position_valid) {
+		/* need global position estimate
+		 * and there was not a previously triggered flight time or max
+		 * wind speed rtl in the current flight */
+
+		if (status_flags.max_flight_time_exceeded) {
+			events::send(events::ID("state_machine_loiter_rejected_flight_time"),
+			{events::Log::Warning, events::LogInternal::Info},
+			"Mode rejected due to flight time above limit");
+
+		} else if (status_flags.max_wind_speed_exceeded) {
+			events::send(events::ID("state_machine_loiter_rejected_wind"),
+			{events::Log::Warning, events::LogInternal::Info},
+			"Mode rejected due to wind speed above limit");
+
+		} else if (status_flags.global_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
 
@@ -160,15 +173,30 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
 	case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
 	case commander_state_s::MAIN_STATE_ORBIT:
 
-		/* Follow and orbit only implemented for multicopter */
-		if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		/* Follow and orbit only implemented for multicopter,
+		 * and only allow if there was not a previously triggered flight time or max
+		 * wind speed rtl in the current flight */
+
+		if (status_flags.max_flight_time_exceeded) {
+			events::send(events::ID("state_machine_orbit_rejected_flight_time"),
+			{events::Log::Warning, events::LogInternal::Info},
+			"Mode rejected due to flight time above limit");
+
+		} else if (status_flags.max_wind_speed_exceeded) {
+			events::send(events::ID("state_machine_orbit_rejected_wind"),
+			{events::Log::Warning, events::LogInternal::Info},
+			"Mode rejected due to wind speed above limit");
+
+		} else if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 			ret = TRANSITION_CHANGED;
 		}
 
 		break;
 
 	case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF:
-		if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		if (is_vtol(status) && status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING &&
+		    status_flags.global_position_valid &&
+		    status_flags.home_position_valid) {
 			ret = TRANSITION_CHANGED;
 		}
 
@@ -176,10 +204,22 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
 
 	case commander_state_s::MAIN_STATE_AUTO_MISSION:
 
-		/* need global position, home position, and a valid mission */
-		if (status_flags.global_position_valid &&
-		    status_flags.auto_mission_available) {
+		/* need global position, home position, and a valid mission
+		 * and there was not a previously triggered flight time or max
+		 * wind speed rtl in the current flight */
 
+		if (status_flags.max_flight_time_exceeded) {
+			events::send(events::ID("state_machine_mission_rejected_flight_time"),
+			{events::Log::Warning, events::LogInternal::Info},
+			"Mission rejected due to flight time above limit");
+
+		} else if (status_flags.max_wind_speed_exceeded) {
+			events::send(events::ID("state_machine_mission_rejected_wind"),
+			{events::Log::Warning, events::LogInternal::Info},
+			"Mission rejected due to wind speed above limit");
+
+		} else if (status_flags.global_position_valid &&
+			   status_flags.auto_mission_available) {
 			ret = TRANSITION_CHANGED;
 		}
 
@@ -226,6 +266,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
 
 		break;
 
+	case commander_state_s::MAIN_STATE_INIT:
 	case commander_state_s::MAIN_STATE_MAX:
 	default:
 		break;
@@ -283,6 +324,7 @@ static void enable_failsafe(vehicle_status_s &status, bool old_failsafe, orb_adv
 		events::send<events::px4::enums::failsafe_reason_t>(
 			events::ID("commander_enable_failsafe"), {events::Log::Critical, events::LogInternal::Info},
 			"Failsafe enabled: {1}", event_failsafe_reason);
+		status.failsafe_but_user_took_over = false;
 	}
 
 	status.failsafe = true;
@@ -307,6 +349,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 
 	bool old_failsafe = status.failsafe;
 	status.failsafe = false;
+	status.failsafe_but_user_took_over &= status.data_link_lost;
 
 	// Safe to do reset flags here, as if loss state persists flags will be restored in the code below
 	reset_link_loss_globals(armed, old_failsafe, rc_loss_act);
@@ -325,6 +368,11 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc);
 			set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t);
 
+		} else if (status.data_link_lost && data_link_loss_act_configured && is_armed && !status.failsafe_but_user_took_over) {
+			// Data link lost, data link loss reaction configured -> do configured reaction
+			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink);
+			set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);
+
 		} else {
 			switch (internal_state.main_state) {
 			case commander_state_s::MAIN_STATE_ACRO:
@@ -364,6 +412,11 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 				 * this enables POSCTL using e.g. flow.
 				 * For fixedwing, a global position is needed. */
 
+			} else if (status.data_link_lost && data_link_loss_act_configured && is_armed && !status.failsafe_but_user_took_over) {
+				// Data link lost, data link loss reaction configured -> do configured reaction
+				enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink);
+				set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);
+
 			} else if (check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags,
 							       rc_fallback_allowed, status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)) {
 				// nothing to do - everything done in check_invalid_pos_nav_state
@@ -469,7 +522,12 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 
 		/* require global position and home, also go into failsafe on an engine failure */
 
-		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
+		if (status.data_link_lost && data_link_loss_act_configured && is_armed) {
+			// Data link lost, data link loss reaction configured -> do configured reaction
+			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink);
+			set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);
+
+		} else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 		} else {
 			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
@@ -484,7 +542,22 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, true)) {
 			// nothing to do - everything done in check_invalid_pos_nav_state
 
+		} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
+			// failsafe: datalink is lost
+			// Trigger RTL
+			set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);
+
+			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink);
+
+		} else if (status.rc_signal_lost && status_flags.rc_signal_found_once && !data_link_loss_act_configured && is_armed) {
+			// Trigger failsafe on RC loss only if RC was present once before
+			// Otherwise fly without RC, as follow-target only depends on the datalink
+			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc);
+
+			set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);
+
 		} else {
+			// no failsafe, RC is not mandatory for follow_target
 			status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET;
 		}
 
@@ -526,45 +599,55 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 		break;
 
 	case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
-	case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF:
+	case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF: {
 
-		// require local position
+			// require local position
 
-		if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
-			// nothing to do - everything done in check_invalid_pos_nav_state
+			const bool in_forward_flight = status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
+						       || status.in_transition_mode;
 
-		} else if (status_flags.vtol_transition_failure) {
+			// deny entering failsafe while doing a vtol takeoff after the vehicle has started a transition and before it reaches the loiter
+			// altitude. the vtol takeoff navigaton mode will set mission_finished to true as soon as the loiter is established
+			const bool deny_link_failsafe = status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF
+							&& in_forward_flight
+							&& !mission_finished;
 
-			set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
+			if (is_armed
+			    && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
+				// nothing to do - everything done in check_invalid_pos_nav_state
 
-		} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) {
-			// Data link lost, data link loss reaction configured -> do configured reaction
-			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink);
-			set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);
+			} else if (status_flags.vtol_transition_failure) {
+				set_quadchute_nav_state(status, armed, status_flags, quadchute_act);
 
-		} else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD)
-			   && status_flags.rc_signal_found_once && is_armed && !landed) {
-			// RC link lost, rc loss not disabled in loiter, RC was used before -> RC loss reaction after delay
-			// Safety pilot expects to be able to take over by RC in case anything unexpected happens
-			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc);
-			set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t);
+			} else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed && !deny_link_failsafe) {
+				// Data link lost, data link loss reaction configured -> do configured reaction
+				enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink);
+				set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act, 0);
 
-		} else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD)
-			   && status.data_link_lost && !data_link_loss_act_configured
-			   && is_armed && !landed) {
-			// All links lost, no data link loss reaction configured -> immediately do RC loss reaction
-			// Lost all communication, by default it's considered unsafe to continue the mission
-			// This is only reached when flying mission completely without RC (it was not present since boot)
-			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc_and_no_datalink);
-			set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);
+			} else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD)
+				   && status_flags.rc_signal_found_once && is_armed && !landed && !deny_link_failsafe) {
+				// RC link lost, rc loss not disabled in loiter, RC was used before -> RC loss reaction after delay
+				// Safety pilot expects to be able to take over by RC in case anything unexpected happens
+				enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc);
+				set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t);
 
-		} else {
-			status.nav_state = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF ?
-					   vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF : vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF;
+			} else if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD)
+				   && status.data_link_lost && !data_link_loss_act_configured
+				   && is_armed && !landed && !deny_link_failsafe) {
+				// All links lost, no data link loss reaction configured -> immediately do RC loss reaction
+				// Lost all communication, by default it's considered unsafe to continue the mission
+				// This is only reached when flying mission completely without RC (it was not present since boot)
+				enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_datalink);
+				set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);
+
+			} else {
+				status.nav_state = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF ?
+						   vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF : vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF;
+			}
+
+			break;
 		}
 
-		break;
-
 	case commander_state_s::MAIN_STATE_AUTO_LAND:
 
 		// require local position
@@ -595,7 +678,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 	case commander_state_s::MAIN_STATE_OFFBOARD:
 
 		if (status_flags.offboard_control_signal_lost) {
-			if (status.rc_signal_lost) {
+			if (status.rc_signal_lost && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_OFFBOARD)) {
 				// Offboard and RC are lost
 				enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_rc_and_no_offboard);
 				set_offboard_loss_nav_state(status, armed, status_flags, offb_loss_act);
@@ -625,6 +708,12 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
 
 		break;
 
+	case commander_state_s::MAIN_STATE_INIT:
+		status.nav_state = vehicle_status_s::NAVIGATION_STATE_INIT;
+		break;
+
+		break;
+
 	default:
 		break;
 	}
@@ -647,6 +736,14 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or
 	}
 
 	if (fallback_required) {
+
+		if (using_global_pos) {
+			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_global_position);
+
+		} else {
+			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_local_position);
+		}
+
 		if (use_rc) {
 			// fallback to a mode that gives the operator stick control
 			if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
@@ -673,14 +770,6 @@ bool check_invalid_pos_nav_state(vehicle_status_s &status, bool old_failsafe, or
 				status.nav_state = vehicle_status_s::NAVIGATION_STATE_TERMINATION;
 			}
 		}
-
-		if (using_global_pos) {
-			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_global_position);
-
-		} else {
-			enable_failsafe(status, old_failsafe, mavlink_log_pub, event_failsafe_reason_t::no_local_position);
-		}
-
 	}
 
 	return fallback_required;
@@ -943,7 +1032,7 @@ void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t batter
 	// User warning
 	switch (battery_warning) {
 	case battery_status_s::BATTERY_WARNING_LOW:
-		mavlink_log_critical(mavlink_log_pub, "Low %s, return advised\t", battery_level);
+		mavlink_log_gcs_critical(mavlink_log_pub, "Low %s, return advised\t", battery_level);
 		events::send(events::ID("commander_bat_low"), {events::Log::Warning, events::LogInternal::Info},
 			     "Low battery level, return advised");
 
@@ -951,13 +1040,13 @@ void warn_user_about_battery(orb_advert_t *mavlink_log_pub, const uint8_t batter
 
 	case battery_status_s::BATTERY_WARNING_CRITICAL:
 		if (failsafe_action == commander_state_s::MAIN_STATE_MAX) {
-			mavlink_log_critical(mavlink_log_pub, "Critical %s, return now\t", battery_level);
+			mavlink_log_gcs_critical(mavlink_log_pub, "Critical %s, return now\t", battery_level);
 			events::send(events::ID("commander_bat_crit"), {events::Log::Critical, events::LogInternal::Info},
 				     "Critical battery level, return now");
 
 		} else {
-			mavlink_log_critical(mavlink_log_pub, "Critical %s, executing %s in %d seconds\t", battery_level,
-					     failsafe_action_string, static_cast<uint16_t>(param_com_bat_act_t));
+			mavlink_log_gcs_critical(mavlink_log_pub, "Critical %s, executing %s in %d seconds\t", battery_level,
+						 failsafe_action_string, static_cast<uint16_t>(param_com_bat_act_t));
 			events::send<events::px4::enums::navigation_mode_t, uint16_t>(events::ID("commander_bat_crit_act"), {events::Log::Critical, events::LogInternal::Info},
 					"Critical battery level, executing {1} in {2} seconds",
 					failsafe_action_navigation_mode, static_cast<uint16_t>(param_com_bat_act_t));
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
index 919a2be778..905ec2602b 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp
@@ -42,6 +42,7 @@
 #pragma once
 
 #include <matrix/matrix/math.hpp>
+#include <uORB/topics/control_allocator_status.h>
 
 enum class AllocationMethod {
 	NONE = -1,
@@ -187,13 +188,20 @@ public:
 	 * @param actuator_sp input & output setpoint
 	 */
 	virtual void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-				    int matrix_index, ActuatorVector &actuator_sp) {}
+				    int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+				    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) {}
 
 	/**
 	 * Get a bitmask of motors to be stopped
 	 */
 	virtual uint32_t getStoppedMotors() const { return 0; }
 
+	/**
+	 * Fill in the unallocated torque and thrust, customized by effectiveness type.
+	 * Can be implemented for every type separately. If not implemented then the effectivenes matrix is used instead.
+	 */
+	virtual void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) {}
+
 protected:
 	FlightPhase _flight_phase{FlightPhase::HOVER_FLIGHT};		///< Current flight phase
 };
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp
index bddc95e913..5d6c877acb 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp
@@ -62,7 +62,8 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat
 }
 
 void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp)
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	// apply flaps
 	actuator_controls_s actuator_controls_0;
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp
index 35583eaf8b..1f8183992d 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp
@@ -50,7 +50,8 @@ public:
 	const char *name() const override { return "Fixed Wing"; }
 
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp) override;
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 private:
 	ActuatorEffectivenessRotors _rotors;
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
index 41fc850954..f5a7aaedbb 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.cpp
@@ -35,6 +35,7 @@
 #include <lib/mathlib/mathlib.h>
 
 using namespace matrix;
+using namespace time_literals;
 
 ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *parent)
 	: ModuleParams(parent)
@@ -45,6 +46,8 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p
 		_param_handles.swash_plate_servos[i].angle = param_find(buffer);
 		snprintf(buffer, sizeof(buffer), "CA_SP0_ARM_L%u", i);
 		_param_handles.swash_plate_servos[i].arm_length = param_find(buffer);
+		snprintf(buffer, sizeof(buffer), "CA_SV_CS%u_TRIM", i);
+		_param_handles.swash_plate_servos[i].trim = param_find(buffer);
 	}
 
 	_param_handles.num_swash_plate_servos = param_find("CA_SP0_COUNT");
@@ -57,6 +60,11 @@ ActuatorEffectivenessHelicopter::ActuatorEffectivenessHelicopter(ModuleParams *p
 		_param_handles.pitch_curve[i] = param_find(buffer);
 	}
 
+	_param_handles.yaw_collective_pitch_scale = param_find("CA_HELI_YAW_CP_S");
+	_param_handles.yaw_throttle_scale = param_find("CA_HELI_YAW_TH_S");
+	_param_handles.yaw_ccw = param_find("CA_HELI_YAW_CCW");
+	_param_handles.spoolup_time = param_find("COM_SPOOLUP_TIME");
+
 	updateParams();
 }
 
@@ -78,12 +86,20 @@ void ActuatorEffectivenessHelicopter::updateParams()
 		param_get(_param_handles.swash_plate_servos[i].angle, &angle_deg);
 		_geometry.swash_plate_servos[i].angle = math::radians(angle_deg);
 		param_get(_param_handles.swash_plate_servos[i].arm_length, &_geometry.swash_plate_servos[i].arm_length);
+		param_get(_param_handles.swash_plate_servos[i].trim, &_geometry.swash_plate_servos[i].trim);
 	}
 
 	for (int i = 0; i < NUM_CURVE_POINTS; ++i) {
 		param_get(_param_handles.throttle_curve[i], &_geometry.throttle_curve[i]);
 		param_get(_param_handles.pitch_curve[i], &_geometry.pitch_curve[i]);
 	}
+
+	param_get(_param_handles.yaw_collective_pitch_scale, &_geometry.yaw_collective_pitch_scale);
+	param_get(_param_handles.yaw_throttle_scale, &_geometry.yaw_throttle_scale);
+	param_get(_param_handles.spoolup_time, &_geometry.spoolup_time);
+	int32_t yaw_ccw = 0;
+	param_get(_param_handles.yaw_ccw, &yaw_ccw);
+	_geometry.yaw_sign = (yaw_ccw == 1) ? -1.f : 1.f;
 }
 
 bool
@@ -97,47 +113,140 @@ ActuatorEffectivenessHelicopter::getEffectivenessMatrix(Configuration &configura
 	// As the allocation is non-linear, we use updateSetpoint() instead of the matrix
 	configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
 
-	// Tail (yaw) servo
-	configuration.addActuator(ActuatorType::SERVOS, Vector3f{0.f, 0.f, 1.f}, Vector3f{});
+	// Tail (yaw) motor
+	configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{});
 
 	// N swash plate servos
 	_first_swash_plate_servo_index = configuration.num_actuators_matrix[0];
 
 	for (int i = 0; i < _geometry.num_swash_plate_servos; ++i) {
 		configuration.addActuator(ActuatorType::SERVOS, Vector3f{}, Vector3f{});
+		configuration.trim[configuration.selected_matrix](i) = _geometry.swash_plate_servos[i].trim;
 	}
 
 	return true;
 }
 
 void ActuatorEffectivenessHelicopter::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp)
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
-	// Find index to use for curves
-	float thrust_cmd = -control_sp(ControlAxis::THRUST_Z);
-	float num_intervals = NUM_CURVE_POINTS - 1;
-	// We access idx + 1 below, so max legal index is (size - 2)
-	int idx = math::constrain((int)(thrust_cmd * num_intervals), 0, NUM_CURVE_POINTS - 2);
+	_saturation_flags = {};
 
-	// Local throttle curve gradient and offset
-	float tg = (_geometry.throttle_curve[idx + 1] - _geometry.throttle_curve[idx]) * num_intervals;
-	float to = (_geometry.throttle_curve[idx]) - (tg * idx / num_intervals);
-	float throttle = math::constrain(tg * thrust_cmd + to, 0.0f, 1.0f);
+	// throttle/collective pitch curve
+	const float throttle = math::interpolateN(-control_sp(ControlAxis::THRUST_Z),
+			       _geometry.throttle_curve) * throttleSpoolupProgress();
+	const float collective_pitch = math::interpolateN(-control_sp(ControlAxis::THRUST_Z), _geometry.pitch_curve);
 
-	// Local pitch curve gradient and offset
-	float pg = (_geometry.pitch_curve[idx + 1] - _geometry.pitch_curve[idx]) * num_intervals;
-	float po = (_geometry.pitch_curve[idx]) - (pg * idx / num_intervals);
-	float collective_pitch = math::constrain((pg * thrust_cmd + po), -0.5f, 0.5f);
+	// actuator mapping
+	actuator_sp(0) = mainMotorEnaged() ? throttle : NAN;
 
-	float roll_cmd = control_sp(ControlAxis::ROLL);
-	float pitch_cmd = control_sp(ControlAxis::PITCH);
+	actuator_sp(1) = control_sp(ControlAxis::YAW) * _geometry.yaw_sign
+			 + fabsf(collective_pitch) * _geometry.yaw_collective_pitch_scale
+			 + throttle * _geometry.yaw_throttle_scale;
 
-	actuator_sp(0) = throttle;
+	// Saturation check for yaw
+	if (actuator_sp(1) < actuator_min(1)) {
+		setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_neg, _saturation_flags.yaw_pos);
 
-	for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
-		actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch
-				+ cosf(_geometry.swash_plate_servos[i].angle) * pitch_cmd * _geometry.swash_plate_servos[i].arm_length
-				- sinf(_geometry.swash_plate_servos[i].angle) * roll_cmd * _geometry.swash_plate_servos[i].arm_length;
+	} else if (actuator_sp(1) > actuator_max(1)) {
+		setSaturationFlag(_geometry.yaw_sign, _saturation_flags.yaw_pos, _saturation_flags.yaw_neg);
 	}
 
+	for (int i = 0; i < _geometry.num_swash_plate_servos; i++) {
+		float roll_coeff = sinf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length;
+		float pitch_coeff = cosf(_geometry.swash_plate_servos[i].angle) * _geometry.swash_plate_servos[i].arm_length;
+		actuator_sp(_first_swash_plate_servo_index + i) = collective_pitch
+				+ control_sp(ControlAxis::PITCH) * pitch_coeff
+				- control_sp(ControlAxis::ROLL) * roll_coeff
+				+ _geometry.swash_plate_servos[i].trim;
+
+		// Saturation check for roll & pitch
+		if (actuator_sp(_first_swash_plate_servo_index + i) < actuator_min(_first_swash_plate_servo_index + i)) {
+			setSaturationFlag(roll_coeff, _saturation_flags.roll_pos, _saturation_flags.roll_neg);
+			setSaturationFlag(pitch_coeff, _saturation_flags.pitch_neg, _saturation_flags.pitch_pos);
+
+		} else if (actuator_sp(_first_swash_plate_servo_index + i) > actuator_max(_first_swash_plate_servo_index + i)) {
+			setSaturationFlag(roll_coeff, _saturation_flags.roll_neg, _saturation_flags.roll_pos);
+			setSaturationFlag(pitch_coeff, _saturation_flags.pitch_pos, _saturation_flags.pitch_neg);
+		}
+	}
+}
+
+bool ActuatorEffectivenessHelicopter::mainMotorEnaged()
+{
+	manual_control_switches_s manual_control_switches;
+
+	if (_manual_control_switches_sub.update(&manual_control_switches)) {
+		_main_motor_engaged = manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_NONE
+				      || manual_control_switches.engage_main_motor_switch == manual_control_switches_s::SWITCH_POS_ON;
+	}
+
+	return _main_motor_engaged;
+}
+
+float ActuatorEffectivenessHelicopter::throttleSpoolupProgress()
+{
+	vehicle_status_s vehicle_status;
+
+	if (_vehicle_status_sub.update(&vehicle_status)) {
+		_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
+		_armed_time = vehicle_status.armed_time;
+	}
+
+	const float time_since_arming = (hrt_absolute_time() - _armed_time) / 1e6f;
+	const float spoolup_progress = time_since_arming / _geometry.spoolup_time;
+
+	if (_armed && spoolup_progress < 1.f) {
+		return spoolup_progress;
+	}
+
+	return 1.f;
+}
+
+
+void ActuatorEffectivenessHelicopter::setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag)
+{
+	if (coeff > 0.f) {
+		// A positive change in given axis will increase saturation
+		positive_flag = true;
+
+	} else if (coeff < 0.f) {
+		// A negative change in given axis will increase saturation
+		negative_flag = true;
+	}
+}
+
+void ActuatorEffectivenessHelicopter::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
+{
+
+	// Note: the values '-1', '1' and '0' are just to indicate a negative,
+	// positive or no saturation to the rate controller. The actual magnitude is not used.
+	if (_saturation_flags.roll_pos) {
+		status.unallocated_torque[0] = 1.f;
+
+	} else if (_saturation_flags.roll_neg) {
+		status.unallocated_torque[0] = -1.f;
+	}
+
+	if (_saturation_flags.pitch_pos) {
+		status.unallocated_torque[1] = 1.f;
+
+	} else if (_saturation_flags.pitch_neg) {
+		status.unallocated_torque[1] = -1.f;
+	}
+
+	if (_saturation_flags.yaw_pos) {
+		status.unallocated_torque[2] = 1.f;
+
+	} else if (_saturation_flags.yaw_neg) {
+		status.unallocated_torque[2] = -1.f;
+	}
+
+	if (_saturation_flags.thrust_pos) {
+		status.unallocated_thrust[2] = 1.f;
+
+	} else if (_saturation_flags.thrust_neg) {
+		status.unallocated_thrust[2] = -1.f;
+	}
 }
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
index 3438f017a5..3cd64b2f36 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopter.hpp
@@ -37,6 +37,10 @@
 
 #include <px4_platform_common/module_params.h>
 
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/manual_control_switches.h>
+
 class ActuatorEffectivenessHelicopter : public ModuleParams, public ActuatorEffectiveness
 {
 public:
@@ -47,6 +51,7 @@ public:
 	struct SwashPlateGeometry {
 		float angle;
 		float arm_length;
+		float trim;
 	};
 
 	struct Geometry {
@@ -54,6 +59,10 @@ public:
 		int num_swash_plate_servos{0};
 		float throttle_curve[NUM_CURVE_POINTS];
 		float pitch_curve[NUM_CURVE_POINTS];
+		float yaw_collective_pitch_scale;
+		float yaw_throttle_scale;
+		float yaw_sign;
+		float spoolup_time;
 	};
 
 	ActuatorEffectivenessHelicopter(ModuleParams *parent);
@@ -67,23 +76,55 @@ public:
 	const Geometry &geometry() const { return _geometry; }
 
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp) override;
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
+	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
 private:
+	float throttleSpoolupProgress();
+	bool mainMotorEnaged();
+
 	void updateParams() override;
 
+	struct SaturationFlags {
+		bool roll_pos;
+		bool roll_neg;
+		bool pitch_pos;
+		bool pitch_neg;
+		bool yaw_pos;
+		bool yaw_neg;
+		bool thrust_pos;
+		bool thrust_neg;
+	};
+	static void setSaturationFlag(float coeff, bool &positive_flag, bool &negative_flag);
+
 	struct ParamHandlesSwashPlate {
 		param_t angle;
 		param_t arm_length;
+		param_t trim;
 	};
 	struct ParamHandles {
 		ParamHandlesSwashPlate swash_plate_servos[NUM_SWASH_PLATE_SERVOS_MAX];
 		param_t num_swash_plate_servos;
 		param_t throttle_curve[NUM_CURVE_POINTS];
 		param_t pitch_curve[NUM_CURVE_POINTS];
+		param_t yaw_collective_pitch_scale;
+		param_t yaw_throttle_scale;
+		param_t yaw_ccw;
+		param_t spoolup_time;
 	};
 	ParamHandles _param_handles{};
 
 	Geometry _geometry{};
 
 	int _first_swash_plate_servo_index{};
+	SaturationFlags _saturation_flags;
+
+	// Throttle spoolup state
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	bool _armed{false};
+	uint64_t _armed_time{0};
+
+	uORB::Subscription _manual_control_switches_sub{ORB_ID(manual_control_switches)};
+	bool _main_motor_engaged{true};
 };
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp
new file mode 100644
index 0000000000..c0f29f5a20
--- /dev/null
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessHelicopterTest.cpp
@@ -0,0 +1,111 @@
+/****************************************************************************
+ *
+ *   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 "ActuatorEffectivenessHelicopter.hpp"
+
+using namespace matrix;
+
+TEST(ActuatorEffectivenessHelicopterTest, ThrottleCurve)
+{
+	// Disable autosaving parameters to avoid busy loop in param_set()
+	param_control_autosave(false);
+
+	// Set parameters for a predefined throttle curve
+	float values[5] = {0.f, .3f, .6f, .8f, 1.f};
+	param_t param;
+
+	for (int i = 0; i < 5; i++) {
+		char buffer[17];
+		snprintf(buffer, sizeof(buffer), "CA_HELI_THR_C%u", i);
+		param = param_find(buffer);
+		param_set(param, &values[i]);
+	}
+
+	ActuatorEffectivenessHelicopter helicopter(nullptr);
+	// run getEffectivenessMatrix with empty configuration to correctly initialize _first_swash_plate_servo_index
+	ActuatorEffectiveness::Configuration configuration{};
+	EffectivenessUpdateReason external_update = EffectivenessUpdateReason::MOTOR_ACTIVATION_UPDATE;
+	helicopter.getEffectivenessMatrix(configuration, external_update);
+
+	Vector<float, 6> control_sp{};
+	ActuatorEffectiveness::ActuatorVector actuator_sp{};
+	ActuatorEffectiveness::ActuatorVector actuator_min{};
+	actuator_min.setAll(0.f);
+	ActuatorEffectiveness::ActuatorVector actuator_max{};
+	actuator_max.setAll(1.f);
+
+	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = 0.1f;
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
+	EXPECT_FLOAT_EQ(actuator_sp(0), 0.f);
+
+	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = 0.f;
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
+	EXPECT_FLOAT_EQ(actuator_sp(0), 0.f);
+
+	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.125f;
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
+	EXPECT_FLOAT_EQ(actuator_sp(0), .15f);
+
+	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.25f;
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
+	EXPECT_FLOAT_EQ(actuator_sp(0), .3f);
+
+	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.375f;
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
+	EXPECT_FLOAT_EQ(actuator_sp(0), .45f);
+
+	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.5f;
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
+	EXPECT_FLOAT_EQ(actuator_sp(0), .6f);
+
+	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.625f;
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
+	EXPECT_FLOAT_EQ(actuator_sp(0), .7f);
+
+	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.75f;
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
+	EXPECT_FLOAT_EQ(actuator_sp(0), .8f);
+
+	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -.875f;
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
+	EXPECT_FLOAT_EQ(actuator_sp(0), .9f);
+
+	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -1.f;
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
+	EXPECT_FLOAT_EQ(actuator_sp(0), 1.f);
+
+	control_sp(ActuatorEffectiveness::ControlAxis::THRUST_Z) = -1.1f;
+	helicopter.updateSetpoint(control_sp, 0, actuator_sp, actuator_min, actuator_max);
+	EXPECT_FLOAT_EQ(actuator_sp(0), 1.f);
+}
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp
index 4ac953b53b..41005d38a7 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp
@@ -77,7 +77,8 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration
 }
 
 void ActuatorEffectivenessMCTilt::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp)
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	actuator_sp += _tilt_offsets;
 	// TODO: dynamic matrix update
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp
index db9f790c51..15c12957ed 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp
@@ -56,7 +56,8 @@ public:
 	}
 
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp) override;
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	const char *name() const override { return "MC Tilt"; }
 
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp
index 65b987aba4..2afb41b002 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp
@@ -233,10 +233,11 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry
 	return num_actuators;
 }
 
-uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts, float tilt_control)
+uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectivenessTilts &tilts,
+		float collective_tilt_control)
 {
-	if (!PX4_ISFINITE(tilt_control)) {
-		tilt_control = -1.f;
+	if (!PX4_ISFINITE(collective_tilt_control)) {
+		collective_tilt_control = -1.f;
 	}
 
 	uint32_t nontilted_motors = 0;
@@ -250,8 +251,8 @@ uint32_t ActuatorEffectivenessRotors::updateAxisFromTilts(const ActuatorEffectiv
 		}
 
 		const ActuatorEffectivenessTilts::Params &tilt = tilts.config(tilt_index);
-		float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (tilt_control + 1.f) / 2.f);
-		float tilt_direction = math::radians((float)tilt.tilt_direction);
+		const float tilt_angle = math::lerp(tilt.min_angle, tilt.max_angle, (collective_tilt_control + 1.f) / 2.f);
+		const float tilt_direction = math::radians((float)tilt.tilt_direction);
 		_geometry.rotors[i].axis = tiltedAxis(tilt_angle, tilt_direction);
 	}
 
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
index 692f76e53c..2575d48685 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.cpp
@@ -64,7 +64,8 @@ ActuatorEffectivenessStandardVTOL::getEffectivenessMatrix(Configuration &configu
 }
 
 void ActuatorEffectivenessStandardVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp)
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	// apply flaps
 	if (matrix_index == 1) {
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp
index 7eddbba718..174d499941 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp
@@ -73,7 +73,8 @@ public:
 	}
 
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp) override;
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
index 675548cab6..6f44ecde7d 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
@@ -54,7 +54,7 @@ bool
 ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &configuration,
 		EffectivenessUpdateReason external_update)
 {
-	if (!_combined_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
+	if (!_collective_tilt_updated && external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
 		return false;
 	}
 
@@ -66,9 +66,9 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 	// Update matrix with tilts in vertical position when update is triggered by a manual
 	// configuration (parameter) change. This is to make sure the normalization
 	// scales are tilt-invariant. Note: configuration updates are only possible when disarmed.
-	const float tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ? -1.f :
-					   _last_tilt_control;
-	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, tilt_control_applied)
+	const float collective_tilt_control_applied = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE) ?
+			-1.f : _last_collective_tilt_control;
+	_nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, collective_tilt_control_applied)
 			    << configuration.num_actuators[(int)ActuatorType::MOTORS];
 
 	const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
@@ -86,13 +86,14 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
 
 	// If it was an update coming from a config change, then make sure to update matrix in
 	// the next iteration again with the correct tilt (but without updating the normalization scale).
-	_combined_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
+	_collective_tilt_updated = (external_update == EffectivenessUpdateReason::CONFIGURATION_UPDATE);
 
 	return (mc_rotors_added_successfully && surfaces_added_successfully && tilts_added_successfully);
 }
 
 void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp)
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
 {
 	// apply flaps
 	if (matrix_index == 1) {
@@ -110,24 +111,24 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 		actuator_controls_s actuator_controls_1;
 
 		if (_actuator_controls_1_sub.copy(&actuator_controls_1)) {
-			float control_tilt = actuator_controls_1.control[actuator_controls_s::INDEX_COLLECTIVE_TILT] * 2.f - 1.f;
+			float control_collective_tilt = actuator_controls_1.control[actuator_controls_s::INDEX_COLLECTIVE_TILT] * 2.f - 1.f;
 
-			// set control_tilt to exactly -1 or 1 if close to these end points
-			control_tilt = control_tilt < -0.99f ? -1.f : control_tilt;
-			control_tilt = control_tilt > 0.99f ? 1.f : control_tilt;
+			// set control_collective_tilt to exactly -1 or 1 if close to these end points
+			control_collective_tilt = control_collective_tilt < -0.99f ? -1.f : control_collective_tilt;
+			control_collective_tilt = control_collective_tilt > 0.99f ? 1.f : control_collective_tilt;
 
-			// initialize _last_tilt_control
-			if (!PX4_ISFINITE(_last_tilt_control)) {
-				_last_tilt_control = control_tilt;
+			// initialize _last_collective_tilt_control
+			if (!PX4_ISFINITE(_last_collective_tilt_control)) {
+				_last_collective_tilt_control = control_collective_tilt;
 
-			} else if (fabsf(control_tilt - _last_tilt_control) > 0.01f) {
-				_combined_tilt_updated = true;
-				_last_tilt_control = control_tilt;
+			} else if (fabsf(control_collective_tilt - _last_collective_tilt_control) > 0.01f) {
+				_collective_tilt_updated = true;
+				_last_collective_tilt_control = control_collective_tilt;
 			}
 
 			for (int i = 0; i < _tilts.count(); ++i) {
 				if (_tilts.config(i).tilt_direction == ActuatorEffectivenessTilts::TiltDirection::TowardsFront) {
-					actuator_sp(i + _first_tilt_idx) += control_tilt;
+					actuator_sp(i + _first_tilt_idx) += control_collective_tilt;
 				}
 			}
 		}
@@ -144,6 +145,21 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
 			}
 		}
 	}
+
+	// Set yaw saturation flag in case of yaw through tilt. As in this case the yaw actuation is decoupled from
+	// the other axes (for now neglecting the case of 0 collective thrust), we set the saturation flags
+	// directly if the (normalized) yaw torque setpoint is outside of range (-1, 1).
+	if (matrix_index == 0 && _tilts.hasYawControl()) {
+		_yaw_tilt_saturation_flags.tilt_yaw_neg = false;
+		_yaw_tilt_saturation_flags.tilt_yaw_pos = false;
+
+		if (control_sp(2) < -1.f) {
+			_yaw_tilt_saturation_flags.tilt_yaw_neg = true;
+
+		} else if (control_sp(2) > 1.f) {
+			_yaw_tilt_saturation_flags.tilt_yaw_pos = true;
+		}
+	}
 }
 
 void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &flight_phase)
@@ -167,3 +183,23 @@ void ActuatorEffectivenessTiltrotorVTOL::setFlightPhase(const FlightPhase &fligh
 		break;
 	}
 }
+
+void ActuatorEffectivenessTiltrotorVTOL::getUnallocatedControl(int matrix_index, control_allocator_status_s &status)
+{
+	// only handle matrix 0 (motors and tilts)
+	if (matrix_index == 1) {
+		return;
+	}
+
+	// Note: the values '-1', '1' and '0' are just to indicate a negative,
+	// positive or no saturation to the rate controller. The actual magnitude is not used.
+	if (_yaw_tilt_saturation_flags.tilt_yaw_pos) {
+		status.unallocated_torque[2] = 1.f;
+
+	} else if (_yaw_tilt_saturation_flags.tilt_yaw_neg) {
+		status.unallocated_torque[2] = -1.f;
+
+	} else {
+		status.unallocated_torque[2] = 0.f;
+	}
+}
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
index fb7219973f..5686d2ebda 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
@@ -75,13 +75,17 @@ public:
 	void setFlightPhase(const FlightPhase &flight_phase) override;
 
 	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp) override;
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
 
 	const char *name() const override { return "VTOL Tiltrotor"; }
 
 	uint32_t getStoppedMotors() const override { return _stopped_motors; }
+
+	void getUnallocatedControl(int matrix_index, control_allocator_status_s &status) override;
+
 protected:
-	bool _combined_tilt_updated{true};
+	bool _collective_tilt_updated{true};
 	ActuatorEffectivenessRotors _mc_rotors;
 	ActuatorEffectivenessControlSurfaces _control_surfaces;
 	ActuatorEffectivenessTilts _tilts;
@@ -92,8 +96,15 @@ protected:
 	int _first_control_surface_idx{0}; ///< applies to matrix 1
 	int _first_tilt_idx{0}; ///< applies to matrix 0
 
-	float _last_tilt_control{NAN};
+	float _last_collective_tilt_control{NAN};
 
 	uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
 	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
+
+	struct YawTiltSaturationFlags {
+		bool tilt_yaw_pos;
+		bool tilt_yaw_neg;
+	};
+
+	YawTiltSaturationFlags _yaw_tilt_saturation_flags{};
 };
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt
index e99d718550..31e338de82 100644
--- a/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt
+++ b/src/modules/control_allocator/ActuatorEffectiveness/CMakeLists.txt
@@ -69,4 +69,5 @@ target_link_libraries(ActuatorEffectiveness
 		mathlib
 )
 
+px4_add_functional_gtest(SRC ActuatorEffectivenessHelicopterTest.cpp LINKLIBS ActuatorEffectiveness)
 px4_add_functional_gtest(SRC ActuatorEffectivenessRotorsTest.cpp LINKLIBS ActuatorEffectiveness)
diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
index cc6d76486c..55b5a870fa 100644
--- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
+++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
@@ -143,7 +143,7 @@ public:
 	 * @return Control vector
 	 */
 	matrix::Vector<float, NUM_AXES> getAllocatedControl() const
-	{ return (_effectiveness * _actuator_sp).emult(_control_allocation_scale); }
+	{ return (_effectiveness * (_actuator_sp - _actuator_trim)).emult(_control_allocation_scale); }
 
 	/**
 	 * Get the control effectiveness matrix
diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp
index a90a0da4d7..fdef4890cc 100644
--- a/src/modules/control_allocator/ControlAllocator.cpp
+++ b/src/modules/control_allocator/ControlAllocator.cpp
@@ -54,7 +54,9 @@ ControlAllocator::ControlAllocator() :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
-	_control_allocator_status_pub.advertise();
+	_control_allocator_status_pub[0].advertise();
+	_control_allocator_status_pub[1].advertise();
+
 	_actuator_motors_pub.advertise();
 	_actuator_servos_pub.advertise();
 	_actuator_servos_trim_pub.advertise();
@@ -414,7 +416,8 @@ ControlAllocator::Run()
 
 			// Do allocation
 			_control_allocation[i]->allocate();
-			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp);
+			_actuator_effectiveness->updateSetpoint(c[i], i, _control_allocation[i]->_actuator_sp,
+								_control_allocation[i]->getActuatorMin(), _control_allocation[i]->getActuatorMax());
 
 			if (_has_slew_rate) {
 				_control_allocation[i]->applySlewRateLimit(dt);
@@ -430,7 +433,12 @@ ControlAllocator::Run()
 	// Publish status at limited rate, as it's somewhat expensive and we use it for slower dynamics
 	// (i.e. anti-integrator windup)
 	if (now - _last_status_pub >= 5_ms) {
-		publish_control_allocator_status();
+		publish_control_allocator_status(0);
+
+		if (_num_control_allocation > 1) {
+			publish_control_allocator_status(1);
+		}
+
 		_last_status_pub = now;
 	}
 
@@ -571,24 +579,18 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
 }
 
 void
-ControlAllocator::publish_control_allocator_status()
+ControlAllocator::publish_control_allocator_status(int matrix_index)
 {
 	control_allocator_status_s control_allocator_status{};
 	control_allocator_status.timestamp = hrt_absolute_time();
 
-	// TODO: handle multiple matrices & disabled motors (?)
+	// TODO: disabled motors (?)
 
 	// Allocated control
-	const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[0]->getAllocatedControl();
-	control_allocator_status.allocated_torque[0] = allocated_control(0);
-	control_allocator_status.allocated_torque[1] = allocated_control(1);
-	control_allocator_status.allocated_torque[2] = allocated_control(2);
-	control_allocator_status.allocated_thrust[0] = allocated_control(3);
-	control_allocator_status.allocated_thrust[1] = allocated_control(4);
-	control_allocator_status.allocated_thrust[2] = allocated_control(5);
+	const matrix::Vector<float, NUM_AXES> &allocated_control = _control_allocation[matrix_index]->getAllocatedControl();
 
 	// Unallocated control
-	const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[0]->getControlSetpoint() -
+	const matrix::Vector<float, NUM_AXES> unallocated_control = _control_allocation[matrix_index]->getControlSetpoint() -
 			allocated_control;
 	control_allocator_status.unallocated_torque[0] = unallocated_control(0);
 	control_allocator_status.unallocated_torque[1] = unallocated_control(1);
@@ -597,16 +599,21 @@ ControlAllocator::publish_control_allocator_status()
 	control_allocator_status.unallocated_thrust[1] = unallocated_control(4);
 	control_allocator_status.unallocated_thrust[2] = unallocated_control(5);
 
+	// override control_allocator_status in customized saturation logic for certain effectiveness types
+	_actuator_effectiveness->getUnallocatedControl(matrix_index, control_allocator_status);
+
 	// Allocation success flags
-	control_allocator_status.torque_setpoint_achieved = (Vector3f(unallocated_control(0), unallocated_control(1),
-			unallocated_control(2)).norm_squared() < 1e-6f);
-	control_allocator_status.thrust_setpoint_achieved = (Vector3f(unallocated_control(3), unallocated_control(4),
-			unallocated_control(5)).norm_squared() < 1e-6f);
+	control_allocator_status.torque_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_torque[0],
+			control_allocator_status.unallocated_torque[1],
+			control_allocator_status.unallocated_torque[2]).norm_squared() < 1e-6f);
+	control_allocator_status.thrust_setpoint_achieved = (Vector3f(control_allocator_status.unallocated_thrust[0],
+			control_allocator_status.unallocated_thrust[1],
+			control_allocator_status.unallocated_thrust[2]).norm_squared() < 1e-6f);
 
 	// Actuator saturation
-	const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation[0]->getActuatorSetpoint();
-	const matrix::Vector<float, NUM_ACTUATORS> &actuator_min = _control_allocation[0]->getActuatorMin();
-	const matrix::Vector<float, NUM_ACTUATORS> &actuator_max = _control_allocation[0]->getActuatorMax();
+	const matrix::Vector<float, NUM_ACTUATORS> &actuator_sp = _control_allocation[matrix_index]->getActuatorSetpoint();
+	const matrix::Vector<float, NUM_ACTUATORS> &actuator_min = _control_allocation[matrix_index]->getActuatorMin();
+	const matrix::Vector<float, NUM_ACTUATORS> &actuator_max = _control_allocation[matrix_index]->getActuatorMax();
 
 	for (int i = 0; i < NUM_ACTUATORS; i++) {
 		if (actuator_sp(i) > (actuator_max(i) - FLT_EPSILON)) {
@@ -620,7 +627,7 @@ ControlAllocator::publish_control_allocator_status()
 	// Handled motor failures
 	control_allocator_status.handled_motor_failure_mask = _handled_motor_failure_bitmask;
 
-	_control_allocator_status_pub.publish(control_allocator_status);
+	_control_allocator_status_pub[matrix_index].publish(control_allocator_status);
 }
 
 void
diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp
index 1b016dd331..0281aa925e 100644
--- a/src/modules/control_allocator/ControlAllocator.hpp
+++ b/src/modules/control_allocator/ControlAllocator.hpp
@@ -64,6 +64,7 @@
 #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/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_motors.h>
@@ -131,7 +132,7 @@ private:
 
 	void check_for_motor_failures();
 
-	void publish_control_allocator_status();
+	void publish_control_allocator_status(int matrix_index);
 
 	void publish_actuator_controls();
 
@@ -174,7 +175,7 @@ private:
 	uORB::Subscription _vehicle_thrust_setpoint1_sub{ORB_ID(vehicle_thrust_setpoint), 1};	 /**< vehicle thrust setpoint subscription (2. instance) */
 
 	// Outputs
-	uORB::Publication<control_allocator_status_s>	_control_allocator_status_pub{ORB_ID(control_allocator_status)};	/**< actuator setpoint publication */
+	uORB::PublicationMulti<control_allocator_status_s> _control_allocator_status_pub[2] {ORB_ID(control_allocator_status), ORB_ID(control_allocator_status)};
 
 	uORB::Publication<actuator_motors_s>	_actuator_motors_pub{ORB_ID(actuator_motors)};
 	uORB::Publication<actuator_servos_s>	_actuator_servos_pub{ORB_ID(actuator_servos)};
diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml
index c92e29edeb..7c57d0ab4d 100644
--- a/src/modules/control_allocator/module.yaml
+++ b/src/modules/control_allocator/module.yaml
@@ -391,6 +391,8 @@ parameters:
         CA_SP0_ANG${i}:
             description:
                 short: Angle for swash plate servo ${i}
+                long: |
+                  The angle is measured clockwise (as seen from top), with 0 pointing forwards (X axis).
             type: float
             decimal: 0
             increment: 10
@@ -422,19 +424,59 @@ parameters:
             num_instances: 5
             min: 0
             max: 1
-            default: [0, 0.3, 0.6, 0.8, 1]
+            default: [1, 1, 1, 1, 1]
         CA_HELI_PITCH_C${i}:
             description:
                 short: Collective pitch curve at position ${i}
                 long: |
                   Defines the collective pitch at the interval position ${i} for a given thrust setpoint.
+
+                  Use negative values if the swash plate needs to move down to provide upwards thrust.
             type: float
             decimal: 3
             increment: 0.1
             num_instances: 5
-            min: 0
+            min: -1
             max: 1
-            default: [0.05, 0.15, 0.25, 0.35, 0.45]
+            default: [-0.05, 0.0725, 0.2, 0.325, 0.45]
+        CA_HELI_YAW_CP_S:
+          description:
+            short: Scale for yaw compensation based on collective pitch
+            long: |
+              This allows to add a proportional factor of the collective pitch command to the yaw command.
+              A negative value is needed when positive thrust of the tail rotor rotates the vehicle opposite to the main rotor turn direction.
+
+              tail_output += CA_HELI_YAW_CP_S * collective_pitch
+          type: float
+          decimal: 3
+          increment: 0.1
+          min: -2
+          max: 2
+          default: 0.0
+        CA_HELI_YAW_TH_S:
+          description:
+            short: Scale for yaw compensation based on throttle
+            long: |
+              This allows to add a proportional factor of the throttle command to the yaw command.
+              A negative value is needed when positive thrust of the tail rotor rotates the vehicle opposite to the main rotor turn direction.
+
+              tail_output += CA_HELI_YAW_TH_S * throttle
+          type: float
+          decimal: 3
+          increment: 0.1
+          min: -2
+          max: 2
+          default: 0.0
+        CA_HELI_YAW_CCW:
+          description:
+            short: Main rotor turns counter-clockwise
+            long: |
+              Default configuration is for a clockwise turning main rotor and
+              positive thrust of the tail rotor is expected to rotate the vehicle clockwise.
+              Set this parameter to true if the tail rotor provides thrust in counter-clockwise direction
+              which is mostly the case when the main rotor turns counter-clockwise.
+          type: boolean
+          default: 0
 
         # Others
         CA_FAILURE_MODE:
@@ -854,8 +896,8 @@ mixer:
                   - actuator_type: 'motor'
                     count: 1
                     item_label_prefix: ['Rotor']
-                  - actuator_type: 'servo'
-                    item_label_prefix: ['Yaw tail servo']
+                  - actuator_type: 'motor'
+                    item_label_prefix: ['Yaw tail Motor']
                     count: 1
                   - actuator_type: 'servo'
                     group_label: 'Swash plate servos'
@@ -866,3 +908,14 @@ mixer:
                             label: 'Angle'
                           - name: 'CA_SP0_ARM_L${i}'
                             label: 'Arm Length (relative)'
+                          - name: 'CA_SV_CS${i}_TRIM'
+                            label: 'Trim'
+                    parameters:
+                      - label: 'Yaw compensation scale based on collective pitch'
+                        name: CA_HELI_YAW_CP_S
+                      - label: 'Yaw compensation scale based on throttle'
+                        name: CA_HELI_YAW_TH_S
+                      - label: 'Main rotor turns counter-clockwise'
+                        name: CA_HELI_YAW_CCW
+                      - label: 'Throttle spoolup time'
+                        name: COM_SPOOLUP_TIME
diff --git a/src/modules/dataman/dataman.cpp b/src/modules/dataman/dataman.cpp
index 220471db01..35e1d23235 100644
--- a/src/modules/dataman/dataman.cpp
+++ b/src/modules/dataman/dataman.cpp
@@ -50,6 +50,7 @@
 #include <drivers/drv_hrt.h>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
+#include <stdlib.h>
 
 #include "dataman.h"
 
@@ -157,7 +158,7 @@ static unsigned g_func_counts[dm_number_of_funcs];
 
 /* table of maximum number of instances for each item type */
 static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
-	DM_KEY_SAFE_POINTS_MAX,
+	DM_KEY_SAFE_POINT_ITEMS_MAX,
 	DM_KEY_FENCE_POINTS_MAX,
 	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
 	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
@@ -169,7 +170,7 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
 
 /* Table of the len of each item type */
 static constexpr size_t g_per_item_size[DM_KEY_NUM_KEYS] = {
-	sizeof(struct mission_safe_point_s) + DM_SECTOR_HDR_SIZE,
+	sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE,
 	sizeof(struct mission_fence_point_s) + DM_SECTOR_HDR_SIZE,
 	sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE,
 	sizeof(struct mission_item_s) + DM_SECTOR_HDR_SIZE,
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index bcc5b2984a..042c624247 100644
--- a/src/modules/dataman/dataman.h
+++ b/src/modules/dataman/dataman.h
@@ -59,7 +59,7 @@ typedef enum {
 
 #if defined(MEMORY_CONSTRAINED_SYSTEM)
 enum {
-	DM_KEY_SAFE_POINTS_MAX = 8,
+	DM_KEY_SAFE_POINT_ITEMS_MAX = 8,
 	DM_KEY_FENCE_POINTS_MAX = 16,
 	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
 	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
@@ -69,7 +69,7 @@ enum {
 #else
 /** The maximum number of instances for each item type */
 enum {
-	DM_KEY_SAFE_POINTS_MAX = 8,
+	DM_KEY_SAFE_POINT_ITEMS_MAX = 64,
 	DM_KEY_FENCE_POINTS_MAX = 64,
 	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
 	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
@@ -83,11 +83,11 @@ struct dataman_compat_s {
 };
 
 /* increment this define whenever a binary incompatible change is performed */
-#define DM_COMPAT_VERSION	2ULL
+#define DM_COMPAT_VERSION	3ULL
 
 #define DM_COMPAT_KEY ((DM_COMPAT_VERSION << 32) + (sizeof(struct mission_item_s) << 24) + \
 		       (sizeof(struct mission_s) << 16) + (sizeof(struct mission_stats_entry_s) << 12) + \
-		       (sizeof(struct mission_fence_point_s) << 8) + (sizeof(struct mission_safe_point_s) << 4) + \
+		       (sizeof(struct mission_fence_point_s) << 8) + (sizeof(struct mission_item_s) << 4) + \
 		       sizeof(struct dataman_compat_s))
 
 /** Retrieve from the data manager store */
diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h
index 521f108fe6..92dac82bfb 100644
--- a/src/modules/ekf2/EKF/common.h
+++ b/src/modules/ekf2/EKF/common.h
@@ -86,8 +86,9 @@ enum MagFuseType : uint8_t {
 	AUTO    = 0,   	///< The selection of either heading or 3D magnetometer fusion will be automatic
 	HEADING = 1,   	///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg
 	MAG_3D  = 2,   	///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions
-	INDOOR  = 3,   	///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
-	NONE    = 4    	///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
+	UNUSED  = 3,    ///< Not implemented
+	INDOOR  = 4,   	///< The same as option 0, but magnetometer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates.
+	NONE    = 5    	///< Do not use magnetometer under any circumstance. Other sources of yaw may be used if selected via the EKF2_AID_MASK parameter.
 };
 
 enum TerrainFusionMask : uint8_t {
@@ -259,16 +260,15 @@ struct parameters {
 	float accel_noise{3.5e-1f};             ///< IMU acceleration noise use for covariance prediction (m/sec**2)
 
 	// process noise
-	float gyro_bias_p_noise{1.0e-3f};       ///< process noise for IMU rate gyro bias prediction (rad/sec**2)
-	float accel_bias_p_noise{1.0e-2f};      ///< process noise for IMU accelerometer bias prediction (m/sec**3)
-	float mage_p_noise{1.0e-3f};            ///< process noise for earth magnetic field prediction (Gauss/sec)
-	float magb_p_noise{1.0e-4f};            ///< process noise for body magnetic field prediction (Gauss/sec)
-	float wind_vel_p_noise{1.0e-1f};        ///< process noise for wind velocity prediction (m/sec**2)
-	const float wind_vel_p_noise_scaler{0.5f};      ///< scaling of wind process noise with vertical velocity
-
-	float terrain_p_noise{5.0f};            ///< process noise for terrain offset (m/sec)
-	float terrain_gradient{0.5f};           ///< gradient of terrain used to estimate process noise due to changing position (m/m)
-	const float terrain_timeout{10.f};      ///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
+	float gyro_bias_p_noise{1.0e-3f};	///< process noise for IMU rate gyro bias prediction (rad/sec**2)
+	float accel_bias_p_noise{1.0e-2f};	///< process noise for IMU accelerometer bias prediction (m/sec**3)
+	float mage_p_noise{1.0e-3f};		///< process noise for earth magnetic field prediction (Gauss/sec)
+	float magb_p_noise{1.0e-4f};		///< process noise for body magnetic field prediction (Gauss/sec)
+	float wind_vel_nsd{1.0e-2f}; 		///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
+	const float wind_vel_nsd_scaler{0.5f};	///< scaling of wind process noise with vertical velocity
+	float terrain_p_noise{5.0f};		///< process noise for terrain offset (m/sec)
+	float terrain_gradient{0.5f};		///< gradient of terrain used to estimate process noise due to changing position (m/m)
+	const float terrain_timeout{10.f};		///< maximum time for invalid bottom distance measurements before resetting terrain estimate (s)
 
 	// initialization errors
 	float switch_on_gyro_bias{0.1f};        ///< 1-sigma gyro bias uncertainty at switch on (rad/sec)
@@ -404,7 +404,6 @@ struct parameters {
 	float EKFGSF_tas_default{15.0f};                ///< default airspeed value assumed during fixed wing flight if no airspeed measurement available (m/s)
 	const unsigned EKFGSF_reset_delay{1000000};     ///< Number of uSec of bad innovations on main filter in immediate post-takeoff phase before yaw is reset to EKF-GSF value
 	const float EKFGSF_yaw_err_max{0.262f};         ///< Composite yaw 1-sigma uncertainty threshold used to check for convergence (rad)
-	const unsigned EKFGSF_reset_count_limit{3};     ///< Maximum number of times the yaw can be reset to the EKF-GSF yaw estimator value
 };
 
 union fault_status_u {
diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp
index aa06c0d354..38b0997168 100644
--- a/src/modules/ekf2/EKF/control.cpp
+++ b/src/modules/ekf2/EKF/control.cpp
@@ -142,7 +142,8 @@ void Ekf::controlFusionModes()
 			_range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt());
 
 			// Run the kinematic consistency check when not moving horizontally
-			if ((sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
+			if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
+			    && (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
 				_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
 				_rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _time_last_imu);
 			}
@@ -1121,10 +1122,3 @@ void Ekf::controlAuxVelFusion()
 		}
 	}
 }
-
-bool Ekf::hasHorizontalAidingTimedOut() const
-{
-	return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
-	       && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
-	       && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
-}
diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp
index a4631004a4..4af55707a5 100644
--- a/src/modules/ekf2/EKF/covariance.cpp
+++ b/src/modules/ekf2/EKF/covariance.cpp
@@ -187,7 +187,7 @@ void Ekf::predictCovariance()
 		mag_B_sig = 0.0f;
 	}
 
-	float wind_vel_sig;
+	float wind_vel_nsd_scaled;
 
 	// Calculate low pass filtered height rate
 	float alpha_height_rate_lpf = 0.1f * dt; // 10 seconds time constant
@@ -195,10 +195,10 @@ void Ekf::predictCovariance()
 
 	// Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned
 	if (_control_status.flags.wind && (P(22,22) + P(23,23)) < sq(_params.initial_wind_uncertainty)) {
-		wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f) * (1.0f + _params.wind_vel_p_noise_scaler * fabsf(_height_rate_lpf));
+		wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.0f, 1.0f) * (1.0f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
 
 	} else {
-		wind_vel_sig = 0.0f;
+		wind_vel_nsd_scaled = 0.0f;
 	}
 
 	// compute noise variance for stationary processes
@@ -216,7 +216,7 @@ void Ekf::predictCovariance()
 	// body frame magnetic field states
 	process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig);
 	// wind velocity states
-	process_noise.slice<2, 1>(22, 0) = sq(wind_vel_sig);
+	process_noise.slice<2, 1>(22, 0) = sq(wind_vel_nsd_scaled) * dt;
 
 	// assign IMU noise variances
 	// inputs to the system are 3 delta angles and 3 delta velocities
diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h
index df86bdadf1..96618898fc 100644
--- a/src/modules/ekf2/EKF/ekf.h
+++ b/src/modules/ekf2/EKF/ekf.h
@@ -851,7 +851,6 @@ private:
 	// control fusion of GPS observations
 	void controlGpsFusion();
 	bool shouldResetGpsFusion() const;
-	bool hasHorizontalAidingTimedOut() const;
 	bool isYawFailure() const;
 
 	void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing);
@@ -1055,9 +1054,6 @@ private:
 
 	BaroBiasEstimator _baro_b_est{};
 
-	int64_t _ekfgsf_yaw_reset_time{0};	///< timestamp of last emergency yaw reset (uSec)
-	uint8_t _ekfgsf_yaw_reset_count{0};	// number of times the yaw has been reset to the EKF-GSF estimate
-
 	void runYawEKFGSF();
 
 	// Resets the main Nav EKf yaw to the estimator from the EKF-GSF yaw estimator
diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp
index fc89d182b3..97cb07a33c 100644
--- a/src/modules/ekf2/EKF/ekf_helper.cpp
+++ b/src/modules/ekf2/EKF/ekf_helper.cpp
@@ -1781,9 +1781,6 @@ bool Ekf::resetYawToEKFGSF()
 		_inhibit_ev_yaw_use = true;
 	}
 
-	_ekfgsf_yaw_reset_time = _time_last_imu;
-	_ekfgsf_yaw_reset_count++;
-
 	return true;
 }
 
diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp
index 326390cfae..d3dd6c2813 100644
--- a/src/modules/ekf2/EKF/estimator_interface.cpp
+++ b/src/modules/ekf2/EKF/estimator_interface.cpp
@@ -528,6 +528,17 @@ bool EstimatorInterface::isHorizontalAidingActive() const
 	return getNumberOfActiveHorizontalAidingSources() > 0;
 }
 
+bool EstimatorInterface::isVerticalVelocityAidingActive() const
+{
+	return getNumberOfActiveVerticalVelocityAidingSources() > 0;
+}
+
+int EstimatorInterface::getNumberOfActiveVerticalVelocityAidingSources() const
+{
+	return int(_control_status.flags.gps)
+	       + int(_control_status.flags.ev_vel);
+}
+
 void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
 {
 	if (buffer_name) {
diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h
index 7144ee60fe..b81d92fb1a 100644
--- a/src/modules/ekf2/EKF/estimator_interface.h
+++ b/src/modules/ekf2/EKF/estimator_interface.h
@@ -181,6 +181,9 @@ public:
 
 	int getNumberOfActiveHorizontalAidingSources() const;
 
+	bool isVerticalVelocityAidingActive() const;
+	int getNumberOfActiveVerticalVelocityAidingSources() const;
+
 	const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; }
 
 	// get the velocity of the body frame origin in local NED earth frame
diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp
index 61a8bd7b6f..d17fbb0c87 100644
--- a/src/modules/ekf2/EKF/gps_control.cpp
+++ b/src/modules/ekf2/EKF/gps_control.cpp
@@ -78,34 +78,24 @@ void Ekf::controlGpsFusion()
 					fuseGpsVel();
 					fuseGpsPos();
 
-					if (shouldResetGpsFusion()) {
-						const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);
+					bool do_vel_pos_reset = shouldResetGpsFusion();
 
-						/* A reset is not performed when getting GPS back after a significant period of no data
-						 * because the timeout could have been caused by bad GPS.
-						 * The total number of resets allowed per boot cycle is limited.
+					if (isYawFailure()
+					    && _control_status.flags.in_air
+					    && isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
+					    && (_time_last_hor_vel_fuse > _time_last_on_ground_us)) {
+						/* A rapid reset to the yaw emergency estimate is performed if horizontal velocity innovation checks continuously
+						 * fails while the difference between the yaw emergency estimator and the yas estimate is large.
+						 * This enables recovery from a bad yaw estimate. A reset is not performed if the fault condition was
+						 * present before flight to prevent triggering due to GPS glitches or other sensor errors.
 						 */
-						if (isYawFailure()
-						    && _control_status.flags.in_air
-						    && !was_gps_signal_lost
-						    && _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit
-						    && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)) {
-							// The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
-							// to improve its estimate if the previous reset was not successful.
-							if (resetYawToEKFGSF()) {
-								ECL_WARN("GPS emergency yaw reset");
-							}
-
-						} else {
-							// use GPS velocity data to check and correct yaw angle if a FW vehicle
-							if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
-								// if flying a fixed wing aircraft, do a complete reset that includes yaw
-								_mag_yaw_reset_req = true;
-							}
-
-							_warning_events.flags.gps_fusion_timout = true;
-							ECL_WARN("GPS fusion timeout - resetting");
+						if (resetYawToEKFGSF()) {
+							ECL_WARN("GPS emergency yaw reset");
+							do_vel_pos_reset = true;
 						}
+					}
+
+					if (do_vel_pos_reset) {
 
 						resetVelocityToGps(_gps_sample_delayed);
 						resetHorizontalPositionToGps(_gps_sample_delayed);
@@ -176,30 +166,20 @@ bool Ekf::shouldResetGpsFusion() const
 	/* We are relying on aiding to constrain drift so after a specified time
 	 * with no aiding we need to do something
 	 */
-	const bool is_reset_required = hasHorizontalAidingTimedOut()
+	const bool has_horizontal_aiding_timed_out = isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
+						     && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
+						     && isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
+
+	const bool is_reset_required = has_horizontal_aiding_timed_out
 				       || isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
 
-	/* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of
-	 * navigation casued by a bad yaw estimate.
-
-	 * A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity
-	 * innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff,
-	 * different test criteria are used that take longer to trigger and reduce false positives. A reset is
-	 * not performed if the fault condition was present before flight to prevent triggering due to GPS glitches
-	 * or other sensor errors.
-	 */
-	const bool is_recent_takeoff_nav_failure = _control_status.flags.in_air
-			&& isRecent(_time_last_on_ground_us, 30000000)
-			&& isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
-			&& (_time_last_hor_vel_fuse > _time_last_on_ground_us);
-
 	const bool is_inflight_nav_failure = _control_status.flags.in_air
 					     && isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
 					     && isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
 					     && (_time_last_hor_vel_fuse > _time_last_on_ground_us)
 					     && (_time_last_hor_pos_fuse > _time_last_on_ground_us);
 
-	return (is_reset_required || is_recent_takeoff_nav_failure || is_inflight_nav_failure);
+	return (is_reset_required || is_inflight_nav_failure);
 }
 
 bool Ekf::isYawFailure() const
diff --git a/src/modules/ekf2/EKF/zero_velocity_update.cpp b/src/modules/ekf2/EKF/zero_velocity_update.cpp
index 3a750292e8..fc5dc145d4 100644
--- a/src/modules/ekf2/EKF/zero_velocity_update.cpp
+++ b/src/modules/ekf2/EKF/zero_velocity_update.cpp
@@ -45,13 +45,16 @@ void Ekf::controlZeroVelocityUpdate()
 
 	if (zero_velocity_update_data_ready) {
 		const bool continuing_conditions_passing = _control_status.flags.vehicle_at_rest
-				&& _control_status_prev.flags.vehicle_at_rest;
+				&& _control_status_prev.flags.vehicle_at_rest
+				&& !isVerticalVelocityAidingActive(); // otherwise the filter is "too rigid" to follow a position drift
 
 		if (continuing_conditions_passing) {
 			Vector3f vel_obs{0, 0, 0};
 			Vector3f innovation = _state.vel - vel_obs;
 
-			const float obs_var = sq(0.001f);
+			// Set a low variance initially for faster leveling and higher
+			// later to let the states follow the measurements
+			const float obs_var = _control_status.flags.tilt_align ? sq(0.2f) : sq(0.001f);
 			Vector3f innov_var{
 				P(4, 4) + obs_var,
 				P(5, 5) + obs_var,
diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp
index 0e43e218a4..05798f375c 100644
--- a/src/modules/ekf2/EKF2.cpp
+++ b/src/modules/ekf2/EKF2.cpp
@@ -72,7 +72,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
 	_param_ekf2_acc_b_noise(_params->accel_bias_p_noise),
 	_param_ekf2_mag_e_noise(_params->mage_p_noise),
 	_param_ekf2_mag_b_noise(_params->magb_p_noise),
-	_param_ekf2_wind_noise(_params->wind_vel_p_noise),
+	_param_ekf2_wind_nsd(_params->wind_vel_nsd),
 	_param_ekf2_terr_noise(_params->terrain_p_noise),
 	_param_ekf2_terr_grad(_params->terrain_gradient),
 	_param_ekf2_gps_v_noise(_params->gps_vel_noise),
@@ -556,6 +556,8 @@ void EKF2::Run()
 					_gyro_cal = {};
 					_mag_cal = {};
 
+					_preflt_checker.reset();
+
 				} else if (was_in_air && !in_air) {
 					// landed
 					_ekf.set_in_air_status(false);
@@ -870,7 +872,10 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 	_estimator_innovations_pub.publish(innovations);
 
 	// calculate noise filtered velocity innovations which are used for pre-flight checking
-	if (!_ekf.control_status_flags().in_air) {
+	vehicle_status_s vehicle_status;
+	_status_sub.copy(&vehicle_status);
+
+	if (vehicle_status.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
 		// TODO: move to run before publications
 		_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
 		_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
@@ -883,10 +888,6 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
 		_preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt);
 
 		_preflt_checker.update(_ekf.get_imu_sample_delayed().delta_ang_dt, innovations);
-
-	} else if (_ekf.control_status_flags().in_air != _ekf.control_status_prev_flags().in_air) {
-		// reset preflight checks if transitioning back to landed
-		_preflt_checker.reset();
 	}
 }
 
@@ -1014,6 +1015,9 @@ void EKF2::PublishLocalPosition(const hrt_abstime &timestamp)
 	_ekf.get_posNE_reset(&lpos.delta_xy[0], &lpos.xy_reset_counter);
 	_ekf.get_velNE_reset(&lpos.delta_vxy[0], &lpos.vxy_reset_counter);
 
+	lpos.dead_reckoning = _ekf.control_status_flags().inertial_dead_reckoning
+			      || _ekf.control_status_flags().wind_dead_reckoning;
+
 	// get control limit information
 	_ekf.get_ekf_ctrl_limits(&lpos.vxy_max, &lpos.vz_max, &lpos.hagl_min, &lpos.hagl_max);
 
@@ -1485,6 +1489,16 @@ void EKF2::UpdateAirspeedSample(ekf2_timestamps_s &ekf2_timestamps)
 					.eas2tas = airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s,
 				};
 				_ekf.setAirspeedData(airspeed_sample);
+
+			} else if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_trim_m_s)
+				   && PX4_ISFINITE(airspeed_validated.true_airspeed_trim_m_s)
+				   && airspeed_validated.calibrated_airspeed_trim_m_s > 0.f) {
+				airspeedSample airspeed_sample {
+					.time_us = airspeed_validated.timestamp,
+					.true_airspeed = airspeed_validated.true_airspeed_trim_m_s,
+					.eas2tas = airspeed_validated.true_airspeed_trim_m_s / airspeed_validated.calibrated_airspeed_trim_m_s,
+				};
+				_ekf.setAirspeedData(airspeed_sample);
 			}
 
 			_airspeed_validated_timestamp_last = airspeed_validated.timestamp;
@@ -2070,17 +2084,29 @@ int EKF2::task_spawn(int argc, char *argv[])
 		}
 
 		int32_t sens_mag_mode = 1;
-		param_get(param_find("SENS_MAG_MODE"), &sens_mag_mode);
+		const param_t param_sens_mag_mode = param_find("SENS_MAG_MODE");
+		param_get(param_sens_mag_mode, &sens_mag_mode);
 
 		if (sens_mag_mode == 0) {
-			param_get(param_find("EKF2_MULTI_MAG"), &mag_instances);
+			const param_t param_ekf2_mult_mag = param_find("EKF2_MULTI_MAG");
+			param_get(param_ekf2_mult_mag, &mag_instances);
 
 			// Mags (1 - 4 supported)
-			if (mag_instances < 1 || mag_instances > 4) {
+			if (mag_instances > 4) {
 				const int32_t mag_instances_limited = math::constrain(mag_instances, static_cast<int32_t>(1), static_cast<int32_t>(4));
 				PX4_WARN("EKF2_MULTI_MAG limited %" PRId32 " -> %" PRId32, mag_instances, mag_instances_limited);
-				param_set_no_notification(param_find("EKF2_MULTI_MAG"), &mag_instances_limited);
+				param_set_no_notification(param_ekf2_mult_mag, &mag_instances_limited);
 				mag_instances = mag_instances_limited;
+
+			} else if (mag_instances <= 1) {
+				// properly disable multi-magnetometer at sensors hub level
+				PX4_WARN("EKF2_MULTI_MAG disabled, resetting SENS_MAG_MODE");
+
+				// re-enable at sensors level
+				sens_mag_mode = 1;
+				param_set(param_sens_mag_mode, &sens_mag_mode);
+
+				mag_instances = 1;
 			}
 
 		} else {
diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp
index 714f3a1ff7..0f5e97481b 100644
--- a/src/modules/ekf2/EKF2.hpp
+++ b/src/modules/ekf2/EKF2.hpp
@@ -337,7 +337,6 @@ private:
 	uORB::PublicationMulti<vehicle_odometry_s>           _odometry_pub;
 	uORB::PublicationMulti<wind_s>              _wind_pub;
 
-
 	PreFlightChecker _preflt_checker;
 
 	Ekf _ekf;
@@ -377,8 +376,8 @@ private:
 		_param_ekf2_mag_e_noise,	///< process noise for earth magnetic field prediction (Gauss/sec)
 		(ParamExtFloat<px4::params::EKF2_MAG_B_NOISE>)
 		_param_ekf2_mag_b_noise,	///< process noise for body magnetic field prediction (Gauss/sec)
-		(ParamExtFloat<px4::params::EKF2_WIND_NOISE>)
-		_param_ekf2_wind_noise,	///< process noise for wind velocity prediction (m/sec**2)
+		(ParamExtFloat<px4::params::EKF2_WIND_NSD>)
+		_param_ekf2_wind_nsd,	///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
 		(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise,	///< process noise for terrain offset (m/sec)
 		(ParamExtFloat<px4::params::EKF2_TERR_GRAD>)
 		_param_ekf2_terr_grad,	///< gradient of terrain used to estimate process noise due to changing position (m/m)
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index 125c61399e..65340b0a3f 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -177,7 +177,7 @@ PARAM_DEFINE_FLOAT(EKF2_AVEL_DELAY, 5);
  * @bit 7 Max horizontal speed (EKF2_REQ_HDRIFT)
  * @bit 8 Max vertical velocity discrepancy (EKF2_REQ_VDRIFT)
  */
-PARAM_DEFINE_INT32(EKF2_GPS_CHECK, 245);
+PARAM_DEFINE_INT32(EKF2_GPS_CHECK, 255);
 
 /**
  * Required EPH to use GPS.
@@ -320,15 +320,17 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 1.0e-4f);
 PARAM_DEFINE_FLOAT(EKF2_MAG_E_NOISE, 1.0e-3f);
 
 /**
- * Process noise for wind velocity prediction.
+ * Process noise spectral density for wind velocity prediction.
+ *
+ * When unaided, the wind estimate uncertainty (1-sigma, in m/s) increases by this amount every second.
  *
  * @group EKF2
  * @min 0.0
  * @max 1.0
- * @unit m/s^2
+ * @unit m/s^2/sqrt(Hz)
  * @decimal 3
  */
-PARAM_DEFINE_FLOAT(EKF2_WIND_NOISE, 1.0e-1f);
+PARAM_DEFINE_FLOAT(EKF2_WIND_NSD, 1.0e-2f);
 
 /**
  * Measurement noise for gps horizontal velocity.
@@ -1047,6 +1049,8 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_PITCH, 0.0f);
 /**
  * Range sensor aid.
  *
+ * WARNING: Deprecated, use MPC_ALT_MODE instead. This feature will be removed in the next release
+ *
  * If this parameter is enabled then the estimator will make use of the range finder measurements
  * to estimate it's height even if range sensor is not the primary height source. It will only do so if conditions
  * for range measurement fusion are met. This enables the range finder to be used during low speed and low altitude
@@ -1062,7 +1066,7 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_PITCH, 0.0f);
  * @value 0 Range aid disabled
  * @value 1 Range aid enabled
  */
-PARAM_DEFINE_INT32(EKF2_RNG_AID, 1);
+PARAM_DEFINE_INT32(EKF2_RNG_AID, 0);
 
 /**
  * Maximum horizontal velocity allowed for range aid mode.
diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp
index 436b52b3da..ed44cad372 100644
--- a/src/modules/ekf2/test/test_EKF_gps.cpp
+++ b/src/modules/ekf2/test/test_EKF_gps.cpp
@@ -172,3 +172,27 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback)
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsHeightFusion());
 	EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
 }
+
+TEST_F(EkfGpsTest, altitudeDrift)
+{
+	// GIVEN: a drifting GNSS altitude
+	const float dt = 0.2f;
+	const float height_rate = 0.15f;
+	const float duration = 80.f;
+
+	// WHEN: running on ground
+	for (int i = 0; i < (duration / dt); i++) {
+		_sensor_simulator._gps.stepHeightByMeters(height_rate * dt);
+		_sensor_simulator.runSeconds(dt);
+	}
+
+	float baro_innov;
+	_ekf->getBaroHgtInnov(baro_innov);
+	BaroBiasEstimator::status status = _ekf->getBaroBiasEstimatorStatus();
+
+	printf("baro innov = %f\n", (double)baro_innov);
+	printf("bias: %f, innov bias = %f\n", (double)status.bias, (double)status.innov);
+
+	// THEN: the baro and local position should follow it
+	EXPECT_LT(fabsf(baro_innov), 0.1f);
+}
diff --git a/src/modules/esc_battery/EscBattery.cpp b/src/modules/esc_battery/EscBattery.cpp
index 798c17aa99..6a6a21f02a 100644
--- a/src/modules/esc_battery/EscBattery.cpp
+++ b/src/modules/esc_battery/EscBattery.cpp
@@ -33,6 +33,8 @@
 
 #include "EscBattery.hpp"
 
+#include <math.h>
+
 using namespace time_literals;
 
 EscBattery::EscBattery() :
@@ -82,22 +84,22 @@ EscBattery::Run()
 
 	if (_esc_status_sub.copy(&esc_status)) {
 
-		int online_bitmask = (1 << esc_status.esc_count) - 1;
-
-		if (online_bitmask != esc_status.esc_online_flags || esc_status.esc_count == 0 ||
-		    esc_status.esc_count > esc_status_s::CONNECTED_ESC_MAX) {
+		if (esc_status.esc_count == 0 || esc_status.esc_count > esc_status_s::CONNECTED_ESC_MAX) {
 			return;
 		}
 
+		const uint8_t online_esc_count = math::countSetBits(esc_status.esc_online_flags);
 		float average_voltage_v = 0.0f;
 		float total_current_a = 0.0f;
 
 		for (unsigned i = 0; i < esc_status.esc_count; ++i) {
-			average_voltage_v += esc_status.esc[i].esc_voltage;
-			total_current_a += esc_status.esc[i].esc_current;
+			if ((1 << i) & esc_status.esc_online_flags) {
+				average_voltage_v += esc_status.esc[i].esc_voltage;
+				total_current_a += esc_status.esc[i].esc_current;
+			}
 		}
 
-		average_voltage_v /= esc_status.esc_count;
+		average_voltage_v /= online_esc_count;
 
 		_battery.setConnected(true);
 		_battery.updateVoltage(average_voltage_v);
diff --git a/src/modules/flight_mode_manager/CMakeLists.txt b/src/modules/flight_mode_manager/CMakeLists.txt
index 1d8e82bf98..2a1762366b 100644
--- a/src/modules/flight_mode_manager/CMakeLists.txt
+++ b/src/modules/flight_mode_manager/CMakeLists.txt
@@ -53,7 +53,7 @@ endif()
 # add core flight tasks to list
 list(APPEND flight_tasks_all
 	Auto
-	AutoFollowMe
+	AutoFollowTarget
 	Descend
 	Failsafe
 	ManualAcceleration
diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp
index a2c33b0997..1459fe55eb 100644
--- a/src/modules/flight_mode_manager/FlightModeManager.cpp
+++ b/src/modules/flight_mode_manager/FlightModeManager.cpp
@@ -167,7 +167,6 @@ void FlightModeManager::start_flight_task()
 {
 	bool task_failure = false;
 	bool should_disable_task = true;
-	int prev_failure_count = _task_failure_count;
 
 	// Do not run any flight task for VTOLs in fixed-wing mode
 	if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
@@ -177,83 +176,34 @@ void FlightModeManager::start_flight_task()
 
 	// Only run transition flight task if altitude control is enabled (e.g. in Altitdue, Position, Auto flight mode)
 	if (_vehicle_status_sub.get().in_transition_mode && _vehicle_control_mode_sub.get().flag_control_altitude_enabled) {
-
-		should_disable_task = false;
-		FlightTaskError error = switchTask(FlightTaskIndex::Transition);
-
-		if (error != FlightTaskError::NoError) {
-			if (prev_failure_count == 0) {
-				PX4_WARN("Transition activation failed with error: %s", errorToString(error));
-			}
-
-			task_failure = true;
-			_task_failure_count++;
-
-		} else {
-			// we want to be in this mode, reset the failure count
-			_task_failure_count = 0;
-		}
-
+		switchTask(FlightTaskIndex::Transition);
 		return;
 	}
 
 	// Auto-follow me
 	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
 		should_disable_task = false;
-		FlightTaskError error = switchTask(FlightTaskIndex::AutoFollowMe);
+		FlightTaskError error = switchTask(FlightTaskIndex::AutoFollowTarget);
 
 		if (error != FlightTaskError::NoError) {
-			if (prev_failure_count == 0) {
-				PX4_WARN("Follow-Me activation failed with error: %s", errorToString(error));
-			}
-
 			task_failure = true;
-			_task_failure_count++;
-
-		} else {
-			// we want to be in this mode, reset the failure count
-			_task_failure_count = 0;
 		}
 
 	} else if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
 
 		// Emergency descend
 		should_disable_task = false;
-		FlightTaskError error = FlightTaskError::NoError;
-
-		error = switchTask(FlightTaskIndex::Descend);
-
-		if (error != FlightTaskError::NoError) {
-			if (prev_failure_count == 0) {
-				PX4_WARN("Descend activation failed with error: %s", errorToString(error));
-			}
 
+		if (switchTask(FlightTaskIndex::Descend) != FlightTaskError::NoError) {
 			task_failure = true;
-			_task_failure_count++;
-
-		} else {
-			// we want to be in this mode, reset the failure count
-			_task_failure_count = 0;
 		}
 
 	} else if (_vehicle_control_mode_sub.get().flag_control_auto_enabled) {
 		// Auto related maneuvers
 		should_disable_task = false;
-		FlightTaskError error = FlightTaskError::NoError;
-
-		error = switchTask(FlightTaskIndex::Auto);
-
-		if (error != FlightTaskError::NoError) {
-			if (prev_failure_count == 0) {
-				PX4_WARN("Auto activation failed with error: %s", errorToString(error));
-			}
 
+		if (switchTask(FlightTaskIndex::Auto) != FlightTaskError::NoError) {
 			task_failure = true;
-			_task_failure_count++;
-
-		} else {
-			// we want to be in this mode, reset the failure count
-			_task_failure_count = 0;
 		}
 
 	}
@@ -284,18 +234,7 @@ void FlightModeManager::start_flight_task()
 			break;
 		}
 
-		if (error != FlightTaskError::NoError) {
-			if (prev_failure_count == 0) {
-				PX4_WARN("Position-Ctrl activation failed with error: %s", errorToString(error));
-			}
-
-			task_failure = true;
-			_task_failure_count++;
-
-		} else {
-			check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_POSCTL);
-			task_failure = false;
-		}
+		task_failure = error != FlightTaskError::NoError;
 	}
 
 	// manual altitude control
@@ -314,18 +253,7 @@ void FlightModeManager::start_flight_task()
 			break;
 		}
 
-		if (error != FlightTaskError::NoError) {
-			if (prev_failure_count == 0) {
-				PX4_WARN("Altitude-Ctrl activation failed with error: %s", errorToString(error));
-			}
-
-			task_failure = true;
-			_task_failure_count++;
-
-		} else {
-			check_failure(task_failure, vehicle_status_s::NAVIGATION_STATE_ALTCTL);
-			task_failure = false;
-		}
+		task_failure = error != FlightTaskError::NoError;
 	}
 
 	if (_vehicle_status_sub.get().nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) {
@@ -334,6 +262,15 @@ void FlightModeManager::start_flight_task()
 
 	// check task failure
 	if (task_failure) {
+		// If failsafe task is activated for a prolonged time, print an error, as this is unexpected
+		if (_failsafe_task_activated_start == 0) {
+			_failsafe_task_activated_start = hrt_absolute_time();
+		}
+
+		if (hrt_absolute_time() - _failsafe_task_activated_start > 5_s && !_failsafe_task_error_printed) {
+			PX4_ERR("Failsafe task activated");
+			_failsafe_task_error_printed = true;
+		}
 
 		// for some reason no flighttask was able to start.
 		// go into failsafe flighttask
@@ -344,63 +281,15 @@ void FlightModeManager::start_flight_task()
 			switchTask(FlightTaskIndex::None);
 		}
 
-	} else if (should_disable_task) {
-		switchTask(FlightTaskIndex::None);
+	} else {
+		_failsafe_task_activated_start = 0;
+		_failsafe_task_error_printed = false;
+
+		if (should_disable_task) {
+			switchTask(FlightTaskIndex::None);
+		}
 	}
 
-	_last_vehicle_nav_state = _vehicle_status_sub.get().nav_state;
-}
-
-void FlightModeManager::check_failure(bool task_failure, uint8_t nav_state)
-{
-	if (!task_failure) {
-		// we want to be in this mode, reset the failure count
-		_task_failure_count = 0;
-
-	} else if (_task_failure_count > NUM_FAILURE_TRIES) {
-		// tell commander to switch mode
-		PX4_WARN("Previous flight task failed, switching to mode %" PRIu8, nav_state);
-		send_vehicle_cmd_do(nav_state);
-		_task_failure_count = 0; // avoid immediate resending of a vehicle command in the next iteration
-	}
-}
-
-void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state)
-{
-	vehicle_command_s command{};
-	command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
-	command.param1 = (float)1; // base mode
-	command.param3 = (float)0; // sub mode
-	command.target_system = 1;
-	command.target_component = 1;
-	command.source_system = 1;
-	command.source_component = 1;
-	command.confirmation = false;
-	command.from_external = false;
-
-	// set the main mode
-	switch (nav_state) {
-	case vehicle_status_s::NAVIGATION_STATE_STAB:
-		command.param2 = (float)PX4_CUSTOM_MAIN_MODE_STABILIZED;
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
-		command.param2 = (float)PX4_CUSTOM_MAIN_MODE_ALTCTL;
-		break;
-
-	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
-		command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO;
-		command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-		break;
-
-	default: //vehicle_status_s::NAVIGATION_STATE_POSCTL
-		command.param2 = (float)PX4_CUSTOM_MAIN_MODE_POSCTL;
-		break;
-	}
-
-	// publish the vehicle command
-	command.timestamp = hrt_absolute_time();
-	_vehicle_command_pub.publish(command);
 }
 
 void FlightModeManager::handleCommand()
@@ -417,15 +306,15 @@ void FlightModeManager::handleCommand()
 		    && _vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 			// switch to the commanded task
 			bool switch_succeeded = (switchTask(desired_task) == FlightTaskError::NoError);
-			uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
+			uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
 
 			// if we are in/switched to the desired task
 			if (switch_succeeded) {
-				cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+				cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 				// if the task is running apply parameters to it and see if it rejects
 				if (isAnyTaskActive() && !_current_task.task->applyCommandParameters(command)) {
-					cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+					cmd_result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 
 					// if we just switched and parameters are not accepted, go to failsafe
 					if (switch_succeeded) {
@@ -460,7 +349,7 @@ void FlightModeManager::generateTrajectorySetpoint(const float dt,
 	_current_task.task->setYawHandler(_wv_controller);
 
 	// If the task fails sned out empty NAN setpoints and the controller will emergency failsafe
-	trajectory_setpoint_s setpoint = FlightTask::empty_setpoint;
+	trajectory_setpoint_s setpoint = FlightTask::empty_trajectory_setpoint;
 	vehicle_constraints_s constraints = FlightTask::empty_constraints;
 
 	if (_current_task.task->updateInitialize() && _current_task.task->update()) {
@@ -532,7 +421,7 @@ FlightTaskError FlightModeManager::switchTask(FlightTaskIndex new_task_index)
 	}
 
 	// Save current setpoints for the next FlightTask
-	trajectory_setpoint_s last_setpoint = FlightTask::empty_setpoint;
+	trajectory_setpoint_s last_setpoint = FlightTask::empty_trajectory_setpoint;
 	ekf_reset_counters_s last_reset_counters{};
 
 	if (isAnyTaskActive()) {
diff --git a/src/modules/flight_mode_manager/FlightModeManager.hpp b/src/modules/flight_mode_manager/FlightModeManager.hpp
index 9d8a6cb835..b733aa2986 100644
--- a/src/modules/flight_mode_manager/FlightModeManager.hpp
+++ b/src/modules/flight_mode_manager/FlightModeManager.hpp
@@ -89,8 +89,6 @@ private:
 	void Run() override;
 	void updateParams() override;
 	void start_flight_task();
-	void check_failure(bool task_failure, uint8_t nav_state);
-	void send_vehicle_cmd_do(uint8_t nav_state);
 	void handleCommand();
 	void generateTrajectorySetpoint(const float dt, const vehicle_local_position_s &vehicle_local_position);
 	void limitAltitude(trajectory_setpoint_s &setpoint, const vehicle_local_position_s &vehicle_local_position);
@@ -118,8 +116,6 @@ private:
 	int _initTask(FlightTaskIndex task_index);
 	FlightTaskIndex switchVehicleCommand(const int command);
 
-	static constexpr int NUM_FAILURE_TRIES = 10; ///< number of tries before switching to a failsafe flight task
-
 	/**
 	 * Union with all existing tasks: we use it to make sure that only the memory of the largest existing
 	 * task is needed, and to avoid using dynamic memory allocations.
@@ -134,8 +130,9 @@ private:
 	WeatherVane *_wv_controller{nullptr};
 	int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
 	uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_UNINITIALIZED};
-	int _task_failure_count{0};
-	uint8_t _last_vehicle_nav_state{0};
+
+	hrt_abstime _failsafe_task_activated_start{0};
+	bool _failsafe_task_error_printed{false};
 
 	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; ///< loop duration performance counter
 	hrt_abstime _time_stamp_last_loop{0}; ///< time stamp of last loop iteration
@@ -154,7 +151,6 @@ private:
 
 	uORB::Publication<landing_gear_s> _landing_gear_pub{ORB_ID(landing_gear)};
 	uORB::Publication<trajectory_setpoint_s> _trajectory_setpoint_pub{ORB_ID(trajectory_setpoint)};
-	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
 	uORB::Publication<vehicle_constraints_s> _vehicle_constraints_pub{ORB_ID(vehicle_constraints)};
 
diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
index 4952d61e14..f6ec9e80f9 100644
--- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp
@@ -154,11 +154,8 @@ bool FlightTaskAuto::update()
 		_velocity_setpoint(2) = NAN;
 		break;
 
-	case WaypointType::takeoff:
-		// Takeoff is completely defined by target position
-		_gear.landing_gear = landing_gear_s::GEAR_DOWN;
-
 	// FALLTHROUGH
+	case WaypointType::takeoff:
 	case WaypointType::loiter:
 	case WaypointType::position:
 	default:
@@ -234,9 +231,9 @@ void FlightTaskAuto::_prepareLandSetpoints()
 	_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
 
 	// Slow down automatic descend close to ground
-	float vertical_speed = math::gradual(_dist_to_ground,
-					     _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
-					     _param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get());
+	float vertical_speed = math::interpolate(_dist_to_ground,
+			       _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+			       _param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get());
 
 	bool range_dist_available = PX4_ISFINITE(_dist_to_bottom);
 
@@ -355,12 +352,11 @@ bool FlightTaskAuto::_evaluateTriplets()
 	const float cruise_speed_from_triplet = _sub_triplet_setpoint.get().current.cruising_speed;
 
 	if (PX4_ISFINITE(cruise_speed_from_triplet)
-	    && (cruise_speed_from_triplet > 0.f)
 	    && (_sub_triplet_setpoint.get().current.timestamp > _time_last_cruise_speed_override)) {
 		_mc_cruise_speed = cruise_speed_from_triplet;
 	}
 
-	if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < 0.0f)) {
+	if (!PX4_ISFINITE(_mc_cruise_speed) || (_mc_cruise_speed < FLT_EPSILON)) {
 		// If no speed is planned use the default cruise speed as limit
 		_mc_cruise_speed = _param_mpc_xy_cruise.get();
 	}
@@ -511,7 +507,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 			   && _sub_triplet_setpoint.get().current.yaw_valid) {
 			// Use the yaw computed in Navigator except during takeoff because
 			// Navigator is not handling the yaw reset properly.
-			// But: use if from Navigator during takeoff if disable_weather_vane is true,
+			// But: use yaw from Navigator during takeoff if disable_weather_vane is true,
 			// because we're then aligning to the transition waypoint.
 			// TODO: fix in navigator
 			_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt
new file mode 100644
index 0000000000..41085c5272
--- /dev/null
+++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/CMakeLists.txt
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+add_subdirectory(follow_target_estimator)
+
+px4_add_library(FlightTaskAutoFollowTarget
+	FlightTaskAutoFollowTarget.cpp
+)
+
+target_link_libraries(FlightTaskAutoFollowTarget PUBLIC FlightTaskAuto follow_target_estimator)
+target_include_directories(FlightTaskAutoFollowTarget PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
new file mode 100644
index 0000000000..eca0843c93
--- /dev/null
+++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.cpp
@@ -0,0 +1,312 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskAutoFollowTarget.cpp
+ *
+ * Flight Task for follow-me flight mode. It consumes follow_target_estimator messages from
+ * TargetEstimator. The drone then tracks this target's GPS coordinates from a specified
+ * angle and distance.
+ *
+ */
+
+#include "FlightTaskAutoFollowTarget.hpp"
+#include <mathlib/mathlib.h>
+
+using matrix::Vector2f;
+using matrix::Vector3f;
+
+FlightTaskAutoFollowTarget::FlightTaskAutoFollowTarget()
+{
+	_target_estimator.Start();
+}
+
+FlightTaskAutoFollowTarget::~FlightTaskAutoFollowTarget()
+{
+	_target_estimator.Stop();
+}
+
+bool FlightTaskAutoFollowTarget::activate(const trajectory_setpoint_s &last_setpoint)
+{
+	bool ret = FlightTask::activate(last_setpoint);
+
+	_position_setpoint = _position;
+
+	if (!PX4_ISFINITE(_position_setpoint(0)) || !PX4_ISFINITE(_position_setpoint(1))
+	    || !PX4_ISFINITE(_position_setpoint(2))) {
+		_position_setpoint = _position;
+	}
+
+	_target_position_filtered.reset(Vector3f{NAN, NAN, NAN});
+	_offset_vector_filtered.reset(Vector2f(0, 0));
+	_follow_angle_filtered.reset(0.0f);
+	_velocity_ff_scale.reset(0.0f);
+
+	// Initialize to something such that the drone at least points at the target, even if it's the wrong angle for the perspective.
+	// The drone will move into position as soon as the target starts moving and its heading becomes known.
+	Vector2f current_drone_heading_2d{cosf(_yaw), -sinf(_yaw)};
+
+	if (PX4_ISFINITE(current_drone_heading_2d(0)) && PX4_ISFINITE(current_drone_heading_2d(1))) {
+		_offset_vector_filtered.reset(current_drone_heading_2d);
+
+	} else {
+		_offset_vector_filtered.reset(Vector2f(1.0f, 0.0f));
+	}
+
+	_yawspeed_setpoint = 0.f;
+
+	return ret;
+}
+
+bool FlightTaskAutoFollowTarget::update()
+{
+	// New position setpoint
+	Vector3f drone_desired_position{NAN, NAN, NAN};
+
+	_follow_target_estimator_sub.update(&_follow_target_estimator);
+
+	if (_follow_target_estimator.timestamp > 0 && _follow_target_estimator.valid) {
+		const Vector3f pos_ned_est{_follow_target_estimator.pos_est};
+		const Vector3f vel_ned_est{_follow_target_estimator.vel_est};
+		const Vector3f acc_ned_est{_follow_target_estimator.acc_est};
+
+		// Reset the smoothness filter once the target position estimate is available
+		if (!PX4_ISFINITE(_target_position_filtered.getState()(0)) || !PX4_ISFINITE(_target_position_filtered.getState()(1))
+		    || !PX4_ISFINITE(_target_position_filtered.getState()(2))) {
+			_target_position_filtered.reset(pos_ned_est);
+		}
+
+		// Low-pass filter on target position. Then use a predicted target's position to compensate the filter delay to some extent.
+		{
+			const Vector3f target_predicted_position = predict_future_pos_ned_est(POSITION_FILTER_ALPHA, pos_ned_est, vel_ned_est,
+					acc_ned_est);
+			_target_position_filtered.setParameters(_deltatime, POSITION_FILTER_ALPHA);
+			_target_position_filtered.update(target_predicted_position);
+		}
+
+		if (_param_nav_ft_fs.get() == FOLLOW_PERSPECTIVE_NONE) {
+			// NOTE: Switching between NONE any any other setting currently causes a jump in the setpoints
+			_offset_vector_filtered.reset(Vector2f{0, 0});
+
+		} else {
+			// Define and rotate offset vector based on follow-me perspective setting
+			const float new_follow_angle_rad = math::radians(update_follow_me_angle_setting(_param_nav_ft_fs.get()));
+
+			// Use shortest rotation to get to the new angle
+			// Example: if the current angle setting is 270, and the new angle setting is 0, it's
+			// faster to rotate to 360 rather than 0.
+			// Usually the controller would automatically take the shortest path. But here some trickery
+			// is necessary because the yaw angle is run through a low-pass filter.
+			if (_follow_angle_rad - new_follow_angle_rad  > M_PI_F) {
+				_follow_angle_rad = new_follow_angle_rad + M_TWOPI_F;
+
+			} else if (_follow_angle_rad - new_follow_angle_rad  < -M_PI_F) {
+				_follow_angle_rad = new_follow_angle_rad - M_TWOPI_F;
+
+			} else {
+				_follow_angle_rad = new_follow_angle_rad;
+			}
+
+			// Lowpass the angle setting to smoothly transition to a new perspective when the user makes a change.
+			// In particular this has an effect when the setting is modified by 180 degrees, in which case the drone
+			// would pass above the target without the filter. The filtering makes it so that the drone flies around
+			// the target into the new postion.
+			_follow_angle_filtered.setParameters(_deltatime, FOLLOW_ANGLE_FILTER_ALPHA);
+			_follow_angle_filtered.update(_follow_angle_rad);
+
+			// Wrap around 360 degrees
+			if (_follow_angle_filtered.getState() > M_TWOPI_F) {
+				_follow_angle_filtered.reset(_follow_angle_filtered.getState() - M_TWOPI_F);
+				_follow_angle_rad = _follow_angle_rad - M_TWOPI_F;
+
+			} else if (_follow_angle_filtered.getState() < -M_TWOPI_F) {
+				_follow_angle_filtered.reset(_follow_angle_filtered.getState() + M_TWOPI_F);
+				_follow_angle_rad = _follow_angle_rad + M_TWOPI_F;
+			}
+
+			// Assume the target's velocity vector is its heading and use it to construct the offset vector
+			// such that drone_pos_setpoint = target_pose + offset_vector
+			if (vel_ned_est.longerThan(MINIMUM_SPEED_FOR_HEADING_CHANGE) &&
+			    vel_ned_est.longerThan(FLT_EPSILON)) {
+				// Compute offset vector relative to target position. At the same time the offset vector defines the
+				// vieweing angle of the drone
+				_target_velocity_unit_vector = Vector2f(vel_ned_est.xy()).unit_or_zero();
+			}
+
+			float offset_x = (float)cos(_follow_angle_filtered.getState()) * _target_velocity_unit_vector(0) - (float)sin(
+						 _follow_angle_filtered.getState()) * _target_velocity_unit_vector(1);
+			float offset_y = (float)sin(_follow_angle_filtered.getState()) * _target_velocity_unit_vector(0) + (float)cos(
+						 _follow_angle_filtered.getState()) * _target_velocity_unit_vector(1);
+
+			// Lowpass on the offset vector to have smooth transitions when the target turns, or when the
+			// setting for the perspective is changed by the user. This introduces only a delay in the
+			// tracking / viewing angle without disadvantages
+			_offset_vector_filtered.setParameters(_deltatime, DIRECTION_FILTER_ALPHA);
+			_offset_vector_filtered.update(Vector2f{offset_x, offset_y});
+		}
+
+		// Correct the desired distance by the target scale determined from object detection
+		const float desired_distance_to_target = _param_nav_ft_dst.get();
+		drone_desired_position.xy() = Vector2f(_target_position_filtered.getState().xy()) +
+					      _offset_vector_filtered.getState().unit_or_zero() * desired_distance_to_target;
+
+		// Z-position based off curent and initial target altitude
+		// TODO: Parameter NAV_MIN_FT_HT has been repurposed to be used as the desired
+		// altitude above the target. Clarify best solution.
+		switch (_param_nav_ft_alt_m.get()) {
+		case FOLLOW_ALTITUDE_MODE_TRACK_TARGET:
+			drone_desired_position(2) = _target_position_filtered.getState()(2) - _param_nav_min_ft_ht.get();
+			break;
+
+		case FOLLOW_ALTITUDE_MODE_CONSTANT:
+
+		// FALLTHROUGH
+
+		default:
+			// use the current position setpoint, unless it's closer to the ground than the minimum
+			// altitude setting
+			drone_desired_position(2) = math::min(_position_setpoint(2), -_param_nav_min_ft_ht.get());
+		}
+
+		// Set position and velocity setpoints
+		float desired_velocity_ff_scale = 0.0f;  // Used to ramp up velocity feedforward, avoiding harsh jumps in the setpoints
+
+		if (PX4_ISFINITE(drone_desired_position(0)) && PX4_ISFINITE(drone_desired_position(1))
+		    && PX4_ISFINITE(drone_desired_position(2))) {
+			// Only control horizontally if drone is on target altitude to avoid accidents
+			if (fabsf(drone_desired_position(2) - _position(2)) < ALT_ACCEPTANCE_THRESHOLD) {
+
+				// Don't need feed-forward velocity while the target is static
+				if (vel_ned_est.longerThan(MINIMUM_SPEED_FOR_HEADING_CHANGE)) {
+					desired_velocity_ff_scale = 1.0f;
+
+				}
+
+				// Velocity setpoints is a feedforward term derived from position setpoints
+				_velocity_setpoint = (drone_desired_position - _position_setpoint) / _deltatime * _velocity_ff_scale.getState();
+				_position_setpoint = drone_desired_position;
+
+			} else {
+				// Achieve target altitude first before controlling horizontally!
+				_position_setpoint = _position;
+				_position_setpoint(2) = drone_desired_position(2);
+			}
+
+		} else {
+			// Control setpoint: Stay in current position
+			_position_setpoint = _position;
+			_velocity_setpoint.setZero();
+		}
+
+		_velocity_ff_scale.setParameters(_deltatime, VELOCITY_FF_FILTER_ALPHA);
+		_velocity_ff_scale.update(desired_velocity_ff_scale);
+
+		// Emergency ascent when too close to the ground
+		if (PX4_ISFINITE(_dist_to_bottom) && _dist_to_bottom < MINIMUM_SAFETY_ALTITUDE) {
+			_position_setpoint(0) = _position_setpoint(1) = NAN;
+			_position_setpoint(2) = _position(2);
+			_velocity_setpoint(0) = _velocity_setpoint(1) = 0.0f;
+			_velocity_setpoint(2) = -EMERGENCY_ASCENT_SPEED; // Slowly ascend
+		}
+
+		// Yaw setpoint: Face the target
+		const Vector2f target_to_drone = Vector2f(_position.xy()) - Vector2f(
+				_target_position_filtered.getState().xy());
+
+		if (target_to_drone.longerThan(MINIMUM_DISTANCE_TO_TARGET_FOR_YAW_CONTROL)) {
+			_yaw_setpoint = atan2f(-target_to_drone(1), -target_to_drone(0));
+		}
+
+	} else {
+		// Control setpoint: Stay in current position
+		_position_setpoint(0) = _position_setpoint(1) = NAN;
+		_velocity_setpoint.xy() = 0;
+	}
+
+	// Status message
+	follow_target_status_s follow_target_status{};
+	_target_position_filtered.getState().copyTo(follow_target_status.pos_est_filtered);
+
+	// Publish status message for debugging
+	follow_target_status.timestamp = hrt_absolute_time();
+	_follow_target_status_pub.publish(follow_target_status);
+
+	_constraints.want_takeoff = _checkTakeoff();
+
+	return true;
+}
+
+float FlightTaskAutoFollowTarget::update_follow_me_angle_setting(int param_nav_ft_fs) const
+{
+	switch (param_nav_ft_fs) {
+	case FOLLOW_PERSPECTIVE_BEHIND:
+		return FOLLOW_PERSPECTIVE_BEHIND_ANGLE_DEG;
+
+	case FOLLOW_PERSPECTIVE_FRONT:
+		return FOLLOW_PERSPECTIVE_FRONT_ANGLE_DEG;
+
+	case FOLLOW_PERSPECTIVE_FRONT_RIGHT:
+		return FOLLOW_PERSPECTIVE_FRONT_RIGHT_ANGLE_DEG;
+
+	case FOLLOW_PERSPECTIVE_FRONT_LEFT:
+		return FOLLOW_PERSPECTIVE_FRONT_LEFT_ANGLE_DEG;
+
+	case FOLLOW_PERSPECTIVE_MID_RIGHT:
+		return FOLLOW_PERSPECTIVE_MID_RIGHT_ANGLE_DEG;
+
+	case FOLLOW_PERSPECTIVE_MID_LEFT:
+		return FOLLOW_PERSPECTIVE_MID_LEFT_ANGLE_DEG;
+
+	case FOLLOW_PERSPECTIVE_BEHIND_RIGHT:
+		return FOLLOW_PERSPECTIVE_BEHIND_RIGHT_ANGLE_DEG;
+
+	case FOLLOW_PERSPECTIVE_BEHIND_LEFT:
+		return FOLLOW_PERSPECTIVE_BEHIND_LEFT_ANGLE_DEG;
+
+	case FOLLOW_PERSPECTIVE_MIDDLE_FOLLOW:
+		return FOLLOW_PERSPECTIVE_BEHIND_ANGLE_DEG;
+
+	default:
+		// No or invalid option
+		break;
+	}
+
+	// Default: follow from behind
+	return FOLLOW_PERSPECTIVE_BEHIND_ANGLE_DEG;
+}
+
+Vector3f FlightTaskAutoFollowTarget::predict_future_pos_ned_est(float deltatime, const Vector3f &pos_ned_est,
+		const Vector3f &vel_ned_est, const Vector3f &acc_ned_est) const
+{
+	return pos_ned_est + vel_ned_est * deltatime + 0.5f * acc_ned_est * deltatime * deltatime;
+}
diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp
new file mode 100644
index 0000000000..994b57fb72
--- /dev/null
+++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/FlightTaskAutoFollowTarget.hpp
@@ -0,0 +1,182 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightTaskAutoFollowTarget.hpp
+ *
+ * Flight task for autonomous, gps driven follow-me mode.
+ *
+ * @author Alessandro Simovic <potaito-dev@protonmail.com>
+ */
+
+#pragma once
+
+#include "FlightTaskAuto.hpp"
+#include "follow_target_estimator/TargetEstimator.hpp"
+
+#include <parameters/param.h>
+#include <mathlib/mathlib.h>
+#include <uORB/Subscription.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/follow_target_status.h>
+#include <uORB/topics/follow_target_estimator.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+
+// Speed above which the target heading can change.
+// Used to prevent unpredictable jitter at low speeds.
+static constexpr float MINIMUM_SPEED_FOR_HEADING_CHANGE = 0.1f;
+
+// Minimum distance between drone and target for the drone to do any yaw control.
+static constexpr float MINIMUM_DISTANCE_TO_TARGET_FOR_YAW_CONTROL = 1.0f;
+
+// Minimum safety altitude above home (or bottom distance sensor)
+// underneath which the flight task will stop moving horizontally
+static constexpr float MINIMUM_SAFETY_ALTITUDE = 1.0f;
+
+// [m] max vertical deviation from position setpoint, above
+// which no horizontal control is done
+static constexpr float ALT_ACCEPTANCE_THRESHOLD = 3.0f;
+
+// Vertical ascent speed when the drone detects that it
+// is too close to the ground (below MINIMUM_SAFETY_ALTITUDE)
+static constexpr float EMERGENCY_ASCENT_SPEED = 0.2f;
+
+// Filter on setpoints for smooth cinematic experience:
+// Lowpass applied to the estimated position of the target
+// before using it as control input
+static constexpr float POSITION_FILTER_ALPHA =	1.5f;
+
+// Filter on setpoints for smooth cinematic experience:
+// Lowpass applied to the follow-me angle setting, to ensure
+// smooth and circular transitions between settings
+static constexpr float FOLLOW_ANGLE_FILTER_ALPHA = 3.0f;
+
+// Filter on setpoints for smooth cinematic experience:
+// Lowpass applied to the actual NED direction how the drone is facing the target
+// regarless of the setting. Used for dynamic tracking angles when the target makes a turn
+static constexpr float DIRECTION_FILTER_ALPHA =	3.0f;
+
+// Filter on setpoints for smooth cinematic experience:
+// Lowpass applied for ramping up / down velocity feedforward term
+static constexpr float VELOCITY_FF_FILTER_ALPHA = 1.0f;
+
+
+class FlightTaskAutoFollowTarget : public FlightTask
+{
+public:
+	FlightTaskAutoFollowTarget();
+	virtual ~FlightTaskAutoFollowTarget();
+
+	bool activate(const trajectory_setpoint_s &last_setpoint) override;
+	bool update() override;
+
+protected:
+	enum {
+		FOLLOW_PERSPECTIVE_NONE,
+		FOLLOW_PERSPECTIVE_BEHIND,
+		FOLLOW_PERSPECTIVE_FRONT,
+		FOLLOW_PERSPECTIVE_FRONT_RIGHT,
+		FOLLOW_PERSPECTIVE_FRONT_LEFT,
+		FOLLOW_PERSPECTIVE_MID_RIGHT,
+		FOLLOW_PERSPECTIVE_MID_LEFT,
+		FOLLOW_PERSPECTIVE_BEHIND_RIGHT,
+		FOLLOW_PERSPECTIVE_BEHIND_LEFT,
+		FOLLOW_PERSPECTIVE_MIDDLE_FOLLOW,
+		FOLLOW_PERSPECTIVE_INVALID  // Leave this as last!
+	};
+
+	// Angles [deg] for the different follow-me perspectives
+	enum {
+		FOLLOW_PERSPECTIVE_BEHIND_ANGLE_DEG = 180,
+		FOLLOW_PERSPECTIVE_FRONT_ANGLE_DEG = 0,
+		FOLLOW_PERSPECTIVE_FRONT_RIGHT_ANGLE_DEG = 45,
+		FOLLOW_PERSPECTIVE_FRONT_LEFT_ANGLE_DEG = 315,
+		FOLLOW_PERSPECTIVE_MID_RIGHT_ANGLE_DEG = 90,
+		FOLLOW_PERSPECTIVE_MID_LEFT_ANGLE_DEG = 270,
+		FOLLOW_PERSPECTIVE_BEHIND_RIGHT_ANGLE_DEG = 135,
+		FOLLOW_PERSPECTIVE_BEHIND_LEFT_ANGLE_DEG = 225
+	};
+
+	enum {
+		FOLLOW_ALTITUDE_MODE_CONSTANT,
+		FOLLOW_ALTITUDE_MODE_TRACK_TARGET
+	};
+
+	/**
+	 * Get the current follow-me perspective setting from PX4 parameters
+	 *
+	 * @param param_nav_ft_fs value of the parameter NAV_FT_FS
+	 * @return Angle [deg] from which the drone should view the target while following it, with zero degrees indicating the target's 12 o'clock
+	 */
+	float update_follow_me_angle_setting(int param_nav_ft_fs) const;
+
+	/**
+	 * Predict target's position through forward integration of its currently estimated position, velocity and acceleration.
+	 *
+	 * @param deltatime [s] prediction horizon
+	 * @return Future prediction of target position
+	 */
+	matrix::Vector3f predict_future_pos_ned_est(float deltatime, const matrix::Vector3f &pos_ned_est,
+			const matrix::Vector3f &vel_ned_est,
+			const matrix::Vector3f &acc_ned_est) const;
+
+	TargetEstimator _target_estimator;
+
+	// Follow angle is defined with 0 degrees following from front, and then clockwise rotation
+	float _follow_angle_rad{0.0f};
+	AlphaFilter<float> _follow_angle_filtered;
+
+	// Estimator for target position and velocity
+	follow_target_estimator_s _follow_target_estimator;
+	matrix::Vector2f _target_velocity_unit_vector;
+
+	// Lowpass filters for smoothingtarget position because it's used for setpoint generation
+	AlphaFilter<matrix::Vector3f> _target_position_filtered;
+
+	// Lowpass filter for smoothing the offset vector and have more dynamic shots when target changes direction
+	AlphaFilter<matrix::Vector2f> _offset_vector_filtered;
+
+	// Lowpass filter assuming  values 0-1, for avoiding big steps in velocity feedforward
+	AlphaFilter<float> _velocity_ff_scale;
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::NAV_MIN_FT_HT>) _param_nav_min_ft_ht,
+		(ParamFloat<px4::params::NAV_FT_DST>) _param_nav_ft_dst,
+		(ParamInt<px4::params::NAV_FT_FS>) _param_nav_ft_fs,
+		(ParamInt<px4::params::NAV_FT_ALT_M>) _param_nav_ft_alt_m
+	)
+
+	uORB::Subscription _follow_target_estimator_sub{ORB_ID(follow_target_estimator)};
+
+	uORB::PublicationMulti<follow_target_status_s> _follow_target_status_pub{ORB_ID(follow_target_status)};
+};
diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowMe/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/CMakeLists.txt
similarity index 88%
rename from src/modules/flight_mode_manager/tasks/AutoFollowMe/CMakeLists.txt
rename to src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/CMakeLists.txt
index 2e757dc29f..5640a1c403 100644
--- a/src/modules/flight_mode_manager/tasks/AutoFollowMe/CMakeLists.txt
+++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/CMakeLists.txt
@@ -31,9 +31,14 @@
 #
 ############################################################################
 
-px4_add_library(FlightTaskAutoFollowMe
-	FlightTaskAutoFollowMe.cpp
+px4_add_library(follow_target_estimator
+	TargetEstimator.cpp
 )
 
-target_link_libraries(FlightTaskAutoFollowMe PUBLIC FlightTaskAuto)
-target_include_directories(FlightTaskAutoFollowMe PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_compile_options(follow_target_estimator PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
+
+target_link_libraries(follow_target_estimator
+	PRIVATE
+		mathlib
+		px4_work_queue
+)
diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp
new file mode 100644
index 0000000000..fa112b90bd
--- /dev/null
+++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp
@@ -0,0 +1,348 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file TargetEstimator.cpp
+ *
+ */
+
+#include "TargetEstimator.hpp"
+
+#include <mathlib/mathlib.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/log.h>
+
+using matrix::Vector2f;
+using matrix::Vector3f;
+using matrix::Vector3;
+
+TargetEstimator::TargetEstimator() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
+{
+
+}
+
+TargetEstimator::~TargetEstimator()
+{
+	Stop();
+}
+
+bool TargetEstimator::Start()
+{
+	// Initialize this filter
+	reset();
+
+	// force initialize parameters update
+	parameters_update(true);
+
+	// follow_target messages needed for estimation
+	if (!_follow_target_sub.registerCallback()) {
+		PX4_ERR("target_estimator callback registration failed");
+	}
+
+	if (!_vehicle_local_position_sub.registerCallback()) {
+		PX4_ERR("target_estimator callback registration failed");
+	}
+
+	return true;
+}
+
+void TargetEstimator::Stop()
+{
+	// clear all registered callbacks
+	_follow_target_sub.unregisterCallback();
+	_vehicle_local_position_sub.unregisterCallback();
+
+	Deinit();
+}
+
+void TargetEstimator::Run()
+{
+	ScheduleDelayed(10_ms);
+
+	update();
+}
+
+void TargetEstimator::parameters_update(bool force)
+{
+	// check for parameter updates
+	if (_parameter_update_sub.updated() || force) {
+		// clear update
+		parameter_update_s update;
+		_parameter_update_sub.copy(&update);
+
+		// update parameters from storage
+		updateParams();
+	}
+}
+
+void TargetEstimator::update()
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	// compute deltatime between update() calls
+	if (_last_iteration_timestamp == 0) {
+		_last_iteration_timestamp = now;
+		return;
+	}
+
+	const float deltatime = math::constrain((now - _last_iteration_timestamp) * 1e-6f, 1e-3f, 2.0f);
+	_last_iteration_timestamp = now;
+
+	parameters_update();
+	update_filter_gains(_filter_gains);
+
+
+	// Get GPS reference location for NED frame, needed for projection
+	_vehicle_local_position_sub.update(&_vehicle_local_position);
+
+	// Perform sensor fusion update if there's a new GPS message from the follow-target
+	prediction_update(deltatime);
+
+	follow_target_s follow_target;
+
+	if (_follow_target_sub.update(&follow_target)) {
+
+		// Don't perform measurement update if two follow_target messages with identical timestamps are used
+		// This can happen when using the MAVSDK and more than one outgoing follow_target message is queued.
+		const bool duplicate_measurement_received = follow_target.timestamp == _last_follow_target_timestamp;
+
+		// Skip measurements that lie in the past
+		const bool measurement_in_the_past = _last_position_fusion_timestamp >= follow_target.timestamp;
+
+		// Need at least one vehicle_local_position before estimator can work
+		const bool vehicle_local_position_invalid = _vehicle_local_position.timestamp == 0;
+
+		if (!duplicate_measurement_received && !measurement_in_the_past && !vehicle_local_position_invalid) {
+			measurement_update(follow_target);
+		}
+	}
+
+	// Keep position estimate as the last known position
+	// but stop moving the estimate
+	if (is_stale(GPS_MESSAGE_STALE_TIMEOUT_MS)) {
+		_filter_states.vel_ned_est.setZero();
+		_filter_states.acc_ned_est.setZero();
+	}
+
+	const bool states_are_finite = _filter_states.is_finite();
+
+	if (!states_are_finite) {
+		reset();
+	}
+
+	// Publish estimator message
+	follow_target_estimator_s follow_target_estimator{};
+	follow_target_estimator.timestamp = hrt_absolute_time();
+	follow_target_estimator.valid = states_are_finite;
+	follow_target_estimator.stale = is_stale(GPS_MESSAGE_STALE_TIMEOUT_MS);
+	follow_target_estimator.last_filter_reset_timestamp = _last_filter_reset_timestamp;
+	follow_target_estimator.lat_est = get_lat_lon_alt_est()(0);
+	follow_target_estimator.lon_est = get_lat_lon_alt_est()(1);
+	follow_target_estimator.alt_est = get_lat_lon_alt_est()(2);
+	_filter_states.pos_ned_est.copyTo(follow_target_estimator.pos_est);
+	_filter_states.vel_ned_est.copyTo(follow_target_estimator.vel_est);
+	_filter_states.acc_ned_est.copyTo(follow_target_estimator.acc_est);
+	_follow_target_estimator_pub.publish(follow_target_estimator);
+}
+
+void TargetEstimator::update_filter_gains(filter_gains_s &filter_gains) const
+{
+	const float responsiveness_param = math::constrain(_param_nav_ft_rs.get(), .1F, 1.0F);
+
+	if (fabsf(filter_gains.responsiveness - responsiveness_param) < FLT_EPSILON) {
+		// Parameter did not change since last execution. Skip calculations
+		return;
+	}
+
+	filter_gains.responsiveness = responsiveness_param;
+
+	// The "G" gain is equivalent to "(1-responsiveness)", but beta is required for H and K gains
+	// From alpha-beta-gamma filter equations: G = 1-beta^3
+	// Therefore: beta = (1-Gp)^(1/3) = (1-(1-responsiveness))^(1/3) = (r)^(1/3)
+	const float beta_p = std::pow((filter_gains.responsiveness), 1.0f / 3.0f);
+	const float beta_v = 0.9f * beta_p; // velocity fusion gain is slightly lower. TODO: individual parameter?
+
+	// Estimator gains for horizontal position update
+	filter_gains.G_p = 1.0f - beta_p * beta_p * beta_p;
+	filter_gains.H_p = 1.5f * (1.0f - beta_p) * (1.0f - beta_p) * (1.0f + beta_p);
+	filter_gains.K_p = 0.5f * (1.0f - beta_p) * (1.0f - beta_p) * (1.0f - beta_p);
+
+	// Estimator gains for velocity update
+	filter_gains.G_v = 1.0f - beta_v * beta_v ;
+	filter_gains.H_v = (1.0f - beta_v) * (1.0f - beta_v);
+}
+
+bool TargetEstimator::measurement_can_be_fused(const Vector3f &current_measurement,
+		const Vector3f &previous_measurement,
+		uint64_t last_fusion_timestamp, float min_delta_t) const
+{
+	const bool measurement_valid = PX4_ISFINITE(current_measurement(0)) && PX4_ISFINITE(current_measurement(1))
+				       && PX4_ISFINITE(current_measurement(2));
+
+	const bool sensor_data_changed = Vector3f(current_measurement - previous_measurement).longerThan(2.0f * FLT_EPSILON)
+					 || !PX4_ISFINITE(previous_measurement(0)) || !PX4_ISFINITE(previous_measurement(1))
+					 || !PX4_ISFINITE(previous_measurement(2));
+
+	const bool fusion_old_enough = hrt_absolute_time() - last_fusion_timestamp >
+				       min_delta_t * 1000;
+
+	return measurement_valid && fusion_old_enough && sensor_data_changed;
+}
+
+void TargetEstimator::measurement_update(follow_target_s follow_target)
+{
+	// Decompose follow_target message into the individual measurements for position and velocity
+	const Vector3f vel_measured{follow_target.vx, follow_target.vy, follow_target.vz};
+	Vector3f pos_measured{NAN, NAN, -(follow_target.alt - _vehicle_local_position.ref_alt)};
+	_reference_position.initReference(_vehicle_local_position.ref_lat, _vehicle_local_position.ref_lon);
+	_reference_position.project(follow_target.lat, follow_target.lon, pos_measured(0), pos_measured(1));
+
+	// Initialize filter if necessary
+	if (_last_follow_target_timestamp == 0) {
+		_filter_states.pos_ned_est = pos_measured;
+
+		if (PX4_ISFINITE(vel_measured(0)) && PX4_ISFINITE(vel_measured(1)) && PX4_ISFINITE(vel_measured(2))) {
+			_filter_states.vel_ned_est = vel_measured;
+
+		} else {
+			_filter_states.vel_ned_est.setAll(0.0f);
+		}
+
+		_pos_measurement_old = pos_measured;
+		_vel_measurement_old = vel_measured;
+		_filter_states.acc_ned_est.setZero();
+	}
+
+	_last_follow_target_timestamp = follow_target.timestamp;
+
+	// Fuse position measurement
+	//
+	// Filter duplicate GPS POS and VEL messages
+	// QGC sends the same GPS coordinates multiple times per second, even though the phone's GPS
+	// typically only updates at 1 Hz
+
+	if (measurement_can_be_fused(pos_measured, _pos_measurement_old, _last_position_fusion_timestamp,
+				     MINIMUM_TIME_BETWEEN_POS_FUSIONS_MS)) {
+		// Update with only position measurement
+
+		const float dt_update_pos = math::constrain((follow_target.timestamp - _last_position_fusion_timestamp) * 1e-6f, 1e-3f,
+					    20.0f);  // seconds
+		_last_position_fusion_timestamp = follow_target.timestamp;
+
+		const Vector3f pos_innovation = pos_measured - _filter_states.pos_ned_est;
+
+		// Position, velocity and acceleration update
+		_filter_states.pos_ned_est += _filter_gains.G_p * pos_innovation;
+		_filter_states.vel_ned_est += _filter_gains.H_p / (dt_update_pos) * pos_innovation;
+		_filter_states.acc_ned_est += 2.0f * _filter_gains.K_p /
+					      (dt_update_pos * dt_update_pos) * pos_innovation;
+
+		_pos_measurement_old = pos_measured;
+	}
+
+	// Fuse velocity measurement
+	//
+	// Use target's velocity data for update only if
+	// - the last velocity fusion is a while ago to prevent repeated measurements to cause a quick convergence
+	// - the target is considered to be moving. Otherwise it's enough to only update the position
+	// - the GPS velocity measurement from the target is not stale
+	// Additionally also wait with first velocity fusion until at least one position fusion has been done (states become finite)
+	if (measurement_can_be_fused(vel_measured, _vel_measurement_old, _last_velocity_fusion_timestamp,
+				     MINIMUM_TIME_BETWEEN_VEL_FUSIONS_MS)) {
+		// Update with only velocity measurement
+
+		const float dt_update_vel = math::constrain((follow_target.timestamp - _last_velocity_fusion_timestamp) * 1e-6f, 1e-3f,
+					    20.0f); // seconds
+		_last_velocity_fusion_timestamp = follow_target.timestamp;
+
+		const Vector3f vel_innovation = vel_measured - _filter_states.vel_ned_est;
+
+		// Velocity and acceleration update
+		_filter_states.vel_ned_est += _filter_gains.G_v * vel_innovation;
+		_filter_states.acc_ned_est += _filter_gains.H_v / (dt_update_vel) * vel_innovation;
+
+		_vel_measurement_old = vel_measured;
+	}
+
+	_filter_states.saturate_acceleration(ACCELERATION_SATURATION);
+
+}
+
+void TargetEstimator::prediction_update(float deltatime)
+{
+	// Temporary copy to not mix old and new values during the update calculations
+	const Vector3f vel_ned_est_prev = _filter_states.vel_ned_est;
+	const Vector3f acc_ned_est_prev = _filter_states.acc_ned_est;
+
+	if (PX4_ISFINITE(vel_ned_est_prev(0)) && PX4_ISFINITE(vel_ned_est_prev(1)) && PX4_ISFINITE(vel_ned_est_prev(2))) {
+		_filter_states.pos_ned_est += deltatime * vel_ned_est_prev + 0.5f * acc_ned_est_prev * deltatime * deltatime;
+	}
+
+	if (PX4_ISFINITE(acc_ned_est_prev(0)) && PX4_ISFINITE(acc_ned_est_prev(1)) && PX4_ISFINITE(acc_ned_est_prev(2))) {
+		_filter_states.vel_ned_est += deltatime * acc_ned_est_prev;
+	}
+}
+
+Vector3<double> TargetEstimator::get_lat_lon_alt_est() const
+{
+	Vector3<double> lat_lon_alt{(double)NAN, (double)NAN, (double)NAN};
+
+	if (PX4_ISFINITE(_filter_states.pos_ned_est(0)) && PX4_ISFINITE(_filter_states.pos_ned_est(0))) {
+		_reference_position.reproject(_filter_states.pos_ned_est(0), _filter_states.pos_ned_est(1),
+					      lat_lon_alt(0), lat_lon_alt(1));
+		lat_lon_alt(2) = -(double)_filter_states.pos_ned_est(2) + (double)_vehicle_local_position.ref_alt;
+	}
+
+	return lat_lon_alt;
+}
+
+bool TargetEstimator::is_stale(const float timeout_duration_ms) const
+{
+	const bool measurements_stale = (hrt_absolute_time() - _last_follow_target_timestamp) / 1000.0f >=
+					timeout_duration_ms;
+	return measurements_stale;
+}
+
+void TargetEstimator::reset()
+{
+	_last_filter_reset_timestamp = hrt_absolute_time();  // debug only
+	_last_position_fusion_timestamp = _last_velocity_fusion_timestamp = 0;
+	_last_follow_target_timestamp = 0;
+	_filter_states.pos_ned_est.setAll(NAN);
+	_filter_states.vel_ned_est.setAll(NAN);
+	_filter_states.acc_ned_est.setAll(NAN);
+	_pos_measurement_old.setAll(NAN);
+	_vel_measurement_old.setAll(NAN);
+}
diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp
new file mode 100644
index 0000000000..ab3fe70e29
--- /dev/null
+++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp
@@ -0,0 +1,216 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file TargetEstimator.hpp
+ *
+ * @author Alessandro Simovic <potaito-dev@protonmail.com>
+ */
+
+#pragma once
+
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+#include <uORB/topics/follow_target.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/follow_target_estimator.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/PublicationMulti.hpp>
+
+#include <drivers/drv_hrt.h>
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <lib/geo/geo.h>
+#include <mathlib/mathlib.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+static constexpr float GPS_MESSAGE_STALE_TIMEOUT_MS =
+	3000.0f;  	// Duration after which the connection to the target is considered lost
+static constexpr float MINIMUM_TIME_BETWEEN_POS_FUSIONS_MS = 500.0f;
+static constexpr float MINIMUM_TIME_BETWEEN_VEL_FUSIONS_MS = 100.0f;
+static constexpr float ACCELERATION_SATURATION = 20.0f; 		// 2*g
+static constexpr float MINIMUM_SPEED_FOR_TARGET_MOVING =
+	0.1f; 	// speed threshold above which the target is considered to be moving
+
+using namespace time_literals;
+
+struct filter_gains_s {
+	// Position fusion gains
+	float G_p;
+	float H_p;
+	float K_p;
+
+	// Velocity fusion gains
+	float G_v;
+	float H_v;
+
+	// Estimator position / velocity update gains
+	float responsiveness;
+};
+
+struct filter_states_s {
+	matrix::Vector3f pos_ned_est{};	// target's position in NED frame
+	matrix::Vector3f vel_ned_est{};	// target's velocity in NED frame
+	matrix::Vector3f acc_ned_est{};	// target's acceleration in NED frame
+
+	/**
+	 * Check if all state are finite
+	 *
+	 * @return true if all state are finite, or false as soon as any state is NAN
+	 */
+	bool is_finite()
+	{
+		return  PX4_ISFINITE(pos_ned_est(0)) && PX4_ISFINITE(pos_ned_est(1)) && PX4_ISFINITE(pos_ned_est(2)) &&
+			PX4_ISFINITE(vel_ned_est(0)) && PX4_ISFINITE(vel_ned_est(1)) && PX4_ISFINITE(vel_ned_est(2)) &&
+			PX4_ISFINITE(acc_ned_est(0)) && PX4_ISFINITE(acc_ned_est(1)) && PX4_ISFINITE(acc_ned_est(2));
+	}
+
+	/**
+	 * Limits the acceleration state to some sane value to prevent unrealistic
+	 * spikes in the acceleration, which could cause severely unexpected behaviour in the drone
+	 * that is tracking the target
+	 *
+	 * @param saturation [m/s^2] limit to use for acceleration saturation
+	 */
+	void saturate_acceleration(float saturation)
+	{
+		if (acc_ned_est.norm_squared() > saturation) {
+			acc_ned_est = acc_ned_est.unit_or_zero() * saturation * saturation;
+		}
+	}
+};
+
+class TargetEstimator : public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	TargetEstimator();
+	~TargetEstimator() override;
+
+	bool Start();
+	void Stop();
+
+protected:
+	void Run() override;
+
+	/**
+	 * Process new measurement data and iterate filter to current time
+	 */
+	void update();
+
+	/**
+	 * Check for parameter changes and update them if needed.
+	 *
+	 * @param force for a parameter update
+	 */
+	void parameters_update(bool force = false);
+
+	/**
+	 * Recompute filter gains
+	 *
+	 * @param filter_gains filter gains that will be updated based on current PX4 parameters
+	 */
+	void update_filter_gains(filter_gains_s &filter_gains) const;
+
+	/**
+	 * Checks whether a sensor measurement should be fused or not
+	 *
+	 *
+	 * @param current_measurement the most recent measurement that has not yet been fused
+	 * @param previous_measurement the previous measurement that was accepted and therefore fused
+	 * @param last_fusion_timestamp last timestamp of when this sensor data was fused
+	 * @param min_delta_t minimum amount of time that needs to pass between two sensor fusions
+	 */
+	bool measurement_can_be_fused(const matrix::Vector3f &current_measurement, const matrix::Vector3f &previous_measurement,
+				      uint64_t last_fusion_timestamp, float min_delta_t) const;
+
+	/**
+	 * Perform filter update with new follow_target data
+	 *
+	 * @param follow_target GPS data last received from target
+	 */
+	void measurement_update(follow_target_s follow_target);
+
+	/**
+	 * Perform prediction step based on simple position-velocity-acceleration model of a point mass
+	 * Can be called at a much higher frequency than measurement data is being received.
+	 *
+	 * @param deltatime time [s] since the last prediction or measurement update
+	 */
+	void prediction_update(float deltatime);
+
+	/**
+	 * Get current LAT/LON/ALT estimate of target
+	 *
+	 * @return Current position estimate of target as latitude / longitude / altitude vector
+	 */
+	matrix::Vector3<double> get_lat_lon_alt_est() const;
+
+	/**
+	 * Check if last received data from target is too old
+	 *
+	 * @param timeout_duration_ms timeout in [ms] to use for this check
+	 */
+	bool is_stale(const float timeout_duration_ms) const;
+
+	/**
+	 * Reset all filter states causing it to completely forget the old filter state
+	 */
+	void reset();
+
+	filter_gains_s _filter_gains;
+	filter_states_s _filter_states;
+
+	MapProjection _reference_position{};
+	vehicle_local_position_s _vehicle_local_position{};
+
+	hrt_abstime _last_iteration_timestamp{0};
+	hrt_abstime _last_filter_reset_timestamp{0};
+	hrt_abstime _last_position_fusion_timestamp{0};
+	hrt_abstime _last_velocity_fusion_timestamp{0};
+	hrt_abstime _last_follow_target_timestamp{0};
+
+	// Pos/vel from previous measurement update. Required for filtering duplicate messages
+	matrix::Vector3f _pos_measurement_old{};
+	matrix::Vector3f _vel_measurement_old{};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::NAV_FT_RS>) _param_nav_ft_rs
+	)
+
+	// Subscriptions
+	uORB::SubscriptionCallbackWorkItem _follow_target_sub{this, ORB_ID(follow_target)};
+	uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
+	uORB::PublicationMulti<follow_target_estimator_s> _follow_target_estimator_pub{ORB_ID(follow_target_estimator)};
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+};
diff --git a/src/modules/navigator/follow_target_params.c b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c
similarity index 75%
rename from src/modules/navigator/follow_target_params.c
rename to src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c
index 4dc50bf6e5..82855b3096 100644
--- a/src/modules/navigator/follow_target_params.c
+++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.c
@@ -68,10 +68,19 @@ PARAM_DEFINE_FLOAT(NAV_FT_DST, 8.0f);
 /**
  * Side to follow target from
  *
- * The side to follow the target from (front right = 0, behind = 1, front = 2, front left = 3)
+ * The side to follow the target from
+ * none         = 0
+ * behind       = 1
+ * front        = 2
+ * front right  = 3
+ * front left   = 4
+ * mid right    = 5
+ * mid left     = 6
+ * behind right = 7
+ * behind left  = 8
  *
  * @min 0
- * @max 3
+ * @max 8
  * @group Follow target
  */
 PARAM_DEFINE_INT32(NAV_FT_FS, 1);
@@ -79,7 +88,7 @@ PARAM_DEFINE_INT32(NAV_FT_FS, 1);
 /**
  * Dynamic filtering algorithm responsiveness to target movement
  *
- * lower numbers increase the responsiveness to changing long lat
+ * lower values increase the responsiveness to changing long lat
  * but also ignore less noise
  *
  * @min 0.0
@@ -89,3 +98,16 @@ PARAM_DEFINE_INT32(NAV_FT_FS, 1);
  */
 PARAM_DEFINE_FLOAT(NAV_FT_RS, 0.5f);
 
+/**
+ * Altitude control mode
+ *
+ * Maintain altitude or track target's altitude. When maintaining the altitude,
+ * the drone can crash into terrain when the target moves uphill. When tracking
+ * the target's altitude, the follow altitude NAV_MIN_FT_HT should be high enough
+ * to prevent terrain collisions due to GPS inaccuracies of the target.
+ *
+ * @value 0 Maintain constant altitude and track XY position only (2D tracking)
+ * @value 1 Track target's altitude as well (3D tracking)
+ * @group Follow target
+ */
+PARAM_DEFINE_INT32(NAV_FT_ALT_M, 0);
diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
index 726b1e2c00..1e32646a89 100644
--- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
+++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
@@ -3,11 +3,9 @@
 #include <lib/geo/geo.h>
 
 constexpr uint64_t FlightTask::_timeout;
-// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
-const trajectory_setpoint_s FlightTask::empty_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};
-
+const trajectory_setpoint_s FlightTask::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};
 const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, false, {}};
-const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
+const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_DOWN, {}};
 
 bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint)
 {
@@ -21,7 +19,7 @@ bool FlightTask::activate(const trajectory_setpoint_s &last_setpoint)
 void FlightTask::reActivate()
 {
 	// Preserve vertical velocity while on the ground to allow descending by stick for reliable land detection
-	trajectory_setpoint_s setpoint_preserve_vertical{empty_setpoint};
+	trajectory_setpoint_s setpoint_preserve_vertical{empty_trajectory_setpoint};
 	setpoint_preserve_vertical.velocity[2] = _velocity_setpoint(2);
 	activate(setpoint_preserve_vertical);
 }
diff --git a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
index 486c41651b..e2083776b6 100644
--- a/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
+++ b/src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.hpp
@@ -139,10 +139,9 @@ public:
 	const vehicle_trajectory_waypoint_s &getAvoidanceWaypoint() { return _desired_waypoint; }
 
 	/**
-	 * Empty setpoint.
-	 * All setpoints are set to NAN.
+	 * All setpoints are set to NAN (uncontrolled). Timestampt zero.
 	 */
-	static const trajectory_setpoint_s empty_setpoint;
+	static const trajectory_setpoint_s empty_trajectory_setpoint;
 
 	/**
 	 * Empty constraints.
diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
index 243b59dcce..afd729152c 100644
--- a/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
+++ b/src/modules/flight_mode_manager/tasks/ManualAltitude/FlightTaskManualAltitude.cpp
@@ -273,14 +273,14 @@ void FlightTaskManualAltitude::_respectMaxAltitude()
 
 void FlightTaskManualAltitude::_respectGroundSlowdown()
 {
-	// limit speed gradually within the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
+	// Interpolate descent rate between the altitudes MPC_LAND_ALT1 and MPC_LAND_ALT2
 	if (PX4_ISFINITE(_dist_to_ground)) {
-		const float limit_down = math::gradual(_dist_to_ground,
-						       _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
-						       _param_mpc_land_speed.get(), _constraints.speed_down);
-		const float limit_up = math::gradual(_dist_to_ground,
-						     _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
-						     _param_mpc_tko_speed.get(), _constraints.speed_up);
+		const float limit_down = math::interpolate(_dist_to_ground,
+					 _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+					 _param_mpc_land_speed.get(), _constraints.speed_down);
+		const float limit_up = math::interpolate(_dist_to_ground,
+				       _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+				       _param_mpc_tko_speed.get(), _constraints.speed_up);
 		_velocity_setpoint(2) = math::constrain(_velocity_setpoint(2), -limit_up, limit_down);
 	}
 }
diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
index 5aedd0903e..1998bd9dc0 100644
--- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
+++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.cpp
@@ -75,13 +75,16 @@ void FlightTaskManualPosition::_scaleSticks()
 	/* Use same scaling as for FlightTaskManualAltitude */
 	FlightTaskManualAltitude::_scaleSticks();
 
-	/* Constrain length of stick inputs to 1 for xy*/
 	Vector2f stick_xy = _sticks.getPositionExpo().slice<2, 1>(0, 0);
 
-	const float mag = math::constrain(stick_xy.length(), 0.0f, 1.0f);
+	Sticks::limitStickUnitLengthXY(stick_xy);
 
-	if (mag > FLT_EPSILON) {
-		stick_xy = stick_xy.normalized() * mag;
+	if (_param_mpc_vel_man_side.get() >= 0.f) {
+		stick_xy(1) *= _param_mpc_vel_man_side.get() / _param_mpc_vel_manual.get();
+	}
+
+	if ((_param_mpc_vel_man_back.get() >= 0.f) && (stick_xy(0) < 0.f)) {
+		stick_xy(0) *= _param_mpc_vel_man_back.get() / _param_mpc_vel_manual.get();
 	}
 
 	const float max_speed_from_estimator = _sub_vehicle_local_position.get().vxy_max;
diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp
index ca882b619c..37314fb6e7 100644
--- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp
+++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp
@@ -65,6 +65,8 @@ protected:
 
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
 					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+					(ParamFloat<px4::params::MPC_VEL_MAN_SIDE>) _param_mpc_vel_man_side,
+					(ParamFloat<px4::params::MPC_VEL_MAN_BACK>) _param_mpc_vel_man_back,
 					(ParamFloat<px4::params::MPC_ACC_HOR_MAX>) _param_mpc_acc_hor_max,
 					(ParamFloat<px4::params::MPC_HOLD_MAX_XY>) _param_mpc_hold_max_xy
 				       )
diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
index 09457fbabb..4735cb6765 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
@@ -72,7 +72,7 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
 
 	float new_velocity = signFromBool(new_is_clockwise) * new_absolute_velocity;
 
-	if (math::isInRange(new_radius, _radius_min, _radius_max)) {
+	if (math::isInRange(new_radius, _radius_min, _param_mc_orbit_rad_max.get())) {
 		_started_clockwise = new_is_clockwise;
 		_sanitizeParams(new_radius, new_velocity);
 		_orbit_radius = new_radius;
@@ -146,7 +146,7 @@ bool FlightTaskOrbit::sendTelemetry()
 void FlightTaskOrbit::_sanitizeParams(float &radius, float &velocity) const
 {
 	// clip the radius to be within range
-	radius = math::constrain(radius, _radius_min, _radius_max);
+	radius = math::constrain(radius, _radius_min, _param_mc_orbit_rad_max.get());
 	velocity = math::constrain(velocity, -fabsf(_velocity_max), fabsf(_velocity_max));
 
 	bool exceeds_maximum_acceleration = (velocity * velocity) >= _acceleration_max * radius;
diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
index 37ed265918..900c8e469a 100644
--- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp
@@ -70,7 +70,6 @@ protected:
 private:
 	/* TODO: Should be controlled by params */
 	static constexpr float _radius_min = 1.f;
-	static constexpr float _radius_max = 1e3f;
 	static constexpr float _velocity_max = 10.f;
 	static constexpr float _acceleration_max = 2.f;
 	static constexpr float _horizontal_acceptance_radius = 2.f;
@@ -132,6 +131,7 @@ private:
 	uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
 
 	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::MC_ORBIT_RAD_MAX>) _param_mc_orbit_rad_max,
 		(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise, /**< cruise speed for circle approach */
 		(ParamFloat<px4::params::MPC_YAWRAUTO_MAX>) _param_mpc_yawrauto_max,
 		(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp b/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c
similarity index 81%
rename from src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp
rename to src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c
index 733c8d3754..3e655c80e2 100644
--- a/src/modules/flight_mode_manager/tasks/AutoFollowMe/FlightTaskAutoFollowMe.hpp
+++ b/src/modules/flight_mode_manager/tasks/Orbit/flight_task_orbit_params.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   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
@@ -32,19 +32,13 @@
  ****************************************************************************/
 
 /**
- * @file FlightTaskAutoFollowMe.hpp
+ * Maximum radius of orbit
  *
- * Flight task for autonomous, gps driven follow-me mode.
+ * @unit m
+ * @min 1.0
+ * @max 10000.0
+ * @increment 0.5
+ * @decimal 1
+ * @group FlightTaskOrbit
  */
-
-#pragma once
-
-#include "FlightTaskAuto.hpp"
-
-class FlightTaskAutoFollowMe : public FlightTaskAuto
-{
-public:
-	FlightTaskAutoFollowMe() = default;
-	virtual ~FlightTaskAutoFollowMe() = default;
-	bool update() override;
-};
+PARAM_DEFINE_FLOAT(MC_ORBIT_RAD_MAX, 1000.0f);
diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
index b6a33e9bce..5c0d8f9ca7 100644
--- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
+++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.cpp
@@ -81,6 +81,15 @@ void StickAccelerationXY::generateSetpoints(Vector2f stick_xy, const float yaw,
 
 	// Map stick input to acceleration
 	Sticks::limitStickUnitLengthXY(stick_xy);
+
+	if (_param_mpc_vel_man_side.get() >= 0.f) {
+		stick_xy(1) *= _param_mpc_vel_man_side.get() / _param_mpc_vel_manual.get();
+	}
+
+	if ((_param_mpc_vel_man_back.get() >= 0.f) && (stick_xy(0) < 0.f)) {
+		stick_xy(0) *= _param_mpc_vel_man_back.get() / _param_mpc_vel_manual.get();
+	}
+
 	Sticks::rotateIntoHeadingFrameXY(stick_xy, yaw, yaw_sp);
 	_acceleration_setpoint = stick_xy.emult(acceleration_scale);
 	applyJerkLimit(dt);
diff --git a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
index 94637217d6..fe0aee7acb 100644
--- a/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
+++ b/src/modules/flight_mode_manager/tasks/Utility/StickAccelerationXY.hpp
@@ -81,6 +81,8 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
+		(ParamFloat<px4::params::MPC_VEL_MAN_SIDE>) _param_mpc_vel_man_side,
+		(ParamFloat<px4::params::MPC_VEL_MAN_BACK>) _param_mpc_vel_man_back,
 		(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor,
 		(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
 		(ParamFloat<px4::params::MPC_TILTMAX_AIR>) _param_mpc_tiltmax_air
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
index 94b01f2706..80a6729e7e 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp
@@ -35,12 +35,12 @@
 
 using namespace time_literals;
 using math::constrain;
-using math::gradual;
+using math::interpolate;
 using math::radians;
 
 FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
 	ModuleParams(nullptr),
-	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
 	_actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)),
 	_actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)),
 	_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
@@ -155,6 +155,10 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 					Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
 					q.copyTo(_att_sp.q_d);
 
+					_att_sp.roll_reset_integral = false;
+					_att_sp.pitch_reset_integral = false;
+					_att_sp.yaw_reset_integral = false;
+
 					_att_sp.timestamp = hrt_absolute_time();
 
 					_attitude_sp_pub.publish(_att_sp);
@@ -181,6 +185,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
 						_manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
 					_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f,
 							1.0f);
+					_actuator_controls.control[actuator_controls_s::INDEX_WHEEL] = _manual_control_setpoint.r;
 				}
 			}
 		}
@@ -270,9 +275,7 @@ void FixedwingAttitudeControl::Run()
 	perf_begin(_loop_perf);
 
 	// only run controller if attitude changed
-	vehicle_attitude_s att;
-
-	if (_att_sub.update(&att)) {
+	if (_att_sub.updated() || (hrt_elapsed_time(&_last_run) > 20_ms)) {
 
 		// only update parameters if they changed
 		bool params_updated = _parameter_update_sub.updated();
@@ -288,11 +291,26 @@ void FixedwingAttitudeControl::Run()
 			parameters_update();
 		}
 
-		const float dt = math::constrain((att.timestamp - _last_run) * 1e-6f, 0.002f, 0.04f);
-		_last_run = att.timestamp;
+		float dt = 0.f;
 
-		/* get current rotation matrix and euler angles from control state quaternions */
-		matrix::Dcmf R = matrix::Quatf(att.q);
+		static constexpr float DT_MIN = 0.002f;
+		static constexpr float DT_MAX = 0.04f;
+
+		vehicle_attitude_s att{};
+
+		if (_att_sub.copy(&att)) {
+			dt = math::constrain((att.timestamp_sample - _last_run) * 1e-6f, DT_MIN, DT_MAX);
+			_last_run = att.timestamp_sample;
+
+			// get current rotation matrix and euler angles from control state quaternions
+			_R = matrix::Quatf(att.q);
+		}
+
+		if (dt < DT_MIN || dt > DT_MAX) {
+			const hrt_abstime time_now_us = hrt_absolute_time();
+			dt = math::constrain((time_now_us - _last_run) * 1e-6f, DT_MIN, DT_MAX);
+			_last_run = time_now_us;
+		}
 
 		vehicle_angular_velocity_s angular_velocity{};
 		_vehicle_rates_sub.copy(&angular_velocity);
@@ -317,17 +335,17 @@ void FixedwingAttitudeControl::Run()
 			 * Rxy	Ryy  Rzy		-Rzy  Ryy  Rxy
 			 * Rxz	Ryz  Rzz		-Rzz  Ryz  Rxz
 			 * */
-			matrix::Dcmf R_adapted = R;		//modified rotation matrix
+			matrix::Dcmf R_adapted = _R;		//modified rotation matrix
 
 			/* move z to x */
-			R_adapted(0, 0) = R(0, 2);
-			R_adapted(1, 0) = R(1, 2);
-			R_adapted(2, 0) = R(2, 2);
+			R_adapted(0, 0) = _R(0, 2);
+			R_adapted(1, 0) = _R(1, 2);
+			R_adapted(2, 0) = _R(2, 2);
 
 			/* move x to z */
-			R_adapted(0, 2) = R(0, 0);
-			R_adapted(1, 2) = R(1, 0);
-			R_adapted(2, 2) = R(2, 0);
+			R_adapted(0, 2) = _R(0, 0);
+			R_adapted(1, 2) = _R(1, 0);
+			R_adapted(2, 2) = _R(2, 0);
 
 			/* change direction of pitch (convert to right handed system) */
 			R_adapted(0, 0) = -R_adapted(0, 0);
@@ -335,7 +353,7 @@ void FixedwingAttitudeControl::Run()
 			R_adapted(2, 0) = -R_adapted(2, 0);
 
 			/* fill in new attitude data */
-			R = R_adapted;
+			_R = R_adapted;
 
 			/* lastly, roll- and yawspeed have to be swaped */
 			float helper = rollspeed;
@@ -343,7 +361,7 @@ void FixedwingAttitudeControl::Run()
 			yawspeed = helper;
 		}
 
-		const matrix::Eulerf euler_angles(R);
+		const matrix::Eulerf euler_angles(_R);
 
 		vehicle_attitude_setpoint_poll();
 
@@ -360,7 +378,6 @@ void FixedwingAttitudeControl::Run()
 
 		bool wheel_control = false;
 
-		// TODO: manual wheel_control on ground?
 		if (_param_fw_w_en.get() && _att_sp.fw_control_yaw) {
 			wheel_control = true;
 		}
@@ -475,20 +492,23 @@ void FixedwingAttitudeControl::Run()
 			float trim_yaw = _param_trim_yaw.get();
 
 			if (airspeed < _param_fw_airspd_trim.get()) {
-				trim_roll += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_r_vmin.get(),
-						     0.0f);
-				trim_pitch += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_p_vmin.get(),
-						      0.0f);
-				trim_yaw += gradual(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(), _param_fw_dtrim_y_vmin.get(),
-						    0.0f);
+				trim_roll += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
+							 _param_fw_dtrim_r_vmin.get(),
+							 0.0f);
+				trim_pitch += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
+							  _param_fw_dtrim_p_vmin.get(),
+							  0.0f);
+				trim_yaw += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
+							_param_fw_dtrim_y_vmin.get(),
+							0.0f);
 
 			} else {
-				trim_roll += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
-						     _param_fw_dtrim_r_vmax.get());
-				trim_pitch += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
-						      _param_fw_dtrim_p_vmax.get());
-				trim_yaw += gradual(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
-						    _param_fw_dtrim_y_vmax.get());
+				trim_roll += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
+							 _param_fw_dtrim_r_vmax.get());
+				trim_pitch += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
+							  _param_fw_dtrim_p_vmax.get());
+				trim_yaw += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
+							_param_fw_dtrim_y_vmax.get());
 			}
 
 			/* add trim increment if flaps are deployed  */
@@ -503,14 +523,12 @@ void FixedwingAttitudeControl::Run()
 				if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
 					_roll_ctrl.control_attitude(dt, control_input);
 					_pitch_ctrl.control_attitude(dt, control_input);
+					_yaw_ctrl.control_attitude(dt, control_input);
 
 					if (wheel_control) {
 						_wheel_ctrl.control_attitude(dt, control_input);
-						_yaw_ctrl.reset_integrator();
 
 					} else {
-						// runs last, because is depending on output of roll and pitch attitude
-						_yaw_ctrl.control_attitude(dt, control_input);
 						_wheel_ctrl.reset_integrator();
 					}
 
@@ -535,7 +553,10 @@ void FixedwingAttitudeControl::Run()
 					}
 
 					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
+
+					// roll rate control
 					float roll_u = _roll_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(0));
+
 					_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
 						(PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll;
 
@@ -543,7 +564,9 @@ void FixedwingAttitudeControl::Run()
 						_roll_ctrl.reset_integrator();
 					}
 
+					// pitch rate control
 					float pitch_u = _pitch_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(1));
+
 					_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
 						(PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch;
 
@@ -551,27 +574,39 @@ void FixedwingAttitudeControl::Run()
 						_pitch_ctrl.reset_integrator();
 					}
 
-					float yaw_u = 0.0f;
-
-					if (wheel_control) {
-						yaw_u = _wheel_ctrl.control_bodyrate(dt, control_input);
-
-					} else {
-						yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(2));
-					}
+					// yaw rate control
+					const float yaw_u = _yaw_ctrl.control_euler_rate(dt, control_input, bodyrate_ff(2));
 
 					_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw;
 
-					/* add in manual rudder control in manual modes */
-					if (_vcontrol_mode.flag_control_manual_enabled) {
-						_actuator_controls.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r;
-					}
-
 					if (!PX4_ISFINITE(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
+					}
+
+					// wheel control
+					// XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from
+					// position controller during auto modes _manual_control_setpoint.r gets passed
+					// whenever nudging is enabled, otherwise zero
+					const float wheel_u = (wheel_control) ? _wheel_ctrl.control_bodyrate(dt,
+							      control_input) + _att_sp.yaw_sp_move_rate : 0.0f;
+
+					_actuator_controls.control[actuator_controls_s::INDEX_WHEEL] = (PX4_ISFINITE(wheel_u)) ? wheel_u : 0.0f;
+
+					if (!PX4_ISFINITE(wheel_u)) {
 						_wheel_ctrl.reset_integrator();
 					}
 
+					// manual actuator overrides
+					if (_vcontrol_mode.flag_control_manual_enabled) {
+						// add manual inputs to rudder in manual modes. Scaling is controllable via FW_MAN_YR_MAX
+						const float yaw_scale = math::constrain(_param_fw_man_yr_max.get() / math::max(_param_fw_y_rmax.get(), FLT_EPSILON),
+											0.f, 1.f);
+						_actuator_controls.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r * yaw_scale;
+
+						// explicitly command wheel via yaw stick in manual modes
+						_actuator_controls.control[actuator_controls_s::INDEX_WHEEL] = _manual_control_setpoint.r;
+					}
+
 					/* throttle passed through if it is finite */
 					_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] =
 						(PX4_ISFINITE(_att_sp.thrust_body[0])) ? _att_sp.thrust_body[0] : 0.0f;
@@ -598,7 +633,7 @@ void FixedwingAttitudeControl::Run()
 				 */
 				_rates_sp.roll = _roll_ctrl.get_desired_bodyrate();
 				_rates_sp.pitch = _pitch_ctrl.get_desired_bodyrate();
-				_rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate();
+				_rates_sp.yaw = (wheel_control) ? _wheel_ctrl.get_desired_rate() : _yaw_ctrl.get_desired_bodyrate();
 
 				_rates_sp.timestamp = hrt_absolute_time();
 
@@ -670,6 +705,9 @@ void FixedwingAttitudeControl::Run()
 		updateActuatorControlsStatus(dt);
 	}
 
+	// backup schedule
+	ScheduleDelayed(20_ms);
+
 	perf_end(_loop_perf);
 }
 
diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
index 117155ba69..ec4ae5cac7 100644
--- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
+++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -48,7 +48,7 @@
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/tasks.h>
-#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
@@ -83,7 +83,7 @@ static constexpr float kFlapSlewRate = 1.f; //minimum time from none to full fla
 static constexpr float kSpoilerSlewRate = 1.f; //minimum time from none to full spoiler deflection [s]
 
 class FixedwingAttitudeControl final : public ModuleBase<FixedwingAttitudeControl>, public ModuleParams,
-	public px4::WorkItem
+	public px4::ScheduledWorkItem
 {
 public:
 	FixedwingAttitudeControl(bool vtol = false);
@@ -131,15 +131,17 @@ private:
 	uORB::Publication<vehicle_thrust_setpoint_s>	_vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
 	uORB::Publication<vehicle_torque_setpoint_s>	_vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
 
-	actuator_controls_s			_actuator_controls {};		/**< actuator control inputs */
-	manual_control_setpoint_s		_manual_control_setpoint {};	/**< r/c channel data */
-	vehicle_attitude_setpoint_s		_att_sp {};			/**< vehicle attitude setpoint */
-	vehicle_control_mode_s			_vcontrol_mode {};		/**< vehicle control mode */
-	vehicle_local_position_s		_local_pos {};			/**< local position */
-	vehicle_rates_setpoint_s		_rates_sp {};			/* attitude rates setpoint */
-	vehicle_status_s			_vehicle_status {};		/**< vehicle status */
+	actuator_controls_s			_actuator_controls{};
+	manual_control_setpoint_s		_manual_control_setpoint{};
+	vehicle_attitude_setpoint_s		_att_sp{};
+	vehicle_control_mode_s			_vcontrol_mode{};
+	vehicle_local_position_s		_local_pos{};
+	vehicle_rates_setpoint_s		_rates_sp{};
+	vehicle_status_s			_vehicle_status{};
 
-	perf_counter_t	_loop_perf;						/**< loop performance counter */
+	matrix::Dcmf _R{matrix::eye<float, 3>()};
+
+	perf_counter_t _loop_perf;
 
 	hrt_abstime _last_run{0};
 
@@ -164,6 +166,7 @@ private:
 		(ParamFloat<px4::params::FW_ACRO_Z_MAX>) _param_fw_acro_z_max,
 
 		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
+		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
 		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
 		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
 		(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
@@ -226,7 +229,9 @@ private:
 
 		(ParamFloat<px4::params::TRIM_PITCH>) _param_trim_pitch,
 		(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
-		(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw
+		(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw,
+
+		(ParamFloat<px4::params::FW_MAN_YR_MAX>) _param_fw_man_yr_max
 	)
 
 	ECL_RollController		_roll_ctrl;
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index 4afe0f1192..df1b2e2970 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -455,7 +455,7 @@ PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
  * @increment 0.5
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
+PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 30.0f);
 
 /**
  * Flaps setting during take-off
@@ -514,6 +514,24 @@ PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);
  */
 PARAM_DEFINE_INT32(FW_ARSP_SCALE_EN, 1);
 
+/**
+ * Trim / Cruise airspeed
+ *
+ * This is the default cruise airspeed setpoint (calibrated airspeed) used by the system in assisted
+ * and autonomous control modes if no other airspeed setpoint is given.
+ * It is also used for control surface effectiveness scaling:
+ * It is the cruise airspeed around which the airspeed scaling is applied
+ * (scaling = FW_AIRSPD_TRIM / calibrated_airspeed).
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 40
+ * @decimal 1
+ * @increment 0.5
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
+
 /**
  * Manual roll scale
  *
@@ -579,10 +597,12 @@ PARAM_DEFINE_INT32(FW_BAT_SCALE_EN, 0);
  *
  * @min 45
  * @max 720
- * @unit deg
+ * @decimal 1
+ * @increment 5
+ * @unit deg/s
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90);
+PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90.f);
 
 /**
  * Acro body y max rate.
@@ -592,10 +612,12 @@ PARAM_DEFINE_FLOAT(FW_ACRO_X_MAX, 90);
  *
  * @min 45
  * @max 720
- * @unit deg
+ * @decimal 1
+ * @increment 5
+ * @unit deg/s
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90);
+PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90.f);
 
 /**
  * Acro body z max rate.
@@ -605,10 +627,12 @@ PARAM_DEFINE_FLOAT(FW_ACRO_Y_MAX, 90);
  *
  * @min 10
  * @max 180
- * @unit deg
+ * @decimal 1
+ * @increment 5
+ * @unit deg/s
  * @group FW Attitude Control
  */
-PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45);
+PARAM_DEFINE_FLOAT(FW_ACRO_Z_MAX, 45.f);
 
 /**
 * Roll trim increment at minimum airspeed
@@ -762,3 +786,18 @@ PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f);
  * @group FW Attitude Control
  */
 PARAM_DEFINE_INT32(FW_SPOILERS_MAN, 0);
+
+/**
+ * Maximum manually added yaw rate
+ *
+ * This is the maximally added yaw rate setpoint from the yaw stick in any attitude controlled flight mode.
+ * The controller already generates a yaw rate setpoint to coordinate a turn, and this value is added to it.
+ * This is an absolute value, which is applied symmetrically to the negative and positive side.
+ *
+ * @unit deg/s
+ * @min 0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_MAN_YR_MAX, 30.f);
diff --git a/src/modules/fw_pos_control_l1/CMakeLists.txt b/src/modules/fw_pos_control_l1/CMakeLists.txt
index 7aff012788..e7e02669f7 100644
--- a/src/modules/fw_pos_control_l1/CMakeLists.txt
+++ b/src/modules/fw_pos_control_l1/CMakeLists.txt
@@ -33,6 +33,7 @@
 
 add_subdirectory(launchdetection)
 add_subdirectory(runway_takeoff)
+add_subdirectory(figure_eight)
 
 px4_add_module(
 	MODULE modules__fw_pos_control_l1
@@ -41,9 +42,9 @@ px4_add_module(
 		FixedwingPositionControl.cpp
 		FixedwingPositionControl.hpp
 	DEPENDS
+		figure_eight
 		l1
 		launchdetection
-		landing_slope
     npfg
 		runway_takeoff
 		SlewRate
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
index 5c099c8420..7be0988372 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
@@ -53,6 +53,7 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
 	_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")),
+	_figure_eight(_npfg, _l1_control, _wind_vel, _eas2tas),
 	_launchDetector(this),
 	_runway_takeoff(this)
 {
@@ -125,7 +126,7 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_max_climb_rate(_param_fw_t_clmb_max.get());
 	_tecs.set_max_sink_rate(_param_fw_t_sink_max.get());
 	_tecs.set_speed_weight(_param_fw_t_spdweight.get());
-	_tecs.set_equivalent_airspeed_cruise(_param_fw_airspd_trim.get());
+	_tecs.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get());
 	_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get());
 	_tecs.set_equivalent_airspeed_max(_param_fw_airspd_max.get());
 	_tecs.set_min_sink_rate(_param_fw_t_sink_min.get());
@@ -144,19 +145,11 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get());
 	_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
 
-
-	// Landing slope
-	/* check if negative value for 2/3 of flare altitude is set for throttle cut */
-	float land_thrust_lim_alt_relative = _param_fw_lnd_tlalt.get();
-
-	if (land_thrust_lim_alt_relative < 0.0f) {
-		land_thrust_lim_alt_relative = 0.66f * _param_fw_lnd_flalt.get();
-	}
-
-	_landingslope.update(radians(_param_fw_lnd_ang.get()), _param_fw_lnd_flalt.get(), land_thrust_lim_alt_relative,
-			     _param_fw_lnd_hvirt.get());
-
-	landing_status_publish();
+	// ECO settings (APX4 custom)
+	_tecs.set_speed_weight_eco(_param_fw_t_spdweight_eco.get());
+	_tecs.set_height_error_time_constant_eco(_param_fw_t_h_error_tc_eco.get());
+	_tecs.set_fw_eco_alt_err_u(_param_fw_eco_alt_err_u.get());
+	_tecs.set_fw_eco_alt_err_o(_param_fw_eco_alt_err_o.get());
 
 	int check_ret = PX4_OK;
 
@@ -189,29 +182,29 @@ FixedwingPositionControl::parameters_update()
 
 	if (_param_fw_airspd_trim.get() < _param_fw_airspd_min.get() ||
 	    _param_fw_airspd_trim.get() > _param_fw_airspd_max.get()) {
-		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed cruise out of min or max bounds\t");
+		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Airspeed trim out of min or max bounds\t");
 		/* EVENT
 		 * @description
 		 * - <param>FW_AIRSPD_MAX</param>: {1:.1}
 		 * - <param>FW_AIRSPD_MIN</param>: {2:.1}
 		 * - <param>FW_AIRSPD_TRIM</param>: {3:.1}
 		 */
-		events::send<float, float, float>(events::ID("fixedwing_position_control_conf_invalid_cruise_bounds"),
+		events::send<float, float, float>(events::ID("fixedwing_position_control_conf_invalid_trim_bounds"),
 						  events::Log::Error,
-						  "Invalid configuration: Airspeed cruise out of min or max bounds",
+						  "Invalid configuration: Airspeed trim out of min or max bounds",
 						  _param_fw_airspd_max.get(), _param_fw_airspd_min.get(), _param_fw_airspd_trim.get());
 		check_ret = PX4_ERROR;
 	}
 
-	if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get() * 0.9f) {
-		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: Stall airspeed higher than 0.9 of min\t");
+	if (_param_fw_airspd_stall.get() > _param_fw_airspd_min.get()) {
+		mavlink_log_critical(&_mavlink_log_pub, "Config invalid: FW_AIRSPD_STALL airspeed higher than FW_AIRSPD_MIN\t");
 		/* EVENT
 		 * @description
 		 * - <param>FW_AIRSPD_MIN</param>: {1:.1}
 		 * - <param>FW_AIRSPD_STALL</param>: {2:.1}
 		 */
 		events::send<float, float>(events::ID("fixedwing_position_control_conf_invalid_stall"), events::Log::Error,
-					   "Invalid configuration: Stall airspeed higher than 90% of minimum airspeed",
+					   "Invalid configuration: FW_AIRSPD_STALL higher FW_AIRSPD_MIN",
 					   _param_fw_airspd_min.get(), _param_fw_airspd_stall.get());
 		check_ret = PX4_ERROR;
 	}
@@ -229,7 +222,7 @@ FixedwingPositionControl::vehicle_control_mode_poll()
 
 			// reset state when arming
 			if (!was_armed && _control_mode.flag_armed) {
-				reset_takeoff_state(true);
+				reset_takeoff_state();
 				reset_landing_state();
 			}
 		}
@@ -245,22 +238,25 @@ FixedwingPositionControl::vehicle_command_poll()
 		if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
 			// only abort landing before point of no return (horizontal and vertical)
 			if (_control_mode.flag_control_auto_enabled &&
-			    _pos_sp_triplet.current.valid &&
+			    _position_setpoint_current_valid &&
 			    (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) {
 
-				abort_landing(true);
+				updateLandingAbortStatus(position_controller_landing_status_s::ABORTED_BY_OPERATOR);
 			}
 
-		} else if ((vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED)
-			   && (static_cast<uint8_t>(vehicle_command.param1 + .5f) == vehicle_command_s::SPEED_TYPE_AIRSPEED)
-			   && (vehicle_command.param2 > 0.f)) {
+		} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
 
-			if (_control_mode_current == FW_POSCTRL_MODE_AUTO) {
-				_pos_sp_triplet.current.cruising_speed = vehicle_command.param2;
+			if ((static_cast<uint8_t>(vehicle_command.param1 + .5f) == vehicle_command_s::SPEED_TYPE_AIRSPEED)) {
+				if (vehicle_command.param2 > FLT_EPSILON) {	// param2 is an equivalent airspeed setpoint
+					if (_control_mode_current == FW_POSCTRL_MODE_AUTO) {
+						_pos_sp_triplet.current.cruising_speed = vehicle_command.param2;
 
-			} else if (_control_mode_current == FW_POSCTRL_MODE_MANUAL_ALTITUDE
-				   || _control_mode_current == FW_POSCTRL_MODE_MANUAL_POSITION) {
-				_commanded_airspeed_setpoint = vehicle_command.param2;
+					} else if (_control_mode_current == FW_POSCTRL_MODE_MANUAL_ALTITUDE
+						   || _control_mode_current == FW_POSCTRL_MODE_MANUAL_POSITION) {
+						_commanded_manual_airspeed_setpoint = vehicle_command.param2;
+					}
+
+				}
 			}
 
 		}
@@ -380,7 +376,7 @@ FixedwingPositionControl::vehicle_attitude_poll()
 		_body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az};
 		_body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz};
 
-		// update TECS load factor
+		// load factor due to banking
 		const float load_factor = 1.f / cosf(euler_angles(0));
 		_tecs.set_load_factor(load_factor);
 	}
@@ -406,23 +402,19 @@ FixedwingPositionControl::get_manual_airspeed_setpoint()
 					   (_manual_control_setpoint_for_airspeed * 2 - 1);
 		}
 
-	} else if (PX4_ISFINITE(_commanded_airspeed_setpoint)) {
-		altctrl_airspeed = _commanded_airspeed_setpoint;
+	} else if (PX4_ISFINITE(_commanded_manual_airspeed_setpoint)) {
+		altctrl_airspeed = _commanded_manual_airspeed_setpoint;
 	}
 
 	return altctrl_airspeed;
 }
 
 float
-FixedwingPositionControl::get_auto_airspeed_setpoint(const float control_interval, const float pos_sp_cruise_airspeed,
-		const Vector2f &ground_speed)
+FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint,
+		float calibrated_min_airspeed, const Vector2f &ground_speed)
 {
-	// overwrite internal setpoint (e.g. set prior through MAV_CMD_DO_CHANGE_SPEED) in case
-	// the current position_setpoint contains a valid airspeed setpoint
-	float airspeed_setpoint = _param_fw_airspd_trim.get();
-
-	if (PX4_ISFINITE(pos_sp_cruise_airspeed) && pos_sp_cruise_airspeed > FLT_EPSILON) {
-		airspeed_setpoint = pos_sp_cruise_airspeed;
+	if (!PX4_ISFINITE(calibrated_airspeed_setpoint) || calibrated_airspeed_setpoint <= FLT_EPSILON) {
+		calibrated_airspeed_setpoint = _param_fw_airspd_trim.get();
 	}
 
 	// Adapt cruise airspeed when otherwise the min groundspeed couldn't be maintained
@@ -436,42 +428,84 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const float control_interva
 		const float ground_speed_body = _body_velocity(0);
 
 		if (ground_speed_body < _param_fw_gnd_spd_min.get()) {
-			airspeed_setpoint += _param_fw_gnd_spd_min.get() - ground_speed_body;
+			calibrated_airspeed_setpoint += _param_fw_gnd_spd_min.get() - ground_speed_body;
 		}
 	}
 
-	float airspeed_min_adjusted = _param_fw_airspd_min.get();
+	float load_factor_from_bank_angle = 1.0f;
 
-	/*
-	 * Calculate accelerated stall airspeed factor from commanded bank angle and use it to increase minimum airspeed.
-	 *
-	 * Increase lift vector to balance additional weight in bank
-	 * cos(bank angle) = W/L = 1/n, n is the load factor
-	 *
-	 * lift is proportional to airspeed^2 so the increase in stall speed is Vsacc = Vs * sqrt(n)
-	 */
-
-	if (_airspeed_valid && PX4_ISFINITE(_att_sp.roll_body)) {
-		airspeed_min_adjusted = constrain(_param_fw_airspd_stall.get() / sqrtf(cosf(_att_sp.roll_body)),
-						  airspeed_min_adjusted, _param_fw_airspd_max.get());
+	if (PX4_ISFINITE(_att_sp.roll_body)) {
+		load_factor_from_bank_angle = 1.0f / cosf(_att_sp.roll_body);
 	}
 
-	// constrain setpoint
-	airspeed_setpoint = constrain(airspeed_setpoint, airspeed_min_adjusted, _param_fw_airspd_max.get());
+	float weight_ratio = 1.0f;
+
+	if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) {
+		weight_ratio = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO,
+					       MAX_WEIGHT_RATIO);
+	}
+
+	// Here we make sure that the set minimum airspeed is automatically adapted to the current load factor.
+	// The minimum airspeed is the controller limit (FW_AIRSPD_MIN, unless either in takeoff or landing) that should
+	// resemble the vehicles stall speed (CAS) with a 1g load plus some safety margin (as no controller tracks perfectly).
+	// Stall speed increases with the square root of the load factor: V_stall ~ sqrt(load_factor).
+	// The load_factor is composed of a term from the bank angle and a term from the weight ratio.
+	calibrated_min_airspeed *= sqrtf(load_factor_from_bank_angle * weight_ratio);
+
+	// Aditional option to increase the min airspeed setpoint based on wind estimate for more stability in higher winds.
+	if (_airspeed_valid && _wind_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) {
+		calibrated_min_airspeed = math::min(calibrated_min_airspeed + _param_fw_wind_arsp_sc.get() *
+						    _wind_vel.length(), _param_fw_airspd_max.get());
+	}
+
+	calibrated_airspeed_setpoint = constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed,
+				       _param_fw_airspd_max.get());
 
 	// initialize to current airspeed setpoint, also if previous setpoint is out of bounds to not apply slew rate in that case
-	const bool outside_of_limits = _airspeed_slew_rate_controller.getState() < airspeed_min_adjusted
-				       || _airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get();
+	const bool slewed_airspeed_outside_of_limits = _airspeed_slew_rate_controller.getState() < calibrated_min_airspeed
+			|| _airspeed_slew_rate_controller.getState() > _param_fw_airspd_max.get();
 
-	if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState()) || outside_of_limits) {
-		_airspeed_slew_rate_controller.setForcedValue(airspeed_setpoint);
+	if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState()) || slewed_airspeed_outside_of_limits) {
+		_airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint);
 
 	} else if (control_interval > FLT_EPSILON) {
 		// constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s
-		airspeed_setpoint = _airspeed_slew_rate_controller.update(airspeed_setpoint, control_interval);
+		calibrated_airspeed_setpoint = _airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval);
 	}
 
-	return airspeed_setpoint;
+	return calibrated_airspeed_setpoint;
+}
+
+void
+FixedwingPositionControl::update_cruise_mode(float pos_sp_cruising_speed)
+{
+	// APX4 custom function for cruise mode setting (Eco/Normal)
+
+	float airspeed_setpoint = _param_fw_airspd_trim.get();
+
+	if (PX4_ISFINITE(pos_sp_cruising_speed) && pos_sp_cruising_speed > FLT_EPSILON)  {
+		airspeed_setpoint = pos_sp_cruising_speed;
+	}
+
+	const float eco_altitude_amsl_min = max(_tecs.hgt_setpoint() - _param_fw_eco_alt_err_u.get(),
+						_local_pos.ref_alt + _param_fw_eco_alt_min.get());
+	const float eco_altitude_amsl_max = _tecs.hgt_setpoint() + _param_fw_eco_alt_err_o.get();
+
+	const bool within_safe_altitude_band = _current_altitude <= eco_altitude_amsl_max
+					       && _current_altitude >= eco_altitude_amsl_min;
+
+	const bool speed_check = _param_fw_eco_ad_thrld.get() > FLT_EPSILON && airspeed_setpoint < _param_fw_eco_ad_thrld.get();
+
+	const bool eco_checks = speed_check && within_safe_altitude_band
+				&& _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL;
+
+	// reset timer if one of the check fails
+	if (!eco_checks) {
+		_last_time_eco_checks_failed = hrt_absolute_time();
+	}
+
+	// set mode to Eco if conditions are met for 60s
+	_cruise_mode_current = hrt_elapsed_time(&_last_time_eco_checks_failed) > 60_s ? CRUISE_MODE_ECO : CRUISE_MODE_NORMAL;
 }
 
 void
@@ -495,6 +529,9 @@ FixedwingPositionControl::tecs_status_publish()
 	case TECS::ECL_TECS_MODE_CLIMBOUT:
 		t.mode = tecs_status_s::TECS_MODE_CLIMBOUT;
 		break;
+
+	case TECS::ECL_TECS_MODE_ECO:
+		t.mode = tecs_status_s::TECS_MODE_ECO;
 	}
 
 	t.altitude_sp = _tecs.hgt_setpoint();
@@ -533,6 +570,9 @@ FixedwingPositionControl::tecs_status_publish()
 
 	t.throttle_sp = _tecs.get_throttle_setpoint();
 	t.pitch_sp_rad = _tecs.get_pitch_setpoint();
+	t.throttle_trim = _tecs.get_throttle_trim();
+
+	t.flight_phase = _tecs.get_flight_phase(); // APX4 custom
 
 	t.timestamp = hrt_absolute_time();
 
@@ -610,28 +650,52 @@ FixedwingPositionControl::landing_status_publish()
 {
 	position_controller_landing_status_s pos_ctrl_landing_status = {};
 
-	pos_ctrl_landing_status.slope_angle_rad = _landingslope.landing_slope_angle_rad();
-	pos_ctrl_landing_status.horizontal_slope_displacement = _landingslope.horizontal_slope_displacement();
-	pos_ctrl_landing_status.flare_length = _landingslope.flare_length();
-
-	pos_ctrl_landing_status.abort_landing = _land_abort;
-
+	pos_ctrl_landing_status.lateral_touchdown_offset = _lateral_touchdown_position_offset;
+	pos_ctrl_landing_status.flaring = _flare_states.flaring;
+	pos_ctrl_landing_status.abort_status = _landing_abort_status;
 	pos_ctrl_landing_status.timestamp = hrt_absolute_time();
 
 	_pos_ctrl_landing_status_pub.publish(pos_ctrl_landing_status);
 }
 
 void
-FixedwingPositionControl::abort_landing(bool abort)
+FixedwingPositionControl::updateLandingAbortStatus(const uint8_t new_abort_status)
 {
 	// only announce changes
-	if (abort && !_land_abort) {
-		mavlink_log_critical(&_mavlink_log_pub, "Landing aborted\t");
-		// TODO: add reason
-		events::send(events::ID("fixedwing_position_control_land_aborted"), events::Log::Critical, "Landing aborted");
+	if (new_abort_status > 0 && _landing_abort_status != new_abort_status) {
+
+		switch (new_abort_status) {
+		case (position_controller_landing_status_s::ABORTED_BY_OPERATOR): {
+				mavlink_log_critical(&_mavlink_log_pub, "Landing aborted by operator\t");
+				events::send(events::ID("fixedwing_position_control_landing_abort_status_operator_abort"), events::Log::Critical,
+					     "Landing aborted by operator");
+				break;
+			}
+
+		case (position_controller_landing_status_s::TERRAIN_NOT_FOUND): {
+				mavlink_log_critical(&_mavlink_log_pub, "Landing aborted: terrain estimate not found\t");
+				events::send(events::ID("fixedwing_position_control_landing_abort_status_terrain_not_found"), events::Log::Critical,
+					     "Landing aborted: terrain measurement not found");
+				break;
+			}
+
+		case (position_controller_landing_status_s::TERRAIN_TIMEOUT): {
+				mavlink_log_critical(&_mavlink_log_pub, "Landing aborted: terrain estimate timed out\t");
+				events::send(events::ID("fixedwing_position_control_landing_abort_status_terrain_timeout"), events::Log::Critical,
+					     "Landing aborted: terrain estimate timed out");
+				break;
+			}
+
+		default: {
+				mavlink_log_critical(&_mavlink_log_pub, "Landing aborted: unknown criterion\t");
+				events::send(events::ID("fixedwing_position_control_landing_abort_status_unknown_criterion"), events::Log::Critical,
+					     "Landing aborted: unknown criterion");
+			}
+		}
 	}
 
-	_land_abort = abort;
+	_landing_abort_status = (new_abort_status >= position_controller_landing_status_s::UNKNOWN_ABORT_CRITERION) ?
+				position_controller_landing_status_s::UNKNOWN_ABORT_CRITERION : new_abort_status;
 	landing_status_publish();
 }
 
@@ -665,23 +729,9 @@ FixedwingPositionControl::get_waypoint_heading_distance(float heading, position_
 
 	waypoint_prev = temp_prev;
 	waypoint_prev.alt = _current_altitude;
-	waypoint_prev.valid = true;
 
 	waypoint_next = temp_next;
 	waypoint_next.alt = _current_altitude;
-	waypoint_next.valid = true;
-}
-
-float
-FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt)
-{
-	float terrain_alt = _local_pos.ref_alt - (_local_pos.dist_bottom + _local_pos.z);
-
-	if (PX4_ISFINITE(terrain_alt) && _local_pos.dist_bottom_valid) {
-		return terrain_alt;
-	}
-
-	return takeoff_alt;
 }
 
 float
@@ -715,7 +765,8 @@ FixedwingPositionControl::getManualHeightRateSetpoint()
 	} else if (_manual_control_setpoint_for_height_rate < - deadBand) {
 		/* pitching up */
 		float pitch = -(_manual_control_setpoint_for_height_rate + deadBand) / factor;
-		const float climb_rate_target = _param_climbrate_target.get();
+		const float climb_rate_target = (_cruise_mode_current == CRUISE_MODE_ECO) ? _param_climbrate_target_eco.get() :
+						_param_climbrate_target.get(); // APX4 custom
 
 		height_rate_setpoint = pitch * climb_rate_target;
 
@@ -724,21 +775,20 @@ FixedwingPositionControl::getManualHeightRateSetpoint()
 	return height_rate_setpoint;
 }
 
-bool
-FixedwingPositionControl::in_takeoff_situation()
+void
+FixedwingPositionControl::updateManualTakeoffStatus()
 {
-	// a VTOL does not need special takeoff handling
-	if (_vehicle_status.is_vtol) {
-		return false;
+	if (!_completed_manual_takeoff) {
+		const bool at_controllable_airspeed = _airspeed > _param_fw_airspd_min.get()
+						      || !_airspeed_valid;
+		const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+					 && _control_mode.flag_armed;
+		_completed_manual_takeoff = (!_landed && at_controllable_airspeed) || is_hovering;
 	}
-
-	// in air for < 10s
-	return (hrt_elapsed_time(&_time_went_in_air) < 10_s)
-	       && (_current_altitude <= _takeoff_ground_alt + _param_fw_clmbout_diff.get());
 }
 
 void
-FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid)
+FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
 {
 	/* only run position controller in fixed-wing mode and during transitions for VTOL */
 	if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.in_transition_mode) {
@@ -746,8 +796,12 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
 		return; // do not publish the setpoint
 	}
 
+	FW_POSCTRL_MODE commanded_position_control_mode = _control_mode_current;
+
+	_skipping_takeoff_detection = false;
+
 	if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
-	     _control_mode.flag_control_offboard_enabled) && pos_sp_curr_valid) {
+	     _control_mode.flag_control_offboard_enabled) && _position_setpoint_current_valid) {
 
 		if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 
@@ -759,6 +813,13 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
 
 			} else {
 				_control_mode_current = FW_POSCTRL_MODE_AUTO_TAKEOFF;
+
+				if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_TAKEOFF && !_landed) {
+					// skip takeoff detection when switching from any other mode, auto or manual,
+					// while already in air.
+					// TODO: find a better place for / way of doing this
+					_skipping_takeoff_detection = true;
+				}
 			}
 
 		} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
@@ -775,17 +836,22 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
 			_control_mode_current = FW_POSCTRL_MODE_AUTO;
 		}
 
-	} else if (_control_mode.flag_control_auto_enabled && _control_mode.flag_control_climb_rate_enabled
-		   && pos_sp_curr_valid) {
+	} else if (_control_mode.flag_control_auto_enabled
+		   && _control_mode.flag_control_climb_rate_enabled
+		   && _control_mode.flag_armed // only enter this modes if armed, as pure failsafe modes
+		   && !_control_mode.flag_control_position_enabled) {
 
-		// reset timer the first time we switch into this mode
-		if (_control_mode_current != FW_POSCTRL_MODE_AUTO_ALTITUDE && _control_mode_current != FW_POSCTRL_MODE_AUTO_CLIMBRATE) {
+		// failsafe modes engaged if position estimate is invalidated
+
+		if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE
+		    && commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE) {
+			// reset timer the first time we switch into this mode
 			_time_in_fixed_bank_loiter = now;
 		}
 
 		if (hrt_elapsed_time(&_time_in_fixed_bank_loiter) < (_param_nav_gpsf_lt.get() * 1_s)
 		    && !_vehicle_status.in_transition_mode) {
-			if (_control_mode_current != FW_POSCTRL_MODE_AUTO_ALTITUDE) {
+			if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_ALTITUDE) {
 				// Need to init because last loop iteration was in a different mode
 				mavlink_log_critical(&_mavlink_log_pub, "Start loiter with fixed bank angle.\t");
 				events::send(events::ID("fixedwing_position_control_fb_loiter"), events::Log::Critical,
@@ -795,7 +861,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
 			_control_mode_current = FW_POSCTRL_MODE_AUTO_ALTITUDE;
 
 		} else {
-			if (_control_mode_current != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) {
+			if (commanded_position_control_mode != FW_POSCTRL_MODE_AUTO_CLIMBRATE && !_vehicle_status.in_transition_mode) {
 				mavlink_log_critical(&_mavlink_log_pub, "Start descending.\t");
 				events::send(events::ID("fixedwing_position_control_descend"), events::Log::Critical, "Start descending");
 			}
@@ -805,7 +871,7 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, bool
 
 
 	} else if (_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) {
-		if (_control_mode_current != FW_POSCTRL_MODE_MANUAL_POSITION) {
+		if (commanded_position_control_mode != FW_POSCTRL_MODE_MANUAL_POSITION) {
 			/* Need to init because last loop iteration was in a different mode */
 			_hdg_hold_yaw = _yaw; // yaw is not controlled, so set setpoint to current yaw
 			_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
@@ -835,12 +901,18 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
 	if (!_was_in_air && !_landed) {
 		_was_in_air = true;
 		_time_went_in_air = now;
-		_takeoff_ground_alt = _current_altitude; // XXX: is this really a good idea?
+
+		_tecs.resetIntegrals();
+		_tecs.resetTrajectoryGenerators(_current_altitude);
 	}
 
 	/* reset flag when airplane landed */
 	if (_landed) {
 		_was_in_air = false;
+		_completed_manual_takeoff = false;
+
+		_tecs.resetIntegrals();
+		_tecs.resetTrajectoryGenerators(_current_altitude);
 	}
 }
 
@@ -854,9 +926,13 @@ FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_se
 
 		if (!PX4_ISFINITE(_transition_waypoint(0))) {
 			double lat_transition, lon_transition;
-			// create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track
-			// during the transition
-			waypoint_from_heading_and_distance(_current_latitude, _current_longitude, _yaw, HDG_HOLD_DIST_NEXT, &lat_transition,
+
+			// Create a virtual waypoint HDG_HOLD_DIST_NEXT meters in front of the vehicle which the L1 controller can track
+			// during the transition. Use the current yaw setpoint to determine the transition heading, as that one in turn
+			// is set to the transition heading by Navigator, or current yaw if setpoint is not valid.
+			const float transition_heading = PX4_ISFINITE(current_sp.yaw) ? current_sp.yaw : _yaw;
+			waypoint_from_heading_and_distance(_current_latitude, _current_longitude, transition_heading, HDG_HOLD_DIST_NEXT,
+							   &lat_transition,
 							   &lon_transition);
 
 			_transition_waypoint(0) = lat_transition;
@@ -879,6 +955,8 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 				       const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
 				       const position_setpoint_s &pos_sp_next)
 {
+	update_cruise_mode(pos_sp_curr.cruising_speed); // APX4 custom
+
 	position_setpoint_s current_sp = pos_sp_curr;
 	move_position_setpoint_for_vtol_transition(current_sp);
 
@@ -888,7 +966,15 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 
 	if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER
 	    || current_sp.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
-		publishOrbitStatus(current_sp);
+		// APX4 custom start
+		if (current_sp.loiter_pattern == position_setpoint_s::LOITER_TYPE_FIGUREEIGHT) {
+			publishFigureEightStatus(current_sp);
+
+		} else {
+			publishOrbitStatus(current_sp);
+		}
+
+		// APX4 custom end
 	}
 
 	const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
@@ -911,12 +997,31 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
 		control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp);
 		break;
 
+	// APX4 custom start
 	case position_setpoint_s::SETPOINT_TYPE_LOITER:
-		control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
+		if (current_sp.loiter_pattern == position_setpoint_s::LOITER_TYPE_FIGUREEIGHT) {
+			controlAutoFigureEight(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
+
+		} else {
+			control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
+
+		}
+
 		break;
 	}
 
-	if (was_circle_mode && !_l1_control.circle_mode()) {
+	/* reset loiter state */
+	if ((position_sp_type != position_setpoint_s::SETPOINT_TYPE_LOITER) ||
+	    ((position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER) &&
+	     (current_sp.loiter_pattern != position_setpoint_s::LOITER_TYPE_FIGUREEIGHT))) {
+		_figure_eight.resetPattern();
+	}
+
+	// APX4 custom end
+
+	const bool in_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
+
+	if (was_circle_mode && !in_circle_mode) {
 		/* just kicked out of loiter, reset roll integrals */
 		_att_sp.roll_reset_integral = true;
 	}
@@ -951,9 +1056,10 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
 				   radians(_param_fw_p_lim_max.get()),
 				   _param_fw_thr_min.get(),
 				   _param_fw_thr_max.get(),
-				   _param_fw_thr_cruise.get(),
 				   false,
-				   _param_fw_p_lim_min.get());
+				   _param_fw_p_lim_min.get(),
+				   _param_sinkrate_target.get(),
+				   _param_climbrate_target.get());
 
 	_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
 	_att_sp.yaw_body = 0.f;
@@ -985,9 +1091,10 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
 				   radians(_param_fw_p_lim_max.get()),
 				   _param_fw_thr_min.get(),
 				   _param_fw_thr_max.get(),
-				   _param_fw_thr_cruise.get(),
 				   false,
 				   _param_fw_p_lim_min.get(),
+				   _param_sinkrate_target.get(),
+				   _param_climbrate_target.get(),
 				   false,
 				   descend_rate);
 
@@ -1069,48 +1176,36 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 	/* current waypoint (the one currently heading for) */
 	curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
 
-	if (pos_sp_prev.valid) {
+	if (_position_setpoint_previous_valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 		prev_wp(0) = pos_sp_prev.lat;
 		prev_wp(1) = pos_sp_prev.lon;
 
 	} else {
-		/*
-			* No valid previous waypoint, go for the current wp.
-			* This is automatically handled by the L1 library.
-			*/
+		// No valid previous waypoint, go for the current wp.
+		// This is automatically handled by the L1/NPFG libraries.
 		prev_wp(0) = pos_sp_curr.lat;
 		prev_wp(1) = pos_sp_curr.lon;
 	}
 
 	float tecs_fw_thr_min;
 	float tecs_fw_thr_max;
-	float tecs_fw_mission_throttle;
 
-	float mission_throttle = _param_fw_thr_cruise.get();
-
-	if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) &&
-	    pos_sp_curr.cruising_throttle >= 0.0f) {
-		mission_throttle = pos_sp_curr.cruising_throttle;
-	}
-
-	if (mission_throttle < _param_fw_thr_min.get()) {
+	if (pos_sp_curr.gliding_enabled) {
 		/* enable gliding with this waypoint */
 		_tecs.set_speed_weight(2.0f);
 		tecs_fw_thr_min = 0.0;
 		tecs_fw_thr_max = 0.0;
-		tecs_fw_mission_throttle = 0.0;
 
 	} else {
 		tecs_fw_thr_min = _param_fw_thr_min.get();
 		tecs_fw_thr_max = _param_fw_thr_max.get();
-		tecs_fw_mission_throttle = mission_throttle;
 	}
 
 	// waypoint is a plain navigation waypoint
 	float position_sp_alt = pos_sp_curr.alt;
 
 	// Altitude first order hold (FOH)
-	if (pos_sp_prev.valid && PX4_ISFINITE(pos_sp_prev.alt) &&
+	if (_position_setpoint_previous_valid &&
 	    ((pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_POSITION) ||
 	     (pos_sp_prev.type == position_setpoint_s::SETPOINT_TYPE_LOITER))
 	   ) {
@@ -1141,7 +1236,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 		}
 	}
 
-	float target_airspeed = get_auto_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, ground_speed);
+	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
+				_param_fw_airspd_min.get(), ground_speed);
 
 	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
 	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
@@ -1174,7 +1270,10 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
 	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
-	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
+
+	// APX4 custom spoiler depending on flight phase
+	_att_sp.apply_spoilers = _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_DESCEND ?
+				 vehicle_attitude_setpoint_s::SPOILERS_DESCEND : vehicle_attitude_setpoint_s::SPOILERS_OFF;
 
 	tecs_update_pitch_throttle(control_interval,
 				   position_sp_alt,
@@ -1183,9 +1282,10 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
 				   radians(_param_fw_p_lim_max.get()),
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
-				   tecs_fw_mission_throttle,
 				   false,
-				   radians(_param_fw_p_lim_min.get()));
+				   radians(_param_fw_p_lim_min.get()),
+				   _param_sinkrate_target.get(),
+				   _param_climbrate_target.get());
 }
 
 void
@@ -1194,26 +1294,16 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 {
 	float tecs_fw_thr_min;
 	float tecs_fw_thr_max;
-	float tecs_fw_mission_throttle;
 
-	float mission_throttle = _param_fw_thr_cruise.get();
-
-	if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) &&
-	    pos_sp_curr.cruising_throttle >= 0.0f) {
-		mission_throttle = pos_sp_curr.cruising_throttle;
-	}
-
-	if (mission_throttle < _param_fw_thr_min.get()) {
+	if (pos_sp_curr.gliding_enabled) {
 		/* enable gliding with this waypoint */
 		_tecs.set_speed_weight(2.0f);
 		tecs_fw_thr_min = 0.0;
 		tecs_fw_thr_max = 0.0;
-		tecs_fw_mission_throttle = 0.0;
 
 	} else {
 		tecs_fw_thr_min = _param_fw_thr_min.get();
 		tecs_fw_thr_max = _param_fw_thr_max.get();
-		tecs_fw_mission_throttle = mission_throttle;
 	}
 
 	// waypoint is a plain navigation waypoint
@@ -1224,7 +1314,8 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 	Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy};
 	_target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0)));
 
-	float target_airspeed = get_auto_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, ground_speed);
+	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
+				_param_fw_airspd_min.get(), ground_speed);
 
 	if (_param_fw_use_npfg.get()) {
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
@@ -1241,7 +1332,9 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 	_att_sp.yaw_body = _yaw;
 
 	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
-	_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
+	// APX4 custom spoiler depending on flight phase
+	_att_sp.apply_spoilers = _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_DESCEND ?
+				 vehicle_attitude_setpoint_s::SPOILERS_DESCEND : vehicle_attitude_setpoint_s::SPOILERS_OFF;
 
 	tecs_update_pitch_throttle(control_interval,
 				   position_sp_alt,
@@ -1250,9 +1343,10 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
 				   radians(_param_fw_p_lim_max.get()),
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
-				   tecs_fw_mission_throttle,
 				   false,
 				   radians(_param_fw_p_lim_min.get()),
+				   _param_sinkrate_target.get(),
+				   (_cruise_mode_current == CRUISE_MODE_ECO) ? _param_climbrate_target_eco.get() : _param_climbrate_target.get(),
 				   tecs_status_s::TECS_MODE_NORMAL,
 				   pos_sp_curr.vz);
 }
@@ -1268,15 +1362,13 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 	/* current waypoint (the one currently heading for) */
 	curr_wp = Vector2d(pos_sp_curr.lat, pos_sp_curr.lon);
 
-	if (pos_sp_prev.valid) {
+	if (_position_setpoint_previous_valid && pos_sp_prev.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
 		prev_wp(0) = pos_sp_prev.lat;
 		prev_wp(1) = pos_sp_prev.lon;
 
 	} else {
-		/*
-			* No valid previous waypoint, go for the current wp.
-			* This is automatically handled by the L1 library.
-			*/
+		// No valid previous waypoint, go for the current wp.
+		// This is automatically handled by the L1/NPFG libraries.
 		prev_wp(0) = pos_sp_curr.lat;
 		prev_wp(1) = pos_sp_curr.lon;
 	}
@@ -1291,40 +1383,28 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 
 	float tecs_fw_thr_min;
 	float tecs_fw_thr_max;
-	float tecs_fw_mission_throttle;
 
-	float mission_throttle = _param_fw_thr_cruise.get();
-
-	if (PX4_ISFINITE(pos_sp_curr.cruising_throttle) &&
-	    pos_sp_curr.cruising_throttle >= 0.0f) {
-		mission_throttle = pos_sp_curr.cruising_throttle;
-	}
-
-	if (mission_throttle < _param_fw_thr_min.get()) {
+	if (pos_sp_curr.gliding_enabled) {
 		/* enable gliding with this waypoint */
 		_tecs.set_speed_weight(2.0f);
 		tecs_fw_thr_min = 0.0;
 		tecs_fw_thr_max = 0.0;
-		tecs_fw_mission_throttle = 0.0;
 
 	} else {
 		tecs_fw_thr_min = _param_fw_thr_min.get();
 		tecs_fw_thr_max = _param_fw_thr_max.get();
-		tecs_fw_mission_throttle = mission_throttle;
 	}
 
 	/* waypoint is a loiter waypoint */
 	float loiter_radius = pos_sp_curr.loiter_radius;
-	uint8_t loiter_direction = pos_sp_curr.loiter_direction;
 
 	if (fabsf(pos_sp_curr.loiter_radius) < FLT_EPSILON) {
 		loiter_radius = _param_nav_loiter_rad.get();
-		loiter_direction = signNoZero(loiter_radius);
 	}
 
 	const bool in_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
 
-	if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid
+	if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid
 	    && in_circle_mode && _param_fw_lnd_earlycfg.get()) {
 		// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
 		// landing airspeed and potentially tighter altitude control) already such that we don't
@@ -1336,10 +1416,12 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 
 	} else {
 		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
-		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
+		_att_sp.apply_spoilers = _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_DESCEND ?
+					 vehicle_attitude_setpoint_s::SPOILERS_DESCEND : vehicle_attitude_setpoint_s::SPOILERS_OFF;
 	}
 
-	float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_sp, ground_speed);
+	float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_sp, _param_fw_airspd_min.get(),
+				ground_speed);
 
 	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
 	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
@@ -1347,13 +1429,15 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 	if (_param_fw_use_npfg.get()) {
 		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-		_npfg.navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, loiter_direction, get_nav_speed_2d(ground_speed),
+		_npfg.navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
+				     get_nav_speed_2d(ground_speed),
 				     _wind_vel);
 		_att_sp.roll_body = _npfg.getRollSetpoint();
 		target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 	} else {
-		_l1_control.navigate_loiter(curr_wp_local, curr_pos_local, loiter_radius, loiter_direction,
+		_l1_control.navigate_loiter(curr_wp_local, curr_pos_local, loiter_radius,
+					    pos_sp_curr.loiter_direction_counter_clockwise,
 					    get_nav_speed_2d(ground_speed));
 		_att_sp.roll_body = _l1_control.get_roll_setpoint();
 	}
@@ -1362,15 +1446,10 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 
 	float alt_sp = pos_sp_curr.alt;
 
-	if (in_takeoff_situation()) {
-		alt_sp = max(alt_sp, _takeoff_ground_alt + _param_fw_clmbout_diff.get());
-		_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-5.0f), radians(5.0f));
-	}
-
-	if (_land_abort) {
+	if (_landing_abort_status) {
 		if (pos_sp_curr.alt - _current_altitude  < _param_fw_clmbout_diff.get()) {
 			// aborted landing complete, normal loiter over landing point
-			abort_landing(false);
+			updateLandingAbortStatus(position_controller_landing_status_s::NOT_ABORTED);
 
 		} else {
 			// continue straight until vehicle has sufficient altitude
@@ -1387,108 +1466,206 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 				   radians(_param_fw_p_lim_max.get()),
 				   tecs_fw_thr_min,
 				   tecs_fw_thr_max,
-				   tecs_fw_mission_throttle,
 				   false,
-				   radians(_param_fw_p_lim_min.get()));
+				   radians(_param_fw_p_lim_min.get()),
+				   _param_sinkrate_target.get(),
+				   (_cruise_mode_current == CRUISE_MODE_ECO) ? _param_climbrate_target_eco.get() : _param_climbrate_target.get());
+}
+
+void
+FixedwingPositionControl::controlAutoFigureEight(const float control_interval, const Vector2d &curr_pos,
+		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
+{
+	// APX4 custom mode that performs figures of 8 instead of circular orbits
+
+	// airspeed settings
+	float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
+				_param_fw_airspd_min.get(), ground_speed);
+
+	// Lateral Control
+
+	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
+
+	FigureEight::FigureEightPatternParameters params;
+	params.center_pos_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
+	params.loiter_direction_counter_clockwise = pos_sp_curr.loiter_direction_counter_clockwise;
+	params.loiter_minor_radius = pos_sp_curr.loiter_minor_radius;
+	params.loiter_orientation = pos_sp_curr.loiter_orientation;
+	params.loiter_radius = pos_sp_curr.loiter_radius;
+
+	_figure_eight.initializePattern(curr_pos_local, get_nav_speed_2d(ground_speed), params);
+
+	// Apply control
+	_figure_eight.updateSetpoint(curr_pos_local, get_nav_speed_2d(ground_speed), params, target_airspeed,
+				     _param_fw_use_npfg.get());
+	_att_sp.roll_body = _figure_eight.getRollSetpoint();
+	target_airspeed = _figure_eight.getAirspeedSetpoint();
+
+	// TECS
+	float tecs_fw_thr_min;
+	float tecs_fw_thr_max;
+
+	if (pos_sp_curr.gliding_enabled) {
+		/* enable gliding with this waypoint */
+		_tecs.set_speed_weight(2.0f);
+		tecs_fw_thr_min = 0.0;
+		tecs_fw_thr_max = 0.0;
+
+	} else {
+		tecs_fw_thr_min = _param_fw_thr_min.get();
+		tecs_fw_thr_max = _param_fw_thr_max.get();
+	}
+
+	tecs_update_pitch_throttle(control_interval,
+				   pos_sp_curr.alt,
+				   target_airspeed,
+				   radians(_param_fw_p_lim_min.get()),
+				   radians(_param_fw_p_lim_max.get()),
+				   tecs_fw_thr_min,
+				   tecs_fw_thr_max,
+				   false,
+				   radians(_param_fw_p_lim_min.get()),
+				   _param_sinkrate_target.get(),
+				   (_cruise_mode_current == CRUISE_MODE_ECO) ? _param_climbrate_target_eco.get() : _param_climbrate_target.get());
+
+	// Yaw
+	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 }
 
 void
 FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float control_interval,
-		const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
-		const position_setpoint_s &pos_sp_curr)
+		const Vector2d &global_position, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
 {
-	/* current waypoint (the one currently heading for) */
-	Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
-	Vector2d prev_wp{0, 0}; /* previous waypoint */
-	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
-
-	if (pos_sp_prev.valid) {
-		prev_wp(0) = pos_sp_prev.lat;
-		prev_wp(1) = pos_sp_prev.lon;
-
-	} else {
-		/*
-		 * No valid previous waypoint, go for the current wp.
-		 * This is automatically handled by the L1 library.
-		 */
-		prev_wp(0) = pos_sp_curr.lat;
-		prev_wp(1) = pos_sp_curr.lon;
-	}
-
-	// continuously reset launch detection and runway takeoff until armed
 	if (!_control_mode.flag_armed) {
-		_launchDetector.reset();
-		_launch_detection_state = LAUNCHDETECTION_RES_NONE;
-		_launch_detection_notify = 0;
+		reset_takeoff_state();
 	}
 
+	// for now taking current position setpoint altitude as clearance altitude. this is the altitude we need to
+	// clear all occlusions in the takeoff path
+	const float clearance_altitude_amsl = pos_sp_curr.alt;
+
+	// set the altitude to something above the clearance altitude to ensure the vehicle climbs past the value
+	// (navigator will accept the takeoff as complete once crossing the clearance altitude)
+	const float altitude_setpoint_amsl = clearance_altitude_amsl + kClearanceAltitudeBuffer;
+
+	Vector2f local_2D_position{_local_pos.x, _local_pos.y};
+
 	if (_runway_takeoff.runwayTakeoffEnabled()) {
 		if (!_runway_takeoff.isInitialized()) {
-			_runway_takeoff.init(now, _yaw, _current_latitude, _current_longitude);
+			_runway_takeoff.init(now, _yaw, global_position);
 
-			/* need this already before takeoff is detected
-			 * doesn't matter if it gets reset when takeoff is detected eventually */
 			_takeoff_ground_alt = _current_altitude;
 
 			mavlink_log_info(&_mavlink_log_pub, "Takeoff on runway\t");
 			events::send(events::ID("fixedwing_position_control_takeoff"), events::Log::Info, "Takeoff on runway");
 		}
 
-		float terrain_alt = get_terrain_altitude_takeoff(_takeoff_ground_alt);
+		if (_skipping_takeoff_detection) {
+			_runway_takeoff.forceSetFlyState();
+		}
 
-		// update runway takeoff helper
-		_runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt,
-				       _current_latitude, _current_longitude, &_mavlink_log_pub);
+		const float takeoff_airspeed = (_param_fw_tko_airspd.get() > FLT_EPSILON) ? _param_fw_tko_airspd.get() :
+					       _param_fw_airspd_min.get();
 
-		float target_airspeed = get_auto_airspeed_setpoint(control_interval,
-					_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed);
+		_runway_takeoff.update(now, takeoff_airspeed, _airspeed, _current_altitude - _takeoff_ground_alt,
+				       clearance_altitude_amsl - _takeoff_ground_alt,
+				       &_mavlink_log_pub);
 
-		/*
-		 * Update navigation: _runway_takeoff returns the start WP according to mode and phase.
-		 * If we use the navigator heading or not is decided later.
-		 */
-		Vector2f prev_wp_local = _global_local_proj_ref.project(_runway_takeoff.getStartWP()(0),
-					 _runway_takeoff.getStartWP()(1));
+		float adjusted_min_airspeed = _param_fw_airspd_min.get();
+
+		if (takeoff_airspeed < _param_fw_airspd_min.get()) {
+			// adjust underspeed detection bounds for takeoff airspeed
+			_tecs.set_equivalent_airspeed_min(takeoff_airspeed);
+			adjusted_min_airspeed = takeoff_airspeed;
+		}
+
+		float target_airspeed = adapt_airspeed_setpoint(control_interval, takeoff_airspeed, adjusted_min_airspeed,
+					ground_speed);
+
+		// yaw control is disabled once in "taking off" state
+		_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
+
+		// XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface.
+		if (_param_rwto_nudge.get()) {
+			_att_sp.yaw_sp_move_rate = _manual_control_setpoint.r;
+		}
+
+		// tune up the lateral position control guidance when on the ground
+		if (_att_sp.fw_control_yaw) {
+			_npfg.setPeriod(_param_rwto_l1_period.get());
+			_l1_control.set_l1_period(_param_rwto_l1_period.get());
+
+		}
+
+		const Vector2f start_pos_local = _global_local_proj_ref.project(_runway_takeoff.getStartPosition()(0),
+						 _runway_takeoff.getStartPosition()(1));
+		const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
+
+		// the bearing from runway start to the takeoff waypoint is followed until the clearance altitude is exceeded
+		const Vector2f takeoff_bearing_vector = calculateTakeoffBearingVector(start_pos_local, takeoff_waypoint_local);
 
 		if (_param_fw_use_npfg.get()) {
 			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-			// NOTE: current waypoint is passed twice to trigger the "point following" logic -- TODO: create
-			// point following navigation interface instead of this hack.
-			_npfg.navigateWaypoints(curr_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+			_npfg.navigatePathTangent(local_2D_position, start_pos_local, takeoff_bearing_vector, ground_speed,
+						  _wind_vel, 0.0f);
+
 			_att_sp.roll_body = _runway_takeoff.getRoll(_npfg.getRollSetpoint());
+
 			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
+			// use npfg's bearing to commanded course, controlled via yaw angle while on runway
+			const float bearing = _npfg.getBearing();
+
+			// heading hold mode will override this bearing setpoint
+			_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
+
 		} else {
-			_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
-			_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.get_roll_setpoint());
+			// make a fake waypoint in the direction of the takeoff bearing
+			const Vector2f virtual_waypoint = start_pos_local + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT;
+
+			_l1_control.navigate_waypoints(start_pos_local, virtual_waypoint, local_2D_position, ground_speed);
+
+			const float l1_roll_setpoint = _l1_control.get_roll_setpoint();
+			_att_sp.roll_body = _runway_takeoff.getRoll(l1_roll_setpoint);
+
+			// use l1's nav bearing to command course, controlled via yaw angle while on runway
+			const float bearing = _l1_control.nav_bearing();
+
+			// heading hold mode will override this bearing setpoint
+			_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
 		}
 
-		_att_sp.yaw_body = _runway_takeoff.getYaw(_yaw);
-
-
 		// update tecs
-		const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());
+		const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get()));
+		const float pitch_min = _runway_takeoff.getMinPitch(math::radians(_takeoff_pitch_min.get()),
+					math::radians(_param_fw_p_lim_min.get()));
+
+		if (_runway_takeoff.resetIntegrators()) {
+			// reset integrals except yaw (which also counts for the wheel controller)
+			_att_sp.roll_reset_integral = true;
+			_att_sp.pitch_reset_integral = true;
+
+			// throttle is open loop anyway during ground roll, no need to wind up the integrator
+			_tecs.resetIntegrals();
+		}
 
 		tecs_update_pitch_throttle(control_interval,
-					   pos_sp_curr.alt,
+					   altitude_setpoint_amsl,
 					   target_airspeed,
-					   radians(_param_fw_p_lim_min.get()),
-					   radians(takeoff_pitch_max_deg),
+					   pitch_min,
+					   pitch_max,
 					   _param_fw_thr_min.get(),
-					   _param_fw_thr_max.get(), // XXX should we also set runway_takeoff_throttle here?
-					   _param_fw_thr_cruise.get(),
-					   _runway_takeoff.climbout(),
-					   radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _param_fw_p_lim_min.get())));
+					   _param_fw_thr_max.get(),
+					   false,
+					   pitch_min,
+					   _param_sinkrate_target.get(),
+					   _param_fw_t_clmb_max.get());
+
+		_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
 
-		// assign values
-		_att_sp.fw_control_yaw = _runway_takeoff.controlYaw();
 		_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
-
-		// reset integrals except yaw (which also counts for the wheel controller)
-		_att_sp.roll_reset_integral = _runway_takeoff.resetIntegrators();
-		_att_sp.pitch_reset_integral = _runway_takeoff.resetIntegrators();
+		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust());
 
 		// apply flaps for takeoff according to the corresponding scale factor set via FW_FLAPS_TO_SCL
 		_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_TAKEOFF;
@@ -1496,17 +1673,17 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 	} else {
 		/* Perform launch detection */
-		if (_launchDetector.launchDetectionEnabled() &&
+		if (!_skipping_takeoff_detection && _launchDetector.launchDetectionEnabled() &&
 		    _launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
 
 			if (_control_mode.flag_armed) {
 				/* Perform launch detection */
 
 				/* Inform user that launchdetection is running every 4s */
-				if ((now - _launch_detection_notify) > 4_s) {
+				if ((now - _last_time_launch_detection_notified) > 4_s) {
 					mavlink_log_critical(&_mavlink_log_pub, "Launch detection running\t");
 					events::send(events::ID("fixedwing_position_control_launch_detection"), events::Log::Info, "Launch detection running");
-					_launch_detection_notify = now;
+					_last_time_launch_detection_notified = now;
 				}
 
 				/* Detect launch using body X (forward) acceleration */
@@ -1521,23 +1698,38 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			_launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
 		}
 
+		if (!_launch_detected && _launch_detection_state != LAUNCHDETECTION_RES_NONE) {
+			_launch_detected = true;
+			_launch_global_position = global_position;
+			_takeoff_ground_alt = _current_altitude;
+		}
+
+		const Vector2f launch_local_position = _global_local_proj_ref.project(_launch_global_position(0),
+						       _launch_global_position(1));
+		const Vector2f takeoff_waypoint_local = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
+
+		// the bearing from launch to the takeoff waypoint is followed until the clearance altitude is exceeded
+		const Vector2f takeoff_bearing_vector = calculateTakeoffBearingVector(launch_local_position, takeoff_waypoint_local);
+
 		/* Set control values depending on the detection state */
 		if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) {
 			/* Launch has been detected, hence we have to control the plane. */
 
-			float target_airspeed = get_auto_airspeed_setpoint(control_interval, _param_fw_airspd_trim.get(), ground_speed);
-
-			Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
+			float target_airspeed = adapt_airspeed_setpoint(control_interval, _param_fw_airspd_trim.get(),
+						_param_fw_airspd_min.get(), ground_speed);
 
 			if (_param_fw_use_npfg.get()) {
 				_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 				_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-				_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
+				_npfg.navigatePathTangent(local_2D_position, launch_local_position, takeoff_bearing_vector, ground_speed, _wind_vel,
+							  0.0f);
 				_att_sp.roll_body = _npfg.getRollSetpoint();
 				target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 
 			} else {
-				_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
+				// make a fake waypoint in the direction of the takeoff bearing
+				const Vector2f virtual_waypoint = launch_local_position + takeoff_bearing_vector * HDG_HOLD_DIST_NEXT;
+				_l1_control.navigate_waypoints(launch_local_position, virtual_waypoint, local_2D_position, ground_speed);
 				_att_sp.roll_body = _l1_control.get_roll_setpoint();
 			}
 
@@ -1551,41 +1743,47 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 				takeoff_throttle = _param_fw_thr_idle.get();
 			}
 
-			/* select maximum pitch: the launchdetector may impose another limit for the pitch
-			 * depending on the state of the launch */
-			const float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_param_fw_p_lim_max.get());
-			const float altitude_error = pos_sp_curr.alt - _current_altitude;
+			if (_current_altitude < clearance_altitude_amsl) {
+				// select maximum pitch: the launchdetector may impose another limit for the pitch
+				// depending on the state of the launch
+				const float takeoff_pitch_max_deg = _launchDetector.getPitchMax(_param_fw_p_lim_max.get());
 
-			/* apply minimum pitch and limit roll if target altitude is not within climbout_diff meters */
-			if (_param_fw_clmbout_diff.get() > 0.0f && altitude_error > _param_fw_clmbout_diff.get()) {
-				/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
 				tecs_update_pitch_throttle(control_interval,
-							   pos_sp_curr.alt,
+							   altitude_setpoint_amsl,
 							   target_airspeed,
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(takeoff_pitch_max_deg),
 							   _param_fw_thr_min.get(),
 							   takeoff_throttle,
-							   _param_fw_thr_cruise.get(),
 							   true,
-							   radians(_takeoff_pitch_min.get()));
-
-				/* limit roll motion to ensure enough lift */
-				_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));
+							   radians(_takeoff_pitch_min.get()),
+							   _param_sinkrate_target.get(),
+							   _param_climbrate_target.get());
 
 			} else {
 				tecs_update_pitch_throttle(control_interval,
-							   pos_sp_curr.alt,
+							   altitude_setpoint_amsl,
 							   target_airspeed,
 							   radians(_param_fw_p_lim_min.get()),
 							   radians(_param_fw_p_lim_max.get()),
 							   _param_fw_thr_min.get(),
 							   takeoff_throttle,
-							   _param_fw_thr_cruise.get(),
 							   false,
-							   radians(_param_fw_p_lim_min.get()));
+							   radians(_param_fw_p_lim_min.get()),
+							   _param_sinkrate_target.get(),
+							   _param_climbrate_target.get());
 			}
 
+			if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
+				// explicitly set idle throttle until motors are enabled
+				_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
+
+			} else {
+				_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
+			}
+
+			_att_sp.pitch_body = get_tecs_pitch();
+
 		} else {
 			/* Tell the attitude controller to stop integrating while we are waiting
 			 * for the launch */
@@ -1595,6 +1793,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 			/* Set default roll and pitch setpoints during detection phase */
 			_att_sp.roll_body = 0.0f;
+			_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
 			_att_sp.pitch_body = radians(_takeoff_pitch_min.get());
 		}
 
@@ -1602,27 +1801,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 		_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 	}
 
-	if (_launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS && !_runway_takeoff.runwayTakeoffEnabled()) {
-
-		/* making sure again that the correct thrust is used,
-		 * without depending on library calls for safety reasons.
-		   the pre-takeoff throttle and the idle throttle normally map to the same parameter. */
-		_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
-
-	} else if (_runway_takeoff.runwayTakeoffEnabled()) {
-
-		_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(now, get_tecs_thrust());
-
-	} else {
-		/* Copy thrust and pitch values from tecs */
-		// when we are landed state we want the motor to spin at idle speed
-		_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
-	}
-
-	// only use TECS pitch setpoint if launch has not been detected and runway takeoff is not enabled
-	if (!(_launch_detection_state == LAUNCHDETECTION_RES_NONE || _runway_takeoff.runwayTakeoffEnabled())) {
-		_att_sp.pitch_body = get_tecs_pitch();
-	}
+	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, _takeoff_ground_alt);
 
 	if (!_vehicle_status.in_transition_to_fw) {
 		publishLocalPositionSetpoint(pos_sp_curr);
@@ -1631,342 +1810,238 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 
 void
 FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const float control_interval,
-		const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
-		const position_setpoint_s &pos_sp_curr)
+		const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
 {
 	// Enable tighter altitude control for landings
 	_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
 
-	/* current waypoint (the one currently heading for) */
-	Vector2d curr_wp(pos_sp_curr.lat, pos_sp_curr.lon);
-	Vector2d prev_wp{0, 0}; /* previous waypoint */
+	const Vector2f local_position{_local_pos.x, _local_pos.y};
+	Vector2f local_land_point = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
 
-	if (pos_sp_prev.valid) {
-		prev_wp(0) = pos_sp_prev.lat;
-		prev_wp(1) = pos_sp_prev.lon;
+	initializeAutoLanding(now, pos_sp_prev, pos_sp_curr, local_position, local_land_point);
+
+	// touchdown may get nudged by manual inputs
+	local_land_point = calculateTouchdownPosition(control_interval, local_land_point);
+	const Vector2f landing_approach_vector = calculateLandingApproachVector();
+
+	const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
+	float adjusted_min_airspeed = _param_fw_airspd_min.get();
+
+	if (airspeed_land < _param_fw_airspd_min.get()) {
+		// adjust underspeed detection bounds for landing airspeed
+		_tecs.set_equivalent_airspeed_min(airspeed_land);
+		adjusted_min_airspeed = airspeed_land;
+	}
+
+	float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed,
+				ground_speed);
+
+	// calculate the altitude setpoint based on the landing glide slope
+	const float along_track_dist_to_touchdown = -landing_approach_vector.unit_or_zero().dot(
+				local_position - local_land_point);
+	const float glide_slope = _landing_approach_entrance_rel_alt / _landing_approach_entrance_offset_vector.norm();
+
+	// NOTE: this relative altitude can go below zero, this is intentional. in the case the vehicle is tracking the glide
+	// slope at an offset above the track, making the altitude setpoint constant on intersection with terrain causes
+	// an increase in throttle (to slow down and smoothly intersect the flattened altitude setpoint), which is undesirable
+	// directly before the flare. instead, we keep the steady state behavior, and let the flare get triggered once at
+	// the desired altitude
+	const float glide_slope_rel_alt = math::min(along_track_dist_to_touchdown * glide_slope,
+					  _landing_approach_entrance_rel_alt);
+
+	const float terrain_alt = getLandingTerrainAltitudeEstimate(now, pos_sp_curr.alt);
+	const float glide_slope_reference_alt = (_param_fw_lnd_useter.get() ==
+						TerrainEstimateUseOnLanding::kFollowTerrainRelativeLandingGlideSlope) ? terrain_alt : pos_sp_curr.alt;
+
+	float altitude_setpoint;
+
+	if (_current_altitude > glide_slope_reference_alt + glide_slope_rel_alt) {
+		// descend to the glide slope
+		altitude_setpoint = glide_slope_reference_alt + glide_slope_rel_alt;
 
 	} else {
-		/*
-		 * No valid previous waypoint, go for the current wp.
-		 * This is automatically handled by the L1 library.
-		 */
-		prev_wp(0) = pos_sp_curr.lat;
-		prev_wp(1) = pos_sp_curr.lon;
+		// continue horizontally
+		altitude_setpoint = _current_altitude;
 	}
 
-	// save time at which we started landing and reset abort_landing
-	if (_time_started_landing == 0) {
-		reset_landing_state();
-		_time_started_landing = now;
-	}
+	// flare at the maximum of the altitude determined by the time before touchdown and a minimum flare altitude
+	const float flare_rel_alt = math::max(_param_fw_lnd_fl_time.get() * _local_pos.vz, _param_fw_lnd_flalt.get());
 
-	const float bearing_airplane_currwp = get_bearing_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1),
-					      (double)curr_wp(0), (double)curr_wp(1));
+	// the terrain estimate (if enabled) is always used to determine the flaring altitude
+	if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) {
+		// flare and land with minimal speed
 
-	float bearing_lastwp_currwp = bearing_airplane_currwp;
-
-	if (pos_sp_prev.valid) {
-		bearing_lastwp_currwp = get_bearing_to_next_waypoint((double)prev_wp(0), (double)prev_wp(1), (double)curr_wp(0),
-					(double)curr_wp(1));
-	}
-
-	/* Horizontal landing control */
-	/* switch to heading hold for the last meters, continue heading hold after */
-	float wp_distance = get_distance_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1), (double)curr_wp(0),
-			    (double)curr_wp(1));
-
-	/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
-	float wp_distance_save = wp_distance;
-
-	if (fabsf(wrap_pi(bearing_airplane_currwp - bearing_lastwp_currwp)) >= radians(90.0f)) {
-		wp_distance_save = 0.0f;
-	}
-
-	// create virtual waypoint which is on the desired flight path but
-	// some distance behind landing waypoint. This will make sure that the plane
-	// will always follow the desired flight path even if we get close or past
-	// the landing waypoint
-	if (pos_sp_prev.valid) {
-		double lat = pos_sp_curr.lat;
-		double lon = pos_sp_curr.lon;
-
-		create_waypoint_from_line_and_dist(pos_sp_curr.lat, pos_sp_curr.lon,
-						   pos_sp_prev.lat, pos_sp_prev.lon, -1000.0f, &lat, &lon);
-
-		curr_wp(0) = lat;
-		curr_wp(1) = lon;
-	}
-
-	Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
-	Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
-	Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0),
-				 prev_wp(1));
-
-	// we want the plane to keep tracking the desired flight path until we start flaring
-	// if we go into heading hold mode earlier then we risk to be pushed away from the runway by cross winds
-	if ((_param_fw_lnd_hhdist.get() > 0.0f) && !_land_noreturn_horizontal &&
-	    ((wp_distance < _param_fw_lnd_hhdist.get()) || _land_noreturn_vertical)) {
-
-		if (pos_sp_prev.valid) {
-			/* heading hold, along the line connecting this and the last waypoint */
-			_target_bearing = bearing_lastwp_currwp;
-
-		} else {
-			_target_bearing = _yaw;
+		// flaring is a "point of no return"
+		if (!_flare_states.flaring) {
+			_flare_states.flaring = true;
+			_flare_states.start_time = now;
+			_flare_states.initial_height_rate_setpoint = _tecs.hgt_rate_setpoint();
+			_flare_states.initial_throttle_setpoint = _att_sp.thrust_body[0];
+			mavlink_log_info(&_mavlink_log_pub, "Landing, flaring\t");
+			events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info,
+				     "Landing, flaring");
 		}
 
-		_land_noreturn_horizontal = true;
-		mavlink_log_info(&_mavlink_log_pub, "Landing, heading hold\t");
-		events::send(events::ID("fixedwing_position_control_landing"), events::Log::Info, "Landing, heading hold");
-	}
+		// ramp in flare limits and setpoints with the flare time, command a soft touchdown
+		const float seconds_since_flare_start = hrt_elapsed_time(&_flare_states.start_time) / float(1_s);
+		const float flare_ramp_interpolator = math::constrain(seconds_since_flare_start / _param_fw_lnd_fl_time.get(), 0.0f,
+						      1.0f);
 
-	/* Vertical landing control */
-	/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
+		/* lateral guidance first, because npfg will adjust the airspeed setpoint if necessary */
 
-	// default to no terrain estimation, just use landing waypoint altitude
-	float terrain_alt = pos_sp_curr.alt;
+		// tune up the lateral position control guidance when on the ground
+		const float ground_roll_npfg_period = flare_ramp_interpolator * _param_rwto_l1_period.get() +
+						      (1.0f - flare_ramp_interpolator) * _param_npfg_period.get();
+		const float ground_roll_l1_period = flare_ramp_interpolator * _param_rwto_l1_period.get() +
+						    (1.0f - flare_ramp_interpolator) * _param_fw_l1_period.get();
+		_npfg.setPeriod(ground_roll_npfg_period);
+		_l1_control.set_l1_period(ground_roll_l1_period);
 
-	if (_param_fw_lnd_useter.get() == 1) {
-		if (_local_pos.dist_bottom_valid) {
-			// all good, have valid terrain altitude
-			float terrain_vpos = _local_pos.dist_bottom + _local_pos.z;
-			terrain_alt = (_local_pos.ref_alt - terrain_vpos);
-			_last_valid_terrain_alt_estimate = terrain_alt;
-			_last_time_terrain_alt_was_valid = now;
+		const Vector2f local_approach_entrance = local_land_point - landing_approach_vector;
 
-		} else if (_last_time_terrain_alt_was_valid == 0) {
-			// we have started landing phase but don't have valid terrain
-			// wait for some time, maybe we will soon get a valid estimate
-			// until then just use the altitude of the landing waypoint
-			if ((now - _time_started_landing) < 10_s) {
-				terrain_alt = pos_sp_curr.alt;
-
-			} else {
-				// still no valid terrain, abort landing
-				terrain_alt = pos_sp_curr.alt;
-				abort_landing(true);
-			}
-
-		} else if ((!_local_pos.dist_bottom_valid && (now - _last_time_terrain_alt_was_valid) < TERRAIN_ALT_TIMEOUT)
-			   || _land_noreturn_vertical) {
-			// use previous terrain estimate for some time and hope to recover
-			// if we are already flaring (land_noreturn_vertical) then just
-			//  go with the old estimate
-			terrain_alt = _last_valid_terrain_alt_estimate;
-
-		} else {
-			// terrain alt was not valid for long time, abort landing
-			terrain_alt = _last_valid_terrain_alt_estimate;
-			abort_landing(true);
-		}
-	}
-
-	/* Check if we should start flaring with a vertical and a
-	 * horizontal limit (with some tolerance)
-	 * The horizontal limit is only applied when we are in front of the wp
-	 */
-	if ((_current_altitude < terrain_alt + _landingslope.flare_relative_alt()) ||
-	    _land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
-
-		/* land with minimal speed */
-
-		/* force TECS to only control speed with pitch, altitude is only implicitly controlled now */
-		// _tecs.set_speed_weight(2.0f);
-
-		/* kill the throttle if param requests it */
-		float throttle_max = _param_fw_thr_max.get();
-
-		if (((_current_altitude < terrain_alt + _landingslope.motor_lim_relative_alt()) &&
-		     (wp_distance_save < _landingslope.flare_length() + 5.0f)) || // Only kill throttle when close to WP
-		    _land_motor_lim) {
-			throttle_max = min(throttle_max, _param_fw_thr_lnd_max.get());
-
-			if (!_land_motor_lim) {
-				_land_motor_lim  = true;
-				mavlink_log_info(&_mavlink_log_pub, "Landing, limiting throttle\t");
-				events::send(events::ID("fixedwing_position_control_landing_limit_throttle"), events::Log::Info,
-					     "Landing, limiting throttle");
-			}
-		}
-
-		float flare_curve_alt_rel = _landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp,
-					    bearing_airplane_currwp);
-
-		/* avoid climbout */
-		if ((_flare_curve_alt_rel_last < flare_curve_alt_rel && _land_noreturn_vertical) || _land_stayonground) {
-			flare_curve_alt_rel = 0.0f; // stay on ground
-			_land_stayonground = true;
-		}
-
-		const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
-		float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_land, ground_speed);
-
-		const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;
-
-		/* lateral guidance */
 		if (_param_fw_use_npfg.get()) {
 			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-
-			if (_land_noreturn_horizontal) {
-				// heading hold
-				_npfg.navigateHeading(_target_bearing, ground_speed, _wind_vel);
-
-			} else {
-				// normal navigation
-				_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
-			}
-
-			target_airspeed = _npfg.getAirspeedRef() /  _eas2tas;
+			_npfg.navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
+			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 			_att_sp.roll_body = _npfg.getRollSetpoint();
 
+			// use npfg's bearing to commanded course, controlled via yaw angle while on runway
+			_att_sp.yaw_body = _npfg.getBearing();
+
 		} else {
-			if (_land_noreturn_horizontal) {
-				// heading hold
-				_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
+			// make a fake waypoint beyond the land point in the direction of the landing approach bearing
+			// (always HDG_HOLD_DIST_NEXT meters in front of the aircraft's progress along the landing approach vector)
 
-			} else {
-				// normal navigation
-				_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
-			}
+			const float along_track_distance_from_entrance = landing_approach_vector.unit_or_zero().dot(
+						local_position - local_approach_entrance);
 
+			const Vector2f virtual_waypoint = local_approach_entrance + (along_track_distance_from_entrance + HDG_HOLD_DIST_NEXT) *
+							  landing_approach_vector.unit_or_zero();
+
+			_l1_control.navigate_waypoints(local_approach_entrance, virtual_waypoint, local_position, ground_speed);
 			_att_sp.roll_body = _l1_control.get_roll_setpoint();
+
+			// use l1's nav bearing to command course, controlled via yaw angle while on runway
+			_att_sp.yaw_body = _l1_control.nav_bearing();
 		}
 
-		if (_land_noreturn_horizontal) {
-			/* limit roll motion to prevent wings from touching the ground first */
-			_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-10.0f), radians(10.0f));
-		}
+		/* longitudinal guidance */
 
-		/* enable direct yaw control using rudder/wheel */
-		if (_land_noreturn_horizontal) {
-			_att_sp.yaw_body = _target_bearing;
-			_att_sp.fw_control_yaw = true;
+		const float height_rate_setpoint = flare_ramp_interpolator * (-_param_fw_lnd_fl_sink.get()) +
+						   (1.0f - flare_ramp_interpolator) * _flare_states.initial_height_rate_setpoint;
 
-		} else {
-			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
-		}
+		const float pitch_min_rad = flare_ramp_interpolator * radians(_param_fw_lnd_fl_pmin.get()) +
+					    (1.0f - flare_ramp_interpolator) * radians(_param_fw_p_lim_min.get());
+		const float pitch_max_rad = flare_ramp_interpolator * radians(_param_fw_lnd_fl_pmax.get()) +
+					    (1.0f - flare_ramp_interpolator) * radians(_param_fw_p_lim_max.get());
+
+		// idle throttle may be >0 for internal combustion engines
+		// normally set to zero for electric motors
+		const float throttle_max = flare_ramp_interpolator * _param_fw_thr_idle.get() + (1.0f - flare_ramp_interpolator) *
+					   _param_fw_thr_max.get();
 
 		tecs_update_pitch_throttle(control_interval,
-					   terrain_alt + flare_curve_alt_rel,
+					   altitude_setpoint,
 					   target_airspeed,
-					   radians(_param_fw_lnd_fl_pmin.get()),
-					   radians(_param_fw_lnd_fl_pmax.get()),
-					   0.0f,
+					   pitch_min_rad,
+					   pitch_max_rad,
+					   _param_fw_thr_idle.get(),
 					   throttle_max,
-					   throttle_land,
 					   false,
-					   _land_motor_lim ? radians(_param_fw_lnd_fl_pmin.get()) : radians(_param_fw_p_lim_min.get()),
-					   true);
+					   pitch_min_rad,
+					   _param_sinkrate_target.get(),
+					   _param_climbrate_target.get(),
+					   true,
+					   height_rate_setpoint);
 
-		if (!_land_noreturn_vertical) {
-			// just started with the flaring phase
-			_flare_pitch_sp = radians(_param_fw_psp_off.get());
-			_flare_height = _current_altitude - terrain_alt;
-			mavlink_log_info(&_mavlink_log_pub, "Landing, flaring\t");
-			events::send(events::ID("fixedwing_position_control_landing_flaring"), events::Log::Info, "Landing, flaring");
-			_land_noreturn_vertical = true;
+		/* set the attitude and throttle commands */
 
-		} else {
-			if (_local_pos.vz > 0.1f) {
-				_flare_pitch_sp = radians(_param_fw_lnd_fl_pmin.get()) *
-						  constrain((_flare_height - (_current_altitude - terrain_alt)) / _flare_height, 0.0f, 1.0f);
-			}
+		// TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle
+		_att_sp.pitch_body = get_tecs_pitch();
 
-			// otherwise continue using previous _flare_pitch_sp
+		// enable direct yaw control using rudder/wheel
+		_att_sp.fw_control_yaw = true;
+
+		// XXX: hacky way to pass through manual nose-wheel incrementing. need to clean this interface.
+		if (_param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled) {
+			_att_sp.yaw_sp_move_rate = _manual_control_setpoint.r;
 		}
 
-		_att_sp.pitch_body = _flare_pitch_sp;
-		_flare_curve_alt_rel_last = flare_curve_alt_rel;
+		// blend the height rate controlled throttle setpoints with initial throttle setting over the flare
+		// ramp time period to maintain throttle command continuity when switching from altitude to height rate
+		// control
+		const float blended_throttle = flare_ramp_interpolator * get_tecs_thrust() + (1.0f - flare_ramp_interpolator) *
+					       _flare_states.initial_throttle_setpoint;
+
+		_att_sp.thrust_body[0] = blended_throttle;
 
 	} else {
 
-		/* intersect glide slope:
-		 * minimize speed to approach speed
-		 * if current position is higher than the slope follow the glide slope (sink to the
-		 * glide slope)
-		 * also if the system captures the slope it should stay
-		 * on the slope (bool land_onslope)
-		 * if current position is below the slope continue at previous wp altitude
-		 * until the intersection with slope
-		 * */
-
-		float altitude_desired = terrain_alt;
-
-		const float landing_slope_alt_rel_desired = _landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance,
-				bearing_lastwp_currwp, bearing_airplane_currwp);
-
-		if (_current_altitude > terrain_alt + landing_slope_alt_rel_desired || _land_onslope) {
-			/* stay on slope */
-			altitude_desired = terrain_alt + landing_slope_alt_rel_desired;
-
-			if (!_land_onslope) {
-				mavlink_log_info(&_mavlink_log_pub, "Landing, on slope\t");
-				events::send(events::ID("fixedwing_position_control_landing_on_slope"), events::Log::Info, "Landing, on slope");
-				_land_onslope = true;
-			}
-
-		} else {
-			/* continue horizontally */
-			if (pos_sp_prev.valid) {
-				altitude_desired = pos_sp_prev.alt;
-
-			} else {
-				altitude_desired = _current_altitude;
-			}
-		}
-
-		const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
-		float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_approach, ground_speed);
+		// follow the glide slope
 
 		/* lateral guidance */
+
+		const Vector2f local_approach_entrance = local_land_point - landing_approach_vector;
+
 		if (_param_fw_use_npfg.get()) {
 			_npfg.setAirspeedNom(target_airspeed * _eas2tas);
 			_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
-
-			if (_land_noreturn_horizontal) {
-				// heading hold
-				_npfg.navigateHeading(_target_bearing, ground_speed, _wind_vel);
-
-			} else {
-				// normal navigation
-				_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
-			}
-
+			_npfg.navigateWaypoints(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
 			target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
 			_att_sp.roll_body = _npfg.getRollSetpoint();
 
 		} else {
-			if (_land_noreturn_horizontal) {
-				// heading hold
-				_l1_control.navigate_heading(_target_bearing, _yaw, ground_speed);
+			// make a fake waypoint beyond the land point in the direction of the landing approach bearing
+			// (always HDG_HOLD_DIST_NEXT meters in front of the aircraft's progress along the landing approach vector)
 
-			} else {
-				// normal navigation
-				_l1_control.navigate_waypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed);
-			}
+			const float along_track_distance_from_entrance = landing_approach_vector.unit_or_zero().dot(
+						local_position - local_approach_entrance);
 
+			const Vector2f virtual_waypoint = local_approach_entrance + (along_track_distance_from_entrance + HDG_HOLD_DIST_NEXT) *
+							  landing_approach_vector.unit_or_zero();
+
+			_l1_control.navigate_waypoints(local_approach_entrance, virtual_waypoint, local_position, ground_speed);
 			_att_sp.roll_body = _l1_control.get_roll_setpoint();
 		}
 
-		_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
+		/* longitudinal guidance */
+
+		// open the desired max sink rate to encompass the glide slope if within the aircraft's performance limits
+		// x/sqrt(x^2+1) = sin(arctan(x))
+		const float glide_slope_sink_rate = airspeed_land * glide_slope / sqrtf(glide_slope * glide_slope + 1.0f);
+		const float desired_max_sinkrate = math::min(math::max(glide_slope_sink_rate, _param_sinkrate_target.get()),
+						   _param_fw_t_sink_max.get());
 
 		tecs_update_pitch_throttle(control_interval,
-					   altitude_desired,
+					   altitude_setpoint,
 					   target_airspeed,
 					   radians(_param_fw_p_lim_min.get()),
 					   radians(_param_fw_p_lim_max.get()),
 					   _param_fw_thr_min.get(),
 					   _param_fw_thr_max.get(),
-					   _param_fw_thr_cruise.get(),
 					   false,
-					   radians(_param_fw_p_lim_min.get()));
+					   radians(_param_fw_p_lim_min.get()),
+					   desired_max_sinkrate,
+					   _param_climbrate_target.get());
+
+		/* set the attitude and throttle commands */
+
+		// TECS has authority (though constrained) over pitch during flare, throttle is killed
 		_att_sp.pitch_body = get_tecs_pitch();
+
+		// yaw is not controlled in nominal flight
+		_att_sp.yaw_body = _yaw;
+
+		// enable direct yaw control using rudder/wheel
+		_att_sp.fw_control_yaw = false;
+
+		_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_idle.get() : get_tecs_thrust();
 	}
 
-	/* Copy thrust and pitch values from tecs */
-	// when we are landed state we want the motor to spin at idle speed
-	_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
+	_tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get()); // reset after TECS calculation
+
+	_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
 
 	// Apply flaps and spoilers for landing. Amount of deflection is handled in the FW attitdue controller
 	_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_LAND;
@@ -1975,67 +2050,50 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const flo
 	if (!_vehicle_status.in_transition_to_fw) {
 		publishLocalPositionSetpoint(pos_sp_curr);
 	}
+
+	landing_status_publish();
 }
 
 void
 FixedwingPositionControl::control_manual_altitude(const float control_interval, const Vector2d &curr_pos,
 		const Vector2f &ground_speed)
 {
-	/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
+	updateManualTakeoffStatus();
 
-	/* Get demanded airspeed */
-	float altctrl_airspeed = get_manual_airspeed_setpoint();
+	const float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(),
+					     _param_fw_airspd_min.get(), ground_speed);
+	const float height_rate_sp = getManualHeightRateSetpoint();
 
-	// if we assume that user is taking off then help by demanding altitude setpoint well above ground
-	// and set limit to pitch angle to prevent steering into ground
-	// this will only affect planes and not VTOL
-	float pitch_limit_min = _param_fw_p_lim_min.get();
-	float height_rate_sp = NAN;
-	float altitude_sp_amsl = _current_altitude;
+	// TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are
+	// just passed launch
+	const float min_pitch = (_completed_manual_takeoff) ? radians(_param_fw_p_lim_min.get()) :
+				MIN_PITCH_DURING_MANUAL_TAKEOFF;
 
-	if (in_takeoff_situation()) {
-		// if we assume that user is taking off then help by demanding altitude setpoint well above ground
-		// and set limit to pitch angle to prevent steering into ground
-		// this will only affect planes and not VTOL
-		altitude_sp_amsl = _takeoff_ground_alt + _param_fw_clmbout_diff.get();
-		pitch_limit_min = radians(10.0f);
-
-	} else {
-		height_rate_sp = getManualHeightRateSetpoint();
-	}
-
-	/* throttle limiting */
 	float throttle_max = _param_fw_thr_max.get();
 
+	// enable the operator to kill the throttle on ground
 	if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
 		throttle_max = 0.0f;
 	}
 
 	tecs_update_pitch_throttle(control_interval,
-				   altitude_sp_amsl,
-				   altctrl_airspeed,
-				   radians(_param_fw_p_lim_min.get()),
+				   _current_altitude,
+				   calibrated_airspeed_sp,
+				   min_pitch,
 				   radians(_param_fw_p_lim_max.get()),
 				   _param_fw_thr_min.get(),
 				   throttle_max,
-				   _param_fw_thr_cruise.get(),
 				   false,
-				   pitch_limit_min,
+				   min_pitch,
+				   _param_sinkrate_target.get(),
+				   _param_climbrate_target.get(),
 				   false,
 				   height_rate_sp);
 
 	_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_r_lim.get());
 	_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 
-	/* Copy thrust and pitch values from tecs */
-	if (_landed) {
-		// when we are landed state we want the motor to spin at idle speed
-		_att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), throttle_max);
-
-	} else {
-		_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
-	}
-
+	_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
 	_att_sp.pitch_body = get_tecs_pitch();
 
 	// In Manual modes flaps and spoilers are directly controlled in FW Attitude controller and not passed
@@ -2048,33 +2106,24 @@ void
 FixedwingPositionControl::control_manual_position(const float control_interval, const Vector2d &curr_pos,
 		const Vector2f &ground_speed)
 {
-	// if we assume that user is taking off then help by demanding altitude setpoint well above ground
-	// and set limit to pitch angle to prevent steering into ground
-	// this will only affect planes and not VTOL
-	float pitch_limit_min = _param_fw_p_lim_min.get();
-	float height_rate_sp = NAN;
-	float altitude_sp_amsl = _current_altitude;
+	updateManualTakeoffStatus();
 
-	if (in_takeoff_situation()) {
-		// if we assume that user is taking off then help by demanding altitude setpoint well above ground
-		// and set limit to pitch angle to prevent steering into ground
-		// this will only affect planes and not VTOL
-		altitude_sp_amsl = _takeoff_ground_alt + _param_fw_clmbout_diff.get();
-		pitch_limit_min = radians(10.0f);
+	float calibrated_airspeed_sp = adapt_airspeed_setpoint(control_interval, get_manual_airspeed_setpoint(),
+				       _param_fw_airspd_min.get(), ground_speed);
+	const float height_rate_sp = getManualHeightRateSetpoint();
 
-	} else {
-		height_rate_sp = getManualHeightRateSetpoint();
-	}
+	// TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are
+	// just passed launch
+	const float min_pitch = (_completed_manual_takeoff) ? radians(_param_fw_p_lim_min.get()) :
+				MIN_PITCH_DURING_MANUAL_TAKEOFF;
 
-	/* throttle limiting */
 	float throttle_max = _param_fw_thr_max.get();
 
+	// enable the operator to kill the throttle on ground
 	if (_landed && (fabsf(_manual_control_setpoint_for_airspeed) < THROTTLE_THRESH)) {
 		throttle_max = 0.0f;
 	}
 
-	float target_airspeed = get_manual_airspeed_setpoint();
-
 	/* heading control */
 	if (fabsf(_manual_control_setpoint.y) < HDG_HOLD_MAN_INPUT_THRESH &&
 	    fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) {
@@ -2088,7 +2137,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 		/* user tries to do a takeoff in heading hold mode, reset the yaw setpoint on every iteration
 			to make sure the plane does not start rolling
 		*/
-		if (in_takeoff_situation()) {
+		if (!_completed_manual_takeoff) {
 			_hdg_hold_enabled = false;
 			_yaw_lock_engaged = true;
 		}
@@ -2120,11 +2169,11 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 						 prev_wp(1));
 
 			if (_param_fw_use_npfg.get()) {
-				_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+				_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
 				_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
 				_npfg.navigateWaypoints(prev_wp_local, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
 				_att_sp.roll_body = _npfg.getRollSetpoint();
-				target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
+				calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas;
 
 			} else {
 				/* populate l1 control setpoint */
@@ -2133,24 +2182,20 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 			}
 
 			_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
-
-			if (in_takeoff_situation()) {
-				/* limit roll motion to ensure enough lift */
-				_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));
-			}
 		}
 	}
 
 	tecs_update_pitch_throttle(control_interval,
-				   altitude_sp_amsl,
-				   target_airspeed,
-				   radians(_param_fw_p_lim_min.get()),
+				   _current_altitude,
+				   calibrated_airspeed_sp,
+				   min_pitch,
 				   radians(_param_fw_p_lim_max.get()),
 				   _param_fw_thr_min.get(),
 				   throttle_max,
-				   _param_fw_thr_cruise.get(),
 				   false,
-				   pitch_limit_min,
+				   min_pitch,
+				   _param_sinkrate_target.get(),
+				   _param_climbrate_target.get(),
 				   false,
 				   height_rate_sp);
 
@@ -2173,15 +2218,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 		_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
 	}
 
-	/* Copy thrust and pitch values from tecs */
-	if (_landed) {
-		// when we are landed state we want the motor to spin at idle speed
-		_att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), throttle_max);
-
-	} else {
-		_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
-	}
-
+	_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
 	_att_sp.pitch_body = get_tecs_pitch();
 
 	// In Manual modes flaps and spoilers are directly controlled in FW Attitude controller and not passed
@@ -2193,7 +2230,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
 float
 FixedwingPositionControl::get_tecs_pitch()
 {
-	if (_is_tecs_running) {
+	if (_tecs_is_running) {
 		return _tecs.get_pitch_setpoint() + radians(_param_fw_psp_off.get());
 	}
 
@@ -2204,7 +2241,7 @@ FixedwingPositionControl::get_tecs_pitch()
 float
 FixedwingPositionControl::get_tecs_thrust()
 {
-	if (_is_tecs_running) {
+	if (_tecs_is_running) {
 		return min(_tecs.get_throttle_setpoint(), 1.f);
 	}
 
@@ -2281,7 +2318,7 @@ FixedwingPositionControl::Run()
 		}
 
 		if (_control_mode.flag_control_offboard_enabled) {
-			vehicle_local_position_setpoint_s trajectory_setpoint;
+			trajectory_setpoint_s trajectory_setpoint;
 
 			if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
 				bool valid_setpoint = false;
@@ -2297,31 +2334,32 @@ FixedwingPositionControl::Run()
 				_pos_sp_triplet.current.lon = static_cast<double>(NAN);
 				_pos_sp_triplet.current.alt = NAN;
 
-				if (PX4_ISFINITE(trajectory_setpoint.x) && PX4_ISFINITE(trajectory_setpoint.y) && PX4_ISFINITE(trajectory_setpoint.z)) {
+				if (PX4_ISFINITE(trajectory_setpoint.position[0]) && PX4_ISFINITE(trajectory_setpoint.position[1])
+				    && PX4_ISFINITE(trajectory_setpoint.position[2])) {
 					if (_global_local_proj_ref.isInitialized()) {
 						double lat;
 						double lon;
-						_global_local_proj_ref.reproject(trajectory_setpoint.x, trajectory_setpoint.y, lat, lon);
+						_global_local_proj_ref.reproject(trajectory_setpoint.position[0], trajectory_setpoint.position[1], lat, lon);
 						valid_setpoint = true;
 						_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 						_pos_sp_triplet.current.lat = lat;
 						_pos_sp_triplet.current.lon = lon;
-						_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z;
+						_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.position[2];
 					}
 
 				}
 
-				if (PX4_ISFINITE(trajectory_setpoint.vx) && PX4_ISFINITE(trajectory_setpoint.vx)
-				    && PX4_ISFINITE(trajectory_setpoint.vz)) {
+				if (PX4_ISFINITE(trajectory_setpoint.velocity[0]) && PX4_ISFINITE(trajectory_setpoint.velocity[1])
+				    && PX4_ISFINITE(trajectory_setpoint.velocity[2])) {
 					valid_setpoint = true;
 					_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
-					_pos_sp_triplet.current.vx = trajectory_setpoint.vx;
-					_pos_sp_triplet.current.vy = trajectory_setpoint.vy;
-					_pos_sp_triplet.current.vz = trajectory_setpoint.vz;
+					_pos_sp_triplet.current.vx = trajectory_setpoint.velocity[0];
+					_pos_sp_triplet.current.vy = trajectory_setpoint.velocity[1];
+					_pos_sp_triplet.current.vz = trajectory_setpoint.velocity[2];
 
 					if (PX4_ISFINITE(trajectory_setpoint.acceleration[0]) && PX4_ISFINITE(trajectory_setpoint.acceleration[1])
 					    && PX4_ISFINITE(trajectory_setpoint.acceleration[2])) {
-						Vector2f velocity_sp_2d(trajectory_setpoint.vx, trajectory_setpoint.vy);
+						Vector2f velocity_sp_2d(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1]);
 						Vector2f acceleration_sp_2d(trajectory_setpoint.acceleration[0], trajectory_setpoint.acceleration[1]);
 						Vector2f acceleration_normal = acceleration_sp_2d - acceleration_sp_2d.dot(velocity_sp_2d) *
 									       velocity_sp_2d.normalized();
@@ -2344,6 +2382,19 @@ FixedwingPositionControl::Run()
 
 		} else {
 			if (_pos_sp_triplet_sub.update(&_pos_sp_triplet)) {
+
+				_position_setpoint_previous_valid = PX4_ISFINITE(_pos_sp_triplet.previous.lat)
+								    && PX4_ISFINITE(_pos_sp_triplet.previous.lon)
+								    && PX4_ISFINITE(_pos_sp_triplet.previous.alt);
+
+				_position_setpoint_current_valid = PX4_ISFINITE(_pos_sp_triplet.current.lat)
+								   && PX4_ISFINITE(_pos_sp_triplet.current.lon)
+								   && PX4_ISFINITE(_pos_sp_triplet.current.alt);
+
+				_position_setpoint_next_valid = PX4_ISFINITE(_pos_sp_triplet.next.lat)
+								&& PX4_ISFINITE(_pos_sp_triplet.next.lon)
+								&& PX4_ISFINITE(_pos_sp_triplet.next.alt);
+
 				// reset the altitude foh (first order hold) logic
 				_min_current_sp_distance_xy = FLT_MAX;
 			}
@@ -2356,6 +2407,12 @@ FixedwingPositionControl::Run()
 		vehicle_control_mode_poll();
 		wind_poll();
 
+		vehicle_air_data_s air_data;
+
+		if (_vehicle_air_data_sub.update(&air_data)) {
+			_air_density = PX4_ISFINITE(air_data.rho) ? air_data.rho : _air_density;
+		}
+
 		if (_vehicle_land_detected_sub.updated()) {
 			vehicle_land_detected_s vehicle_land_detected;
 
@@ -2369,7 +2426,15 @@ FixedwingPositionControl::Run()
 		Vector2d curr_pos(_current_latitude, _current_longitude);
 		Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
 
-		set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid);
+		set_control_mode_current(_local_pos.timestamp);
+
+		// APX4 custom
+		if (_control_mode_current != FW_POSCTRL_MODE_AUTO) {
+			_last_time_eco_checks_failed = _local_pos.timestamp;
+			_cruise_mode_current = CRUISE_MODE_NORMAL;
+		}
+
+		// APX4 custom end
 
 		update_in_air_states(_local_pos.timestamp);
 
@@ -2385,6 +2450,10 @@ FixedwingPositionControl::Run()
 		_tecs.set_speed_weight(_param_fw_t_spdweight.get());
 		_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
 
+		// restore lateral-directional guidance parameters (changed in takeoff mode)
+		_npfg.setPeriod(_param_npfg_period.get());
+		_l1_control.set_l1_period(_param_fw_l1_period.get());
+
 		_att_sp.roll_reset_integral = false;
 		_att_sp.pitch_reset_integral = false;
 		_att_sp.yaw_reset_integral = false;
@@ -2392,6 +2461,17 @@ FixedwingPositionControl::Run()
 		// by default we don't want yaw to be contoller directly with rudder
 		_att_sp.fw_control_yaw = false;
 
+		// default to zero - is used (IN A HACKY WAY) to pass direct nose wheel steering via yaw stick to the actuators during auto takeoff
+		_att_sp.yaw_sp_move_rate = 0.0f;
+
+		if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING) {
+			reset_landing_state();
+		}
+
+		if (_control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF) {
+			reset_takeoff_state();
+		}
+
 		switch (_control_mode_current) {
 		case FW_POSCTRL_MODE_AUTO: {
 				control_auto(control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
@@ -2410,14 +2490,13 @@ FixedwingPositionControl::Run()
 			}
 
 		case FW_POSCTRL_MODE_AUTO_LANDING: {
-				control_auto_landing(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
+				control_auto_landing(_local_pos.timestamp, control_interval, ground_speed, _pos_sp_triplet.previous,
 						     _pos_sp_triplet.current);
 				break;
 			}
 
 		case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
-				control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
-						     _pos_sp_triplet.current);
+				control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
 				break;
 			}
 
@@ -2432,12 +2511,6 @@ FixedwingPositionControl::Run()
 			}
 
 		case FW_POSCTRL_MODE_OTHER: {
-				/* reset landing and takeoff state */
-				if (!_last_manual) {
-					reset_landing_state();
-					reset_takeoff_state();
-				}
-
 				_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());
 
 				_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
@@ -2450,14 +2523,6 @@ FixedwingPositionControl::Run()
 
 		}
 
-		if (_control_mode_current != FW_POSCTRL_MODE_AUTO_LANDING) {
-			reset_landing_state();
-		}
-
-		if (_control_mode_current != FW_POSCTRL_MODE_AUTO_TAKEOFF) {
-			reset_takeoff_state();
-		}
-
 		if (_control_mode_current != FW_POSCTRL_MODE_OTHER) {
 
 			if (_control_mode.flag_control_manual_enabled) {
@@ -2488,7 +2553,6 @@ FixedwingPositionControl::Run()
 			}
 
 			_l1_control.reset_has_guidance_updated();
-			_last_manual = !_control_mode.flag_control_position_enabled;
 		}
 
 		perf_end(_loop_perf);
@@ -2496,20 +2560,17 @@ FixedwingPositionControl::Run()
 }
 
 void
-FixedwingPositionControl::reset_takeoff_state(bool force)
+FixedwingPositionControl::reset_takeoff_state()
 {
-	// only reset takeoff if !armed or just landed
-	if (!_control_mode.flag_armed || (_was_in_air && _landed) || force) {
+	_runway_takeoff.reset();
 
-		_runway_takeoff.reset();
+	_launchDetector.reset();
+	_launch_detection_state = LAUNCHDETECTION_RES_NONE;
+	_last_time_launch_detection_notified = 0;
 
-		_launchDetector.reset();
-		_launch_detection_state = LAUNCHDETECTION_RES_NONE;
-		_launch_detection_notify = 0;
+	_launch_detected = false;
 
-	} else {
-		_launch_detection_state = LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS;
-	}
+	_takeoff_ground_alt = _current_altitude;
 }
 
 void
@@ -2517,19 +2578,16 @@ FixedwingPositionControl::reset_landing_state()
 {
 	_time_started_landing = 0;
 
-	// reset terrain estimation relevant values
+	_flare_states = FlareStates{};
+
+	_lateral_touchdown_position_offset = 0.0f;
+
 	_last_time_terrain_alt_was_valid = 0;
 
-	_land_noreturn_horizontal = false;
-	_land_noreturn_vertical = false;
-	_land_stayonground = false;
-	_land_motor_lim = false;
-	_land_onslope = false;
-
 	// reset abort land, unless loitering after an abort
-	if (_land_abort && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER)) {
+	if (_landing_abort_status && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER)) {
 
-		abort_landing(false);
+		updateLandingAbortStatus(position_controller_landing_status_s::NOT_ABORTED);
 	}
 }
 
@@ -2562,21 +2620,42 @@ FixedwingPositionControl::get_nav_speed_2d(const Vector2f &ground_speed)
 	return nav_speed_2d;
 }
 
+float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min,
+		float throttle_max)
+{
+	float weight_ratio = 1.0f;
+
+	if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) {
+		weight_ratio = math::constrain(_param_weight_gross.get() / _param_weight_base.get(), MIN_WEIGHT_RATIO,
+					       MAX_WEIGHT_RATIO);
+	}
+
+	float air_density_throttle_scale = 1.0f;
+
+	if (PX4_ISFINITE(_air_density)) {
+		// scale throttle as a function of sqrt(rho0/rho)
+		const float eas2tas = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / _air_density);
+		const float eas2tas_at_5000m_amsl = sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / AIR_DENSITY_STANDARD_ATMOS_5000_AMSL);
+		air_density_throttle_scale = constrain(eas2tas, 1.f, eas2tas_at_5000m_amsl);
+	}
+
+	// compensate trim throttle for both weight and air density
+	return math::constrain(throttle_trim * sqrtf(weight_ratio) * air_density_throttle_scale, throttle_min, throttle_max);
+}
+
 void
 FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
-		float pitch_min_rad, float pitch_max_rad,
-		float throttle_min, float throttle_max, float throttle_cruise,
-		bool climbout_mode, float climbout_pitch_min_rad,
+		float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max, bool climbout_mode,
+		float climbout_pitch_min_rad, const float desired_max_sinkrate, const float desired_max_climbrate,
 		bool disable_underspeed_detection, float hgt_rate_sp)
 {
-	// do not run TECS if we are not in air
-	bool run_tecs = !_landed;
+	_tecs_is_running = true;
 
 	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
 	// (it should also not run during VTOL blending because airspeed is too low still)
 	if (_vehicle_status.is_vtol) {
 		if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) {
-			run_tecs = false;
+			_tecs_is_running = false;
 		}
 
 		if (_vehicle_status.in_transition_mode) {
@@ -2609,9 +2688,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		}
 	}
 
-	_is_tecs_running = run_tecs;
-
-	if (!run_tecs) {
+	if (!_tecs_is_running) {
 		// next time we run TECS we should reinitialize states
 		_reinitialize_tecs = true;
 		return;
@@ -2625,32 +2702,20 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	/* No underspeed protection in landing mode */
 	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
 
-	/* tell TECS to update its state, but let it know when it cannot actually control the plane */
-	bool in_air_alt_control = (!_landed &&
-				   (_control_mode.flag_control_auto_enabled ||
-				    _control_mode.flag_control_velocity_enabled ||
-				    _control_mode.flag_control_altitude_enabled ||
-				    _control_mode.flag_control_climb_rate_enabled));
+	if (_landed) {
+		_tecs.resetIntegrals();
+		_tecs.resetTrajectoryGenerators(_current_altitude);
+	}
 
 	/* update TECS vehicle state estimates */
-	_tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_alt_control,
-					     _current_altitude, _local_pos.vz);
+	_tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), _current_altitude,
+					     _local_pos.vz);
 
-	/* scale throttle cruise by baro pressure */
-	if (_param_fw_thr_alt_scl.get() > FLT_EPSILON) {
-		vehicle_air_data_s air_data;
+	const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min,
+					 throttle_max);
 
-		if (_vehicle_air_data_sub.copy(&air_data)) {
-			if (PX4_ISFINITE(air_data.baro_pressure_pa) && PX4_ISFINITE(_param_fw_thr_alt_scl.get())) {
-				// scale throttle as a function of sqrt(p0/p) (~ EAS -> TAS at low speeds and altitudes ignoring temperature)
-				const float eas2tas = sqrtf(CONSTANTS_STD_PRESSURE_PA / air_data.baro_pressure_pa);
-				const float scale = constrain((eas2tas - 1.0f) * _param_fw_thr_alt_scl.get() + 1.f, 1.f, 2.f);
-
-				throttle_max = constrain(throttle_max * scale, throttle_min, 1.0f);
-				throttle_cruise = constrain(throttle_cruise * scale, throttle_min + 0.01f, throttle_max - 0.01f);
-			}
-		}
-	}
+	const float throttle_limit_max = _cruise_mode_current == CRUISE_MODE_ECO ? _param_fw_thr_max_eco.get() :
+					 _param_fw_thr_max.get(); // APX4 custom
 
 	_tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()),
 				    _current_altitude,
@@ -2662,16 +2727,217 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 				    climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
 				    throttle_min,
 				    throttle_max,
-				    throttle_cruise,
+				    throttle_trim_comp,
+				    throttle_limit_max,
 				    pitch_min_rad - radians(_param_fw_psp_off.get()),
 				    pitch_max_rad - radians(_param_fw_psp_off.get()),
-				    _param_climbrate_target.get(),
-				    _param_sinkrate_target.get(),
-				    hgt_rate_sp);
+				    desired_max_climbrate,
+				    desired_max_sinkrate,
+				    hgt_rate_sp,
+				    _cruise_mode_current == CRUISE_MODE_ECO);
 
 	tecs_status_publish();
 }
 
+float
+FixedwingPositionControl::constrainRollNearGround(const float roll_setpoint, const float altitude,
+		const float terrain_altitude) const
+{
+	// we want the wings level when at the wing height above ground
+	const float height_above_ground = math::max(altitude - (terrain_altitude + _param_fw_wing_height.get()), 0.0f);
+
+	// this is a conservative (linear) approximation of the roll angle that would cause wing tip strike
+	// roll strike = arcsin( 2 * height / span )
+	// d(roll strike)/d(height) = 2 / span / cos(2 * height / span)
+	// d(roll strike)/d(height) (@height=0) = 2 / span
+	// roll strike ~= 2 * height / span
+	const float roll_wingtip_strike = 2.0f * height_above_ground / _param_fw_wing_span.get();
+
+	return math::constrain(roll_setpoint, -roll_wingtip_strike, roll_wingtip_strike);
+}
+
+Vector2f
+FixedwingPositionControl::calculateTakeoffBearingVector(const Vector2f &launch_position,
+		const Vector2f &takeoff_waypoint) const
+{
+	Vector2f takeoff_bearing_vector = takeoff_waypoint - launch_position;
+
+	if (takeoff_bearing_vector.norm_squared() > FLT_EPSILON) {
+		takeoff_bearing_vector.normalize();
+
+	} else {
+		// TODO: a new bearing only based fixed-wing takeoff command / mission item will get rid of the need
+		// for this check
+
+		// takeoff in the direction of the airframe
+		takeoff_bearing_vector(0) = cosf(_yaw);
+		takeoff_bearing_vector(0) = sinf(_yaw);
+	}
+
+	return takeoff_bearing_vector;
+}
+
+void
+FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
+		const position_setpoint_s &pos_sp_curr, const Vector2f &local_position, const Vector2f &local_land_point)
+{
+	if (_time_started_landing == 0) {
+
+		float height_above_land_point;
+		Vector2f local_approach_entrance;
+
+		// set the landing approach entrance location when we have just started the landing and store it
+		// NOTE: the landing approach vector is relative to the land point. ekf resets may cause a local frame
+		// jump, so we reference to the land point, which is globally referenced and will update
+		if (_position_setpoint_previous_valid) {
+			height_above_land_point = pos_sp_prev.alt - pos_sp_curr.alt;
+			local_approach_entrance = _global_local_proj_ref.project(pos_sp_prev.lat, pos_sp_prev.lon);
+
+		} else {
+			// no valid previous waypoint, construct one from the glide slope and direction from current
+			// position to land point
+
+			// NOTE: this is not really a supported use case at the moment, this is just bandaiding any
+			// ill-advised usage of the current implementation
+
+			// TODO: proper handling of on-the-fly landing points would need to involve some more sophisticated
+			// landing pattern generation and corresponding logic
+
+			height_above_land_point = _current_altitude - pos_sp_curr.alt;
+			local_approach_entrance = local_position;
+		}
+
+		_landing_approach_entrance_rel_alt = math::max(height_above_land_point, FLT_EPSILON);
+
+		const Vector2f landing_approach_vector = local_land_point - local_approach_entrance;
+		float landing_approach_distance = landing_approach_vector.norm();
+
+		const float max_glide_slope = tanf(math::radians(_param_fw_lnd_ang.get()));
+		const float glide_slope = _landing_approach_entrance_rel_alt / landing_approach_distance;
+
+		if (glide_slope > max_glide_slope) {
+			// rescale the landing distance - this will have the same effect as dropping down the approach
+			// entrance altitude on the vehicle's behavior. if we reach here.. it means the navigator checks
+			// didn't work, or something is using the control_auto_landing() method inappropriately
+			landing_approach_distance = _landing_approach_entrance_rel_alt / max_glide_slope;
+		}
+
+		if (landing_approach_vector.norm_squared() > FLT_EPSILON) {
+			_landing_approach_entrance_offset_vector = -landing_approach_vector.unit_or_zero() * landing_approach_distance;
+
+		} else {
+			// land in direction of airframe
+			_landing_approach_entrance_offset_vector = Vector2f({cosf(_yaw), sinf(_yaw)}) * landing_approach_distance;
+		}
+
+		// save time at which we started landing and reset landing abort status
+		reset_landing_state();
+		_time_started_landing = now;
+	}
+}
+
+Vector2f
+FixedwingPositionControl::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position)
+{
+	if (fabsf(_manual_control_setpoint.r) > MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE
+	    && _param_fw_lnd_nudge.get() > LandingNudgingOption::kNudgingDisabled
+	    && !_flare_states.flaring) {
+		// laterally nudge touchdown location with yaw stick
+		// positive is defined in the direction of a right hand turn starting from the approach vector direction
+		const float signed_deadzone_threshold = MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE * math::signNoZero(
+				_manual_control_setpoint.r);
+		_lateral_touchdown_position_offset += (_manual_control_setpoint.r - signed_deadzone_threshold) *
+						      MAX_TOUCHDOWN_POSITION_NUDGE_RATE * control_interval;
+		_lateral_touchdown_position_offset =  math::constrain(_lateral_touchdown_position_offset, -_param_fw_lnd_td_off.get(),
+						      _param_fw_lnd_td_off.get());
+	}
+
+	const Vector2f approach_unit_vector = -_landing_approach_entrance_offset_vector.unit_or_zero();
+	const Vector2f approach_unit_normal_vector{-approach_unit_vector(1), approach_unit_vector(0)};
+
+	return local_land_position + approach_unit_normal_vector * _lateral_touchdown_position_offset;
+}
+
+Vector2f
+FixedwingPositionControl::calculateLandingApproachVector() const
+{
+	Vector2f landing_approach_vector = -_landing_approach_entrance_offset_vector;
+	const Vector2f approach_unit_vector = landing_approach_vector.unit_or_zero();
+	const Vector2f approach_unit_normal_vector{-approach_unit_vector(1), approach_unit_vector(0)};
+
+	if (_param_fw_lnd_nudge.get() == LandingNudgingOption::kNudgeApproachAngle) {
+		// nudge the approach angle -- i.e. we adjust the approach vector to reach from the original approach
+		// entrance position to the newly nudged touchdown point
+		// NOTE: this lengthens the landing distance.. which will adjust the glideslope height slightly
+		landing_approach_vector += approach_unit_normal_vector * _lateral_touchdown_position_offset;
+	}
+
+	// if _param_fw_lnd_nudge.get() == LandingNudgingOption::kNudgingDisabled, no nudging
+
+	// if _param_fw_lnd_nudge.get() == LandingNudgingOption::kNudgeApproachPath, the full path (including approach
+	// entrance point) is nudged with the touchdown point, which does not require any additions to the approach vector
+
+	return landing_approach_vector;
+}
+
+float
+FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude)
+{
+	if (_param_fw_lnd_useter.get() > TerrainEstimateUseOnLanding::kDisableTerrainEstimation) {
+
+		if (_local_pos.dist_bottom_valid) {
+
+			const float terrain_estimate = _local_pos.ref_alt + -_local_pos.z - _local_pos.dist_bottom;
+			_last_valid_terrain_alt_estimate = terrain_estimate;
+			_last_time_terrain_alt_was_valid = now;
+
+			return terrain_estimate;
+		}
+
+		if (_last_time_terrain_alt_was_valid == 0) {
+
+			const bool terrain_first_measurement_timed_out = (now - _time_started_landing) > TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT;
+			const bool abort_on_terrain_measurement_timeout = checkLandingAbortBitMask(_param_fw_lnd_abort.get(),
+					position_controller_landing_status_s::TERRAIN_NOT_FOUND);
+
+			if (terrain_first_measurement_timed_out && abort_on_terrain_measurement_timeout) {
+				updateLandingAbortStatus(position_controller_landing_status_s::TERRAIN_NOT_FOUND);
+			}
+
+			return land_point_altitude;
+		}
+
+		if (!_local_pos.dist_bottom_valid) {
+
+			const bool terrain_timed_out = (now - _last_time_terrain_alt_was_valid) > TERRAIN_ALT_TIMEOUT;
+			const bool abort_on_terrain_timeout = checkLandingAbortBitMask(_param_fw_lnd_abort.get(),
+							      position_controller_landing_status_s::TERRAIN_TIMEOUT);
+
+			if (terrain_timed_out && abort_on_terrain_timeout) {
+				updateLandingAbortStatus(position_controller_landing_status_s::TERRAIN_TIMEOUT);
+			}
+
+			return _last_valid_terrain_alt_estimate;
+		}
+	}
+
+	return land_point_altitude;
+}
+
+bool FixedwingPositionControl::checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask,
+		uint8_t landing_abort_criterion)
+{
+	// landing abort status contains a manual criterion at abort_status==1, need to subtract 2 to directly compare
+	// to automatic criteria bits from the parameter FW_LND_ABORT
+	if (landing_abort_criterion <= 1) {
+		return false;
+	}
+
+	landing_abort_criterion -= 2;
+
+	return ((1 << landing_abort_criterion) & automatic_abort_criteria_bitmask) == (1 << landing_abort_criterion);
+}
+
 void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint)
 {
 	if (_global_local_proj_ref.isInitialized()) {
@@ -2700,15 +2966,13 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_
 {
 	orbit_status_s orbit_status{};
 	orbit_status.timestamp = hrt_absolute_time();
-	float loiter_radius = pos_sp.loiter_radius;
-	uint8_t loiter_direction = pos_sp.loiter_direction;
+	float loiter_radius = pos_sp.loiter_radius * (pos_sp.loiter_direction_counter_clockwise ? -1.f : 1.f);
 
 	if (fabsf(loiter_radius) < FLT_EPSILON) {
 		loiter_radius = _param_nav_loiter_rad.get();
-		loiter_direction = signNoZero(loiter_radius);
 	}
 
-	orbit_status.radius = static_cast<float>(loiter_direction) * loiter_radius;
+	orbit_status.radius = loiter_radius;
 	orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
 	orbit_status.x = static_cast<double>(pos_sp.lat);
 	orbit_status.y = static_cast<double>(pos_sp.lon);
@@ -2717,6 +2981,23 @@ void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_
 	_orbit_status_pub.publish(orbit_status);
 }
 
+void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_s pos_sp)
+{
+	// APX4 custom
+
+	figure_eight_status_s figure_eight_status{};
+	figure_eight_status.timestamp = hrt_absolute_time();
+	figure_eight_status.major_radius = pos_sp.loiter_radius * (pos_sp.loiter_direction_counter_clockwise ? -1.f : 1.f);
+	figure_eight_status.minor_radius = pos_sp.loiter_minor_radius;
+	figure_eight_status.orientation = pos_sp.loiter_orientation;
+	figure_eight_status.frame = 5; //MAV_FRAME_GLOBAL_INT
+	figure_eight_status.x = static_cast<int32_t>(pos_sp.lat * 1e7);
+	figure_eight_status.y = static_cast<int32_t>(pos_sp.lon * 1e7);
+	figure_eight_status.z = pos_sp.alt;
+
+	_figure_eight_status_pub.publish(figure_eight_status);
+}
+
 int FixedwingPositionControl::task_spawn(int argc, char *argv[])
 {
 	bool vtol = false;
diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
index df2b58786a..bd1842289a 100644
--- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
@@ -48,6 +48,7 @@
 #ifndef FIXEDWINGPOSITIONCONTROL_HPP_
 #define FIXEDWINGPOSITIONCONTROL_HPP_
 
+#include "figure_eight/FigureEight.hpp" // APX4 custom
 #include "launchdetection/LaunchDetector.h"
 #include "runway_takeoff/RunwayTakeoff.h"
 
@@ -58,7 +59,6 @@
 #include <lib/l1/ECL_L1_Pos_Controller.hpp>
 #include <lib/npfg/npfg.hpp>
 #include <lib/tecs/TECS.hpp>
-#include <lib/landing_slope/Landingslope.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/perf/perf_counter.h>
 #include <lib/slew_rate/SlewRate.hpp>
@@ -94,6 +94,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/wind.h>
 #include <uORB/topics/orbit_status.h>
+#include <uORB/topics/figure_eight_status.h> // APX4 custom
 #include <uORB/uORB.h>
 
 using namespace launchdetection;
@@ -118,9 +119,14 @@ static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;
 // [.] max manual roll/yaw normalized input from user which does not change the locked heading
 static constexpr float HDG_HOLD_MAN_INPUT_THRESH = 0.01f;
 
-// [us] time after which we abort landing if terrain estimate is not valid
+// [us] time after which we abort landing if terrain estimate is not valid. this timer start whenever the terrain altitude
+// was previously valid, and has changed to invalid.
 static constexpr hrt_abstime TERRAIN_ALT_TIMEOUT = 1_s;
 
+// [us] within this timeout, if a distance sensor measurement not yet made, the land waypoint altitude is used for terrain
+// altitude. this timer starts at the beginning of the landing glide slope.
+static constexpr hrt_abstime TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT = 10_s;
+
 // [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes
 static constexpr float THROTTLE_THRESH = 0.05f;
 
@@ -136,6 +142,32 @@ static constexpr float MIN_AUTO_TIMESTEP = 0.01f;
 // [s] maximum time step between auto control updates
 static constexpr float MAX_AUTO_TIMESTEP = 0.05f;
 
+// [.] minimum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived)
+static constexpr float MIN_WEIGHT_RATIO = 0.5f;
+
+// [.] maximum ratio between the actual vehicle weight and the vehicle nominal weight (weight at which the performance limits are derived)
+static constexpr float MAX_WEIGHT_RATIO = 2.0f;
+
+// air density of standard athmosphere at 5000m above mean sea level [kg/m^3]
+static constexpr float AIR_DENSITY_STANDARD_ATMOS_5000_AMSL = 0.7363f;
+
+// [rad] minimum pitch while airspeed has not yet reached a controllable value in manual position controlled takeoff modes
+static constexpr float MIN_PITCH_DURING_MANUAL_TAKEOFF = 0.0f;
+
+// [m] arbitrary buffer altitude added to clearance altitude setpoint during takeoff to ensure aircraft passes the clearance
+// altitude while waiting for navigator to flag it exceeded
+static constexpr float kClearanceAltitudeBuffer = 10.0f;
+
+// [m] a very large number to hopefully avoid the "fly back" case in L1 waypoint following logic once passed the second
+// waypoint in the segment. this is unecessary with NPFG.
+static constexpr float L1_VIRTUAL_TAKEOFF_WP_DIST = 1.0e6f;
+
+// [m/s] maximum rate at which the touchdown position can be nudged
+static constexpr float MAX_TOUCHDOWN_POSITION_NUDGE_RATE = 4.0f;
+
+// [.] normalized deadzone threshold for manual nudging input
+static constexpr float MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE = 0.15f;
+
 class FixedwingPositionControl final : public ModuleBase<FixedwingPositionControl>, public ModuleParams,
 	public px4::WorkItem
 {
@@ -184,154 +216,24 @@ private:
 	uORB::Publication<position_controller_landing_status_s>	_pos_ctrl_landing_status_pub{ORB_ID(position_controller_landing_status)};
 	uORB::Publication<tecs_status_s> _tecs_status_pub{ORB_ID(tecs_status)};
 	uORB::PublicationMulti<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
+	uORB::Publication<figure_eight_status_s> _figure_eight_status_pub{ORB_ID(figure_eight_status)}; // APX4 custom
 
-	manual_control_setpoint_s _manual_control_setpoint {}; // r/c channel data
-	position_setpoint_triplet_s _pos_sp_triplet {}; // triplet of mission items
-	vehicle_attitude_setpoint_s _att_sp {}; // vehicle attitude setpoint
-	vehicle_control_mode_s _control_mode {};
-	vehicle_local_position_s _local_pos {};	// vehicle local position
-	vehicle_status_s _vehicle_status {}; // vehicle status
+	manual_control_setpoint_s _manual_control_setpoint{};
+	position_setpoint_triplet_s _pos_sp_triplet{};
+	vehicle_attitude_setpoint_s _att_sp{};
+	vehicle_control_mode_s _control_mode{};
+	vehicle_local_position_s _local_pos{};
+	vehicle_status_s _vehicle_status{};
 
-	double _current_latitude{0};
-	double _current_longitude{0};
-	float _current_altitude{0.f};
+	bool _position_setpoint_previous_valid{false};
+	bool _position_setpoint_current_valid{false};
+	bool _position_setpoint_next_valid{false};
 
 	perf_counter_t _loop_perf; // loop performance counter
 
-	MapProjection _global_local_proj_ref{};
-	float _global_local_alt0{NAN};
-
-	// [m] ground altitude where the plane was launched
-	float _takeoff_ground_alt{0.0f};
-
-	// [rad] yaw setpoint for manual position mode heading hold
-	float _hdg_hold_yaw{0.0f};
-
-	bool _hdg_hold_enabled{false}; // heading hold enabled
-	bool _yaw_lock_engaged{false}; // yaw is locked for heading hold
-
-	float _min_current_sp_distance_xy{FLT_MAX};
-
-	position_setpoint_s _hdg_hold_prev_wp {}; // position where heading hold started
-	position_setpoint_s _hdg_hold_curr_wp {}; // position to which heading hold flies
-
 	// [us] Last absolute time position control has been called
 	hrt_abstime _last_time_position_control_called{0};
 
-	bool _landed{true};
-
-	/* Landing */
-	bool _land_noreturn_horizontal{false};
-	bool _land_noreturn_vertical{false};
-	bool _land_stayonground{false};
-	bool _land_motor_lim{false};
-	bool _land_onslope{false};
-	bool _land_abort{false};
-
-	Landingslope _landingslope;
-
-	// [us] time at which landing started
-	hrt_abstime _time_started_landing{0};
-
-	// [m] last terrain estimate which was valid
-	float _last_valid_terrain_alt_estimate{0.0f};
-
-	// [us] time at which we had last valid terrain alt
-	hrt_abstime _last_time_terrain_alt_was_valid{0};
-
-	// [m] estimated height to ground at which flare started
-	float _flare_height{0.0f};
-
-	// [m] current forced (i.e. not determined using TECS) flare pitch setpoint
-	float _flare_pitch_sp{0.0f};
-
-	// [m] estimated height to ground at which flare started
-	float _flare_curve_alt_rel_last{0.0f};
-
-	float _target_bearing{0.0f}; // [rad]
-
-	// indicates whether the plane was in the air in the previous interation
-	bool _was_in_air{false};
-
-	// [us] time at which the plane went in the air
-	hrt_abstime _time_went_in_air{0};
-
-	// Takeoff launch detection and runway
-	LaunchDetector _launchDetector;
-	LaunchDetectionResult _launch_detection_state{LAUNCHDETECTION_RES_NONE};
-	hrt_abstime _launch_detection_notify{0};
-
-	RunwayTakeoff _runway_takeoff;
-
-	// true if the last iteration was in manual mode (used to determine when a reset is needed)
-	bool _last_manual{false};
-
-	/* throttle and airspeed states */
-
-	bool _airspeed_valid{false};
-
-	// [us] last time airspeed was received. used to detect timeouts.
-	hrt_abstime _time_airspeed_last_valid{0};
-
-	float _airspeed{0.0f};
-	float _eas2tas{1.0f};
-
-	/* wind estimates */
-
-	// [m/s] wind velocity vector
-	Vector2f _wind_vel{0.0f, 0.0f};
-
-	bool _wind_valid{false};
-
-	hrt_abstime _time_wind_last_received{0}; // [us]
-
-	float _pitch{0.0f};
-	float _yaw{0.0f};
-	float _yawrate{0.0f};
-
-	matrix::Vector3f _body_acceleration{};
-	matrix::Vector3f _body_velocity{};
-
-	bool _reinitialize_tecs{true};
-	bool _is_tecs_running{false};
-
-	hrt_abstime _time_last_tecs_update{0}; // [us]
-
-	float _airspeed_after_transition{0.0f};
-	bool _was_in_transition{false};
-
-	bool _is_vtol_tailsitter{false};
-
-	matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
-
-	// estimator reset counters
-
-	// captures the number of times the estimator has reset the horizontal position
-	uint8_t _pos_reset_counter{0};
-
-	// captures the number of times the estimator has reset the altitude state
-	uint8_t _alt_reset_counter{0};
-
-	// [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands
-	float _manual_control_setpoint_for_height_rate{0.0f};
-
-	// [.] normalized setpoint for manual airspeed control [0,1]; 0,0.5,1 maps to min,cruise,max airspeed commands
-	float _manual_control_setpoint_for_airspeed{0.0f};
-
-	// [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
-	float _commanded_airspeed_setpoint{NAN};
-
-	hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
-
-	// L1 guidance - lateral-directional position control
-	ECL_L1_Pos_Controller _l1_control;
-
-	// nonlinear path following guidance - lateral-directional position control
-	NPFG _npfg;
-
-	// total energy control system - airspeed / altitude control
-	TECS _tecs;
-
 	uint8_t _position_sp_type{0};
 
 	enum FW_POSCTRL_MODE {
@@ -345,15 +247,195 @@ private:
 		FW_POSCTRL_MODE_OTHER
 	} _control_mode_current{FW_POSCTRL_MODE_OTHER}; // used to check if the mode has changed
 
-	param_t _param_handle_airspeed_trans{PARAM_INVALID};
-
-	float _param_airspeed_trans{NAN}; // [m/s]
-
 	enum StickConfig {
 		STICK_CONFIG_SWAP_STICKS_BIT = (1 << 0),
 		STICK_CONFIG_ENABLE_AIRSPEED_SP_MANUAL_BIT = (1 << 1)
 	};
 
+	// VEHICLE STATES
+
+	double _current_latitude{0};
+	double _current_longitude{0};
+	float _current_altitude{0.f};
+
+	float _pitch{0.0f};
+	float _yaw{0.0f};
+	float _yawrate{0.0f};
+
+	matrix::Vector3f _body_acceleration{};
+	matrix::Vector3f _body_velocity{};
+
+	MapProjection _global_local_proj_ref{};
+	float _global_local_alt0{NAN};
+
+	bool _landed{true};
+
+	// indicates whether the plane was in the air in the previous interation
+	bool _was_in_air{false};
+
+	// [us] time at which the plane went in the air
+	hrt_abstime _time_went_in_air{0};
+
+	// MANUAL MODES
+
+	// indicates whether we have completed a manual takeoff in a position control mode
+	bool _completed_manual_takeoff{false};
+
+	// [rad] yaw setpoint for manual position mode heading hold
+	float _hdg_hold_yaw{0.0f};
+
+	bool _hdg_hold_enabled{false}; // heading hold enabled
+	bool _yaw_lock_engaged{false}; // yaw is locked for heading hold
+
+	position_setpoint_s _hdg_hold_prev_wp{}; // position where heading hold started
+	position_setpoint_s _hdg_hold_curr_wp{}; // position to which heading hold flies
+
+	// [.] normalized setpoint for manual altitude control [-1,1]; -1,0,1 maps to min,zero,max height rate commands
+	float _manual_control_setpoint_for_height_rate{0.0f};
+
+	// [.] normalized setpoint for manual airspeed control [0,1]; 0,0.5,1 maps to min,cruise,max airspeed commands
+	float _manual_control_setpoint_for_airspeed{0.0f};
+
+	// [m/s] airspeed setpoint for manual modes commanded via MAV_CMD_DO_CHANGE_SPEED
+	float _commanded_manual_airspeed_setpoint{NAN};
+
+	// AUTO TAKEOFF
+
+	// [m] ground altitude AMSL where the plane was launched
+	float _takeoff_ground_alt{0.0f};
+
+	/* Loitering */ // APX4 custom
+	FigureEight _figure_eight;
+
+	// class handling launch detection methods for fixed-wing takeoff
+	LaunchDetector _launchDetector;
+
+	LaunchDetectionResult _launch_detection_state{LAUNCHDETECTION_RES_NONE};
+
+	// [us] logs the last time the launch detection notification was sent (used not to spam notifications during launch detection)
+	hrt_abstime _last_time_launch_detection_notified{0};
+
+	// true if a launch, specifically using the launch detector, has been detected
+	bool _launch_detected{false};
+
+	// [deg] global position of the vehicle at the time launch is detected (using launch detector)
+	Vector2d _launch_global_position{0, 0};
+
+	// class handling runway takeoff for fixed-wing UAVs with steerable wheels
+	RunwayTakeoff _runway_takeoff;
+
+	bool _skipping_takeoff_detection{false};
+
+	// AUTO LANDING
+
+	// corresponds to param FW_LND_NUDGE
+	enum LandingNudgingOption {
+		kNudgingDisabled = 0,
+		kNudgeApproachAngle,
+		kNudgeApproachPath
+	};
+
+	// [us] Start time of the landing approach. If a fixed-wing landing pattern is used, this timer starts *after any
+	// orbit to altitude only when the aircraft has entered the final *straight approach.
+	hrt_abstime _time_started_landing{0};
+
+	// [m] lateral touchdown position offset manually commanded during landing
+	float _lateral_touchdown_position_offset{0.0f};
+
+	// [m] relative vector from land point to approach entrance (NE)
+	Vector2f _landing_approach_entrance_offset_vector{};
+
+	// [m] relative height above land point
+	float _landing_approach_entrance_rel_alt{0.0f};
+
+	uint8_t _landing_abort_status{position_controller_landing_status_s::NOT_ABORTED};
+
+	// organize flare states XXX: need to split into a separate class at some point!
+	struct FlareStates {
+		bool flaring{false};
+		hrt_abstime start_time{0}; // [us]
+		float initial_height_rate_setpoint{0.0f}; // [m/s]
+		float initial_throttle_setpoint{0.0f};
+	} _flare_states;
+
+	// [m] last terrain estimate which was valid
+	float _last_valid_terrain_alt_estimate{0.0f};
+
+	// [us] time at which we had last valid terrain alt
+	hrt_abstime _last_time_terrain_alt_was_valid{0};
+
+	enum TerrainEstimateUseOnLanding {
+		kDisableTerrainEstimation = 0,
+		kTriggerFlareWithTerrainEstimate,
+		kFollowTerrainRelativeLandingGlideSlope
+	};
+
+	// AIRSPEED
+
+	float _airspeed{0.0f};
+	float _eas2tas{1.0f};
+	bool _airspeed_valid{false};
+	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};
+
+	// [us] last time airspeed was received. used to detect timeouts.
+	hrt_abstime _time_airspeed_last_valid{0};
+
+	// WIND
+
+	// [m/s] wind velocity vector
+	Vector2f _wind_vel{0.0f, 0.0f};
+
+	bool _wind_valid{false};
+
+	hrt_abstime _time_wind_last_received{0}; // [us]
+
+	// TECS
+
+	// total energy control system - airspeed / altitude control
+	TECS _tecs;
+
+	bool _reinitialize_tecs{true};
+	bool _tecs_is_running{false};
+	hrt_abstime _time_last_tecs_update{0}; // [us]
+
+	// VTOL / TRANSITION
+
+	float _airspeed_after_transition{0.0f};
+	bool _was_in_transition{false};
+	bool _is_vtol_tailsitter{false};
+	matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
+	param_t _param_handle_airspeed_trans{PARAM_INVALID};
+	float _param_airspeed_trans{NAN}; // [m/s]
+
+	// ESTIMATOR RESET COUNTERS
+
+	// captures the number of times the estimator has reset the horizontal position
+	uint8_t _pos_reset_counter{0};
+
+	// captures the number of times the estimator has reset the altitude state
+	uint8_t _alt_reset_counter{0};
+
+	// LATERAL-DIRECTIONAL GUIDANCE
+
+	// L1 guidance - lateral-directional position control
+	ECL_L1_Pos_Controller _l1_control;
+
+	// nonlinear path following guidance - lateral-directional position control
+	NPFG _npfg;
+
+	hrt_abstime _time_in_fixed_bank_loiter{0}; // [us]
+	float _min_current_sp_distance_xy{FLT_MAX};
+	float _target_bearing{0.0f}; // [rad]
+
+	// APX4 custom start
+	hrt_abstime _last_time_eco_checks_failed{0};
+
+	enum FW_MODES {
+		CRUISE_MODE_NORMAL,
+		CRUISE_MODE_ECO,
+	} _cruise_mode_current{CRUISE_MODE_NORMAL};		///< used to make flight mode based vehicle limit adaptions (e.g. airspeed setpoint)
+	// APX4 custom end
+
 	// Update our local parameter cache.
 	int parameters_update();
 
@@ -372,7 +454,21 @@ private:
 	void tecs_status_publish();
 	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
 
-	void abort_landing(bool abort);
+	/**
+	 * @brief Sets the landing abort status and publishes landing status.
+	 *
+	 * @param new_abort_status Either 0 (not aborted) or the singular bit >0 which triggered the abort
+	 */
+	void updateLandingAbortStatus(const uint8_t new_abort_status = position_controller_landing_status_s::NOT_ABORTED);
+
+	/**
+	 * @brief Checks if the automatic abort bitmask (from FW_LND_ABORT) contains the given abort criterion.
+	 *
+	 * @param automatic_abort_criteria_bitmask Bitmask containing all active abort criteria
+	 * @param landing_abort_criterion The specifc criterion we are checking for
+	 * @return true if the bitmask contains the criterion
+	 */
+	bool checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask, uint8_t landing_abort_criterion);
 
 	/**
 	 * @brief Get a new waypoint based on heading and distance from current position
@@ -400,9 +496,11 @@ private:
 	float getManualHeightRateSetpoint();
 
 	/**
-	 * @brief Check if we are in a takeoff situation
+	 * @brief Updates a state indicating whether a manual takeoff has been completed.
+	 *
+	 * Criteria include passing an airspeed threshold and not being in a landed state. VTOL airframes always pass.
 	 */
-	bool in_takeoff_situation();
+	void updateManualTakeoffStatus();
 
 	/**
 	 * @brief Update desired altitude base on user pitch stick input
@@ -492,6 +590,18 @@ private:
 	void control_auto_loiter(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
 				 const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
 
+	/**
+	 * Vehicle control for the autonomous figure 8 mode. // APX4 custom
+	 *
+	 * @param control_interval Time since last position control call [s]
+	 * @param curr_pos the current 2D absolute position of the vehicle in [deg].
+	 * @param ground_speed the 2D ground speed of the vehicle in [m/s].
+	 * @param pos_sp_prev the previous position setpoint.
+	 * @param pos_sp_curr the current position setpoint.
+	 */
+	void controlAutoFigureEight(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
+				    const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
+
 	/**
 	 * @brief Controls a desired airspeed, bearing, and height rate.
 	 *
@@ -508,27 +618,25 @@ private:
 	 *
 	 * @param now Current system time [us]
 	 * @param control_interval Time since last position control call [s]
-	 * @param curr_pos Current 2D local position vector of vehicle [m]
+	 * @param global_position Vechile global position [deg]
 	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
-	 * @param pos_sp_prev previous position setpoint
 	 * @param pos_sp_curr current position setpoint
 	 */
-	void control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
-				  const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
+	void control_auto_takeoff(const hrt_abstime &now, const float control_interval, const Vector2d &global_position,
+				  const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr);
 
 	/**
 	 * @brief Controls automatic landing.
 	 *
 	 * @param now Current system time [us]
 	 * @param control_interval Time since last position control call [s]
-	 * @param curr_pos Current 2D local position vector of vehicle [m]
 	 * @param control_interval Time since the last position control update [s]
 	 * @param ground_speed Local 2D ground speed of vehicle [m/s]
 	 * @param pos_sp_prev previous position setpoint
 	 * @param pos_sp_curr current position setpoint
 	 */
-	void control_auto_landing(const hrt_abstime &now, const float control_interval, const Vector2d &curr_pos,
-				  const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
+	void control_auto_landing(const hrt_abstime &now, const float control_interval, const Vector2f &ground_speed,
+				  const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr);
 
 	/* manual control methods */
 
@@ -555,20 +663,23 @@ private:
 
 	float get_manual_airspeed_setpoint();
 
+	void		update_cruise_mode(float pos_sp_cruising_speed); // APX4 custom
+
 	/**
-	 * @brief Returns an calibrated airspeed setpoint for auto modes.
+	 * @brief Returns a adapted calibrated airspeed setpoint
 	 *
 	 * Adjusts the setpoint for wind, accelerated stall, and slew rates.
 	 *
 	 * @param control_interval Time since the last position control update [s]
-	 * @param pos_sp_cru_airspeed The commanded cruise airspeed from the position setpoint [s]
-	 * @param ground_speed Vehicle ground velocity vector [m/s]
-	 * @return Calibrated airspeed setpoint [m/s]
+	 * @param calibrated_airspeed_setpoint Calibrated airspeed septoint (generally from the position setpoint) [m/s]
+	 * @param calibrated_min_airspeed Minimum calibrated airspeed [m/s]
+	 * @param ground_speed Vehicle ground velocity vector (NE) [m/s]
+	 * @return Adjusted calibrated airspeed setpoint [m/s]
 	 */
-	float get_auto_airspeed_setpoint(const float control_interval, const float pos_sp_cru_airspeed,
-					 const Vector2f &ground_speed);
+	float adapt_airspeed_setpoint(const float control_interval, float calibrated_airspeed_setpoint,
+				      float calibrated_min_airspeed, const Vector2f &ground_speed);
 
-	void reset_takeoff_state(bool force = false);
+	void reset_takeoff_state();
 	void reset_landing_state();
 
 	bool using_npfg_with_wind_estimate() const;
@@ -590,11 +701,21 @@ private:
 	 * May also change the position setpoint type depending on the desired behavior.
 	 *
 	 * @param now Current system time [us]
-	 * @param pos_sp_curr_valid True if the current position setpoint is valid
 	 */
-	void set_control_mode_current(const hrt_abstime &now, bool pos_sp_curr_valid);
+	void set_control_mode_current(const hrt_abstime &now);
+
+	/**
+	 * @brief Compensate trim throttle for air density and vehicle weight.
+	 *
+	 * @param trim throttle required at sea level during standard conditions.
+	 * @param throttle_min Minimum allowed trim throttle.
+	 * @param throttle_max Maximum allowed trim throttle.
+	 * @return Trim throttle compensated for air density and vehicle weight.
+	 */
+	float compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min, float throttle_max);
 
 	void publishOrbitStatus(const position_setpoint_s pos_sp);
+	void publishFigureEightStatus(const position_setpoint_s pos_sp); // APX4 custom
 
 	SlewRate<float> _airspeed_slew_rate_controller;
 
@@ -608,18 +729,80 @@ private:
 	 * @param pitch_max_rad Nominal pitch angle command maximum [rad]
 	 * @param throttle_min Minimum throttle command [0,1]
 	 * @param throttle_max Maximum throttle command [0,1]
-	 * @param throttle_cruise Throttle required for level flight at cruising airspeed [0,1]
 	 * @param climbout_mode True if TECS should engage climbout mode
 	 * @param climbout_pitch_min_rad Minimum pitch angle command in climbout mode [rad]
+	 * @param desired_max_sink_rate The desired max sink rate commandable when altitude errors are large [m/s]
+	 * @param desired_max_climb_rate The desired max climb rate commandable when altitude errors are large [m/s]
 	 * @param disable_underspeed_detection True if underspeed detection should be disabled
 	 * @param hgt_rate_sp Height rate setpoint [m/s]
 	 */
-	void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
-					float pitch_min_rad, float pitch_max_rad,
-					float throttle_min, float throttle_max, float throttle_cruise,
-					bool climbout_mode, float climbout_pitch_min_rad,
+	void tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp, float pitch_min_rad,
+					float pitch_max_rad, float throttle_min, float throttle_max, bool climbout_mode,
+					float climbout_pitch_min_rad, const float desired_max_sink_rate, const float desired_max_climb_rate,
 					bool disable_underspeed_detection = false, float hgt_rate_sp = NAN);
 
+	/**
+	 * @brief Constrains the roll angle setpoint near ground to avoid wingtip strike.
+	 *
+	 * @param roll_setpoint Unconstrained roll angle setpoint [rad]
+	 * @param altitude Vehicle altitude (AMSL) [m]
+	 * @param terrain_altitude Terrain altitude (AMSL) [m]
+	 * @return Constrained roll angle setpoint [rad]
+	 */
+	float constrainRollNearGround(const float roll_setpoint, const float altitude, const float terrain_altitude) const;
+
+	/**
+	 * @brief Calculates the unit takeoff bearing vector from the launch position to takeoff waypont.
+	 *
+	 * @param launch_position Vehicle launch position in local coordinates (NE) [m]
+	 * @param takeoff_waypoint Takeoff waypoint position in local coordinates (NE) [m]
+	 * @return Unit takeoff bearing vector
+	 */
+	Vector2f calculateTakeoffBearingVector(const Vector2f &launch_position, const Vector2f &takeoff_waypoint) const;
+
+	/**
+	 * @brief Calculates the touchdown position for landing with optional manual lateral adjustments.
+	 *
+	 * Manual inputs (from the remote) are used to command a rate at which the position moves and the integrated
+	 * position is bounded. This is useful for manually adjusting the landing point in real time when map or GNSS
+	 * errors cause an offset from the desired landing vector.
+	 *
+	 * @param control_interval Time since the last position control update [s]
+	 * @param local_land_position Originally commanded local land position (NE) [m]
+	 * @return (Nudged) Local touchdown position (NE) [m]
+	 */
+	Vector2f calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position);
+
+	/**
+	 * @brief Calculates the vector from landing approach entrance to touchdown point
+	 *
+	 * NOTE: calculateTouchdownPosition() MUST be called before this method
+	 *
+	 * @return Landing approach vector [m]
+	 */
+	Vector2f calculateLandingApproachVector() const;
+
+	/**
+	 * @brief Returns a terrain altitude estimate with consideration of altimeter measurements.
+	 *
+	 * @param now Current system time [us]
+	 * @param land_point_altitude Altitude (AMSL) of the land point [m]
+	 * @return Terrain altitude (AMSL) [m]
+	 */
+	float getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude);
+
+	/**
+	 * @brief Initializes landing states
+	 *
+	 * @param now Current system time [us]
+	 * @param pos_sp_prev Previous position setpoint
+	 * @param pos_sp_curr Current position setpoint
+	 * @param local_position Local aircraft position (NE) [m]
+	 * @param local_land_point Local land point (NE) [m]
+	 */
+	void initializeAutoLanding(const hrt_abstime &now, const position_setpoint_s &pos_sp_prev,
+				   const position_setpoint_s &pos_sp_curr, const Vector2f &local_position, const Vector2f &local_land_point);
+
 	DEFINE_PARAMETERS(
 
 		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
@@ -654,12 +837,9 @@ private:
 		(ParamFloat<px4::params::FW_LND_FL_PMAX>) _param_fw_lnd_fl_pmax,
 		(ParamFloat<px4::params::FW_LND_FL_PMIN>) _param_fw_lnd_fl_pmin,
 		(ParamFloat<px4::params::FW_LND_FLALT>) _param_fw_lnd_flalt,
-		(ParamFloat<px4::params::FW_LND_HHDIST>) _param_fw_lnd_hhdist,
-		(ParamFloat<px4::params::FW_LND_HVIRT>) _param_fw_lnd_hvirt,
 		(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
-		(ParamFloat<px4::params::FW_LND_TLALT>) _param_fw_lnd_tlalt,
 		(ParamBool<px4::params::FW_LND_EARLYCFG>) _param_fw_lnd_earlycfg,
-		(ParamBool<px4::params::FW_LND_USETER>) _param_fw_lnd_useter,
+		(ParamInt<px4::params::FW_LND_USETER>) _param_fw_lnd_useter,
 
 		(ParamFloat<px4::params::FW_P_LIM_MAX>) _param_fw_p_lim_max,
 		(ParamFloat<px4::params::FW_P_LIM_MIN>) _param_fw_p_lim_min,
@@ -684,10 +864,8 @@ private:
 		(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
 		(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
 
-		(ParamFloat<px4::params::FW_THR_ALT_SCL>) _param_fw_thr_alt_scl,
-		(ParamFloat<px4::params::FW_THR_CRUISE>) _param_fw_thr_cruise,
+		(ParamFloat<px4::params::FW_THR_TRIM>) _param_fw_thr_trim,
 		(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,
-		(ParamFloat<px4::params::FW_THR_LND_MAX>) _param_fw_thr_lnd_max,
 		(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,
 		(ParamFloat<px4::params::FW_THR_MIN>) _param_fw_thr_min,
 		(ParamFloat<px4::params::FW_THR_SLEW_MAX>) _param_fw_thr_slew_max,
@@ -708,7 +886,38 @@ private:
 
 		(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min,
 
-		(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad
+		// APX4 custom params
+		(ParamFloat<px4::params::FW_T_ALT_TC_E>) _param_fw_t_h_error_tc_eco,
+		(ParamFloat<px4::params::FW_T_SPDWEIGHT_E>) _param_fw_t_spdweight_eco,
+		(ParamFloat<px4::params::FW_THR_MAX_E>) _param_fw_thr_max_eco,
+		(ParamFloat<px4::params::FW_T_CLMB_R_SP_E>) _param_climbrate_target_eco,
+
+		(ParamFloat<px4::params::FW_ECO_ALT_ERR_U>) _param_fw_eco_alt_err_u,
+		(ParamFloat<px4::params::FW_ECO_ALT_ERR_O>) _param_fw_eco_alt_err_o,
+		(ParamFloat<px4::params::FW_ECO_ALT_MIN>) _param_fw_eco_alt_min,
+		(ParamFloat<px4::params::FW_ECO_AD_THRLD>) _param_fw_eco_ad_thrld,
+		// APX4 custom end
+
+		(ParamFloat<px4::params::NAV_FW_ALT_RAD>) _param_nav_fw_alt_rad,
+
+		(ParamFloat<px4::params::WEIGHT_BASE>) _param_weight_base,
+		(ParamFloat<px4::params::WEIGHT_GROSS>) _param_weight_gross,
+
+		(ParamFloat<px4::params::FW_WING_SPAN>) _param_fw_wing_span,
+		(ParamFloat<px4::params::FW_WING_HEIGHT>) _param_fw_wing_height,
+
+		(ParamFloat<px4::params::RWTO_L1_PERIOD>) _param_rwto_l1_period,
+		(ParamBool<px4::params::RWTO_NUDGE>) _param_rwto_nudge,
+
+		(ParamFloat<px4::params::FW_LND_FL_TIME>) _param_fw_lnd_fl_time,
+		(ParamFloat<px4::params::FW_LND_FL_SINK>) _param_fw_lnd_fl_sink,
+		(ParamFloat<px4::params::FW_LND_TD_OFF>) _param_fw_lnd_td_off,
+		(ParamInt<px4::params::FW_LND_NUDGE>) _param_fw_lnd_nudge,
+		(ParamInt<px4::params::FW_LND_ABORT>) _param_fw_lnd_abort,
+
+		(ParamFloat<px4::params::FW_WIND_ARSP_SC>) _param_fw_wind_arsp_sc,
+
+		(ParamFloat<px4::params::FW_TKO_AIRSPD>) _param_fw_tko_airspd
 	)
 
 };
diff --git a/src/modules/fw_pos_control_l1/figure_eight/CMakeLists.txt b/src/modules/fw_pos_control_l1/figure_eight/CMakeLists.txt
new file mode 100644
index 0000000000..df249c66c9
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/figure_eight/CMakeLists.txt
@@ -0,0 +1,36 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_library(figure_eight
+	FigureEight.cpp
+)
diff --git a/src/modules/fw_pos_control_l1/figure_eight/FigureEight.cpp b/src/modules/fw_pos_control_l1/figure_eight/FigureEight.cpp
new file mode 100644
index 0000000000..e355674e10
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/figure_eight/FigureEight.cpp
@@ -0,0 +1,398 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+/**
+ * @file FigureEight.cpp
+ * Helper class for fixed wing position controller when flying a figure 8 loiter pattern.
+ *
+ */
+
+#include "FigureEight.hpp"
+
+#include <cmath>
+
+#include "lib/geo/geo.h"
+#include "lib/matrix/matrix/Dcm.hpp"
+#include "lib/matrix/matrix/Matrix.hpp"
+#include "lib/matrix/matrix/Vector2.hpp"
+
+static constexpr float NORMALIZED_MAJOR_RADIUS{1.0f};
+static constexpr bool NORTH_CIRCLE_IS_COUNTER_CLOCKWISE{false};
+static constexpr bool SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE{true};
+static constexpr float MINIMUM_MINOR_TO_MAJOR_AXIS_SCALE{2.0f};
+static constexpr float DEFAULT_MAJOR_TO_MINOR_AXIS_RATIO{2.5f};
+static constexpr float MINIMAL_FEASIBLE_MAJOR_TO_MINOR_AXIS_RATIO{2.0f};
+
+FigureEight::FigureEight(NPFG &npfg, ECL_L1_Pos_Controller &l1_control, matrix::Vector2f &wind_vel, float &eas2tas) :
+	ModuleParams(nullptr),
+	_npfg(npfg),
+	_l1_control(l1_control),
+	_wind_vel(wind_vel),
+	_eas2tas(eas2tas)
+{
+
+}
+
+void FigureEight::initializePattern(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+				    const FigureEightPatternParameters &parameters)
+{
+	// Initialize the currently active segment, if it hasn't been active yet, or the sp has been changed.
+	if ((_current_segment == FigureEightSegment::SEGMENT_UNDEFINED) ||
+	    ((fabsf(_active_parameters.center_pos_local(0) - parameters.center_pos_local(0)) > FLT_EPSILON) ||
+	     (fabsf(_active_parameters.center_pos_local(1) - parameters.center_pos_local(1)) > FLT_EPSILON) ||
+	     (fabsf(_active_parameters.loiter_radius - parameters.loiter_radius) > FLT_EPSILON) ||
+	     (fabsf(_active_parameters.loiter_minor_radius - parameters.loiter_minor_radius) > FLT_EPSILON) ||
+	     (fabsf(_active_parameters.loiter_orientation - parameters.loiter_orientation) > FLT_EPSILON) ||
+	     (_active_parameters.loiter_direction_counter_clockwise != parameters.loiter_direction_counter_clockwise))) {
+		matrix::Vector2f rel_pos_to_center;
+		calculatePositionToCenterNormalizedRotated(rel_pos_to_center, curr_pos_local, parameters);
+
+		matrix::Vector2f ground_speed_rotated{ground_speed};
+		float yaw_rotation{calculateRotationAngle(parameters)};
+		ground_speed_rotated.transform(yaw_rotation);
+
+		FigureEightPatternPoints pattern_points;
+		calculateFigureEightPoints(pattern_points, parameters);
+
+		if (rel_pos_to_center(0) > NORMALIZED_MAJOR_RADIUS) { // Far away north
+			_current_segment = FigureEightSegment::SEGMENT_CIRCLE_NORTH;
+
+		} else if (rel_pos_to_center(0) < -NORMALIZED_MAJOR_RADIUS) { // Far away south
+			_current_segment = FigureEightSegment::SEGMENT_CIRCLE_SOUTH;
+
+		} else if (ground_speed_rotated.dot(matrix::Vector2f{1.0f, 0.0f}) > 0.0f) { // Flying northbound
+			if (rel_pos_to_center(0) > pattern_points.normalized_north_circle_offset(0)) { // already at north circle
+				_current_segment = FigureEightSegment::SEGMENT_CIRCLE_NORTH;
+
+			} else {
+				_current_segment = FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST;
+			}
+
+		} else {
+			if (rel_pos_to_center(0) < pattern_points.normalized_south_circle_offset(0)) { // already at south circle
+				_current_segment = FigureEightSegment::SEGMENT_CIRCLE_SOUTH;
+
+			} else {
+				_current_segment = FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST;
+			}
+		}
+
+		_active_parameters = parameters;
+		_pos_passed_circle_center_along_major_axis = false;
+	}
+}
+
+void FigureEight::resetPattern()
+{
+	// Set the current segment invalid
+	_current_segment = FigureEightSegment::SEGMENT_UNDEFINED;
+	_pos_passed_circle_center_along_major_axis = false;
+}
+
+void FigureEight::updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+				 const FigureEightPatternParameters &parameters, float target_airspeed, const bool use_npfg)
+{
+	// Sanitize inputs
+	FigureEightPatternParameters valid_parameters{parameters};
+
+	if (!PX4_ISFINITE(parameters.loiter_minor_radius)) {
+		valid_parameters.loiter_minor_radius = fabsf(_param_nav_loiter_rad.get());
+	}
+
+	if (!PX4_ISFINITE(parameters.loiter_radius)) {
+		valid_parameters.loiter_radius = DEFAULT_MAJOR_TO_MINOR_AXIS_RATIO * valid_parameters.loiter_minor_radius;
+		valid_parameters.loiter_direction_counter_clockwise = _param_nav_loiter_rad.get() < 0;
+	}
+
+	valid_parameters.loiter_radius = math::max(valid_parameters.loiter_radius,
+					 MINIMAL_FEASIBLE_MAJOR_TO_MINOR_AXIS_RATIO * valid_parameters.loiter_minor_radius);
+
+	// Calculate the figure eight pattern points.
+	FigureEightPatternPoints pattern_points;
+	calculateFigureEightPoints(pattern_points, valid_parameters);
+
+	// Check if we need to switch to next segment
+	updateSegment(curr_pos_local, valid_parameters, use_npfg,  pattern_points);
+
+	// Apply control logic based on segment
+	applyControl(curr_pos_local, ground_speed, valid_parameters, target_airspeed, use_npfg, pattern_points);
+}
+
+void FigureEight::calculateFigureEightPoints(FigureEightPatternPoints &pattern_points,
+		const FigureEightPatternParameters &parameters)
+{
+	const float normalized_minor_radius = (parameters.loiter_minor_radius / parameters.loiter_radius) *
+					      NORMALIZED_MAJOR_RADIUS;
+	const float cos_transition_angle = parameters.loiter_minor_radius / (parameters.loiter_radius -
+					   parameters.loiter_minor_radius);
+	const float sin_transition_angle = sqrtf(1.0f - cos_transition_angle * cos_transition_angle);
+	pattern_points.normalized_north_circle_offset = matrix::Vector2f{NORMALIZED_MAJOR_RADIUS - normalized_minor_radius, 0.0f};
+	pattern_points.normalized_north_entry_offset =  matrix::Vector2f{NORMALIZED_MAJOR_RADIUS - ((normalized_minor_radius) * (1.0f + cos_transition_angle)),
+			-normalized_minor_radius * sin_transition_angle};
+	pattern_points.normalized_north_exit_offset = matrix::Vector2f{NORMALIZED_MAJOR_RADIUS - ((normalized_minor_radius) * (1.0f + cos_transition_angle)),
+			normalized_minor_radius * sin_transition_angle};
+	pattern_points.normalized_south_circle_offset = matrix::Vector2f{-NORMALIZED_MAJOR_RADIUS + normalized_minor_radius, 0.0f};
+	pattern_points.normalized_south_entry_offset = matrix::Vector2f{-NORMALIZED_MAJOR_RADIUS + ((normalized_minor_radius) * (1.0f + cos_transition_angle)),
+			-normalized_minor_radius * sin_transition_angle};
+	pattern_points.normalized_south_exit_offset = matrix::Vector2f{-NORMALIZED_MAJOR_RADIUS + ((normalized_minor_radius) * (1.0f + cos_transition_angle)),
+			normalized_minor_radius * sin_transition_angle};
+}
+
+void FigureEight::updateSegment(const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters &parameters,
+				const bool use_npfg, const FigureEightPatternPoints &pattern_points)
+{
+	matrix::Vector2f rel_pos_to_center;
+	calculatePositionToCenterNormalizedRotated(rel_pos_to_center, curr_pos_local, parameters);
+
+	// Get the normalized l1-distance to know when to switch.
+	float switch_distance_normalized;
+
+	if (use_npfg) {
+		switch_distance_normalized = _npfg.switchDistance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
+
+	} else {
+		switch_distance_normalized = _l1_control.switch_distance(FLT_MAX) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
+	}
+
+	// Update segment if segment exit condition has been reached
+	switch (_current_segment) {
+	case FigureEightSegment::SEGMENT_CIRCLE_NORTH: {
+			if (rel_pos_to_center(0) > pattern_points.normalized_north_circle_offset(0)) {
+				_pos_passed_circle_center_along_major_axis = true;
+			}
+
+			matrix::Vector2f vector_to_exit_normalized = pattern_points.normalized_north_exit_offset - rel_pos_to_center;
+
+			/* Exit condition: l1 distance away from north-east point of north circle and at least once was above the circle center. Failsafe action, if poor tracking,
+			-                       switch to next if the vehicle is on the east side and below the  north exit point. */
+			if (_pos_passed_circle_center_along_major_axis &&
+			    ((vector_to_exit_normalized.norm() < switch_distance_normalized) ||
+			     ((rel_pos_to_center(0) < pattern_points.normalized_north_exit_offset(0)) &&
+			      (rel_pos_to_center(1) > FLT_EPSILON)))) {
+				_current_segment = FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST;
+			}
+		}
+		break;
+
+	case FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST: {
+			_pos_passed_circle_center_along_major_axis = false;
+			matrix::Vector2f vector_to_exit_normalized = pattern_points.normalized_south_entry_offset - rel_pos_to_center;
+
+			/* Exit condition: l1 distance away from south-west point of south circle. Failsafe action, if poor tracking,
+			switch to next if the vehicle is on the west side and below entry point of the south circle or has left the radius. */
+			if ((vector_to_exit_normalized.norm() < switch_distance_normalized) ||
+			    ((rel_pos_to_center(0) < pattern_points.normalized_south_entry_offset(0)) && (rel_pos_to_center(1) < FLT_EPSILON)) ||
+			    (rel_pos_to_center(0) < -NORMALIZED_MAJOR_RADIUS)) {
+				_current_segment = FigureEightSegment::SEGMENT_CIRCLE_SOUTH;
+			}
+		}
+		break;
+
+	case FigureEightSegment::SEGMENT_CIRCLE_SOUTH: {
+			if (rel_pos_to_center(0) < pattern_points.normalized_south_circle_offset(0)) {
+				_pos_passed_circle_center_along_major_axis = true;
+			}
+
+			matrix::Vector2f vector_to_exit_normalized = pattern_points.normalized_south_exit_offset - rel_pos_to_center;
+
+			/* Exit condition: l1 distance away from south-east point of south circle and at least once was below the circle center. Failsafe action, if poor tracking,
+			-                       switch to next if the vehicle is on the east side and above the south exit point. */
+			if (_pos_passed_circle_center_along_major_axis &&
+			    ((vector_to_exit_normalized.norm() < switch_distance_normalized) ||
+			     ((rel_pos_to_center(0) > pattern_points.normalized_south_exit_offset(0)) &&
+			      (rel_pos_to_center(1) > FLT_EPSILON)))) {
+				_current_segment = FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST;
+			}
+
+		}
+		break;
+
+	case FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST: {
+			_pos_passed_circle_center_along_major_axis = false;
+			matrix::Vector2f vector_to_exit_normalized = pattern_points.normalized_north_entry_offset - rel_pos_to_center;
+
+			/* Exit condition: l1 distance away from north-west point of north circle. Failsafe action, if poor tracking,
+			switch to next if the vehicle is on the west side and above entry point of the north circle or has left the radius. */
+			if ((vector_to_exit_normalized.norm() < switch_distance_normalized) ||
+			    ((rel_pos_to_center(0) > pattern_points.normalized_north_entry_offset(0)) && (rel_pos_to_center(1) < FLT_EPSILON)) ||
+			    (rel_pos_to_center(0) > NORMALIZED_MAJOR_RADIUS)) {
+				_current_segment = FigureEightSegment::SEGMENT_CIRCLE_NORTH;
+			}
+		}
+		break;
+
+	case FigureEightSegment::SEGMENT_UNDEFINED:
+	default:
+		break;
+	}
+}
+
+void FigureEight::applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+			       const FigureEightPatternParameters &parameters, float target_airspeed, const bool use_npfg,
+			       const FigureEightPatternPoints &pattern_points)
+{
+	switch (_current_segment) {
+	case FigureEightSegment::SEGMENT_CIRCLE_NORTH: {
+			applyCircle(NORTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_north_circle_offset, curr_pos_local,
+				    ground_speed,
+				    parameters, target_airspeed, use_npfg);
+		}
+		break;
+
+	case FigureEightSegment::SEGMENT_NORTHEAST_SOUTHWEST: {
+			// Follow path from north-east to south-west
+			applyLine(pattern_points.normalized_north_exit_offset, pattern_points.normalized_south_entry_offset, curr_pos_local,
+				  ground_speed,
+				  parameters, target_airspeed, use_npfg);
+		}
+		break;
+
+	case FigureEightSegment::SEGMENT_CIRCLE_SOUTH: {
+			applyCircle(SOUTH_CIRCLE_IS_COUNTER_CLOCKWISE, pattern_points.normalized_south_circle_offset, curr_pos_local,
+				    ground_speed,
+				    parameters, target_airspeed, use_npfg);
+		}
+		break;
+
+	case FigureEightSegment::SEGMENT_SOUTHEAST_NORTHWEST: {
+			// follow path from south-east to north-west
+			applyLine(pattern_points.normalized_south_exit_offset, pattern_points.normalized_north_entry_offset, curr_pos_local,
+				  ground_speed,
+				  parameters, target_airspeed, use_npfg);
+		}
+		break;
+
+	case FigureEightSegment::SEGMENT_UNDEFINED:
+	default:
+		break;
+	}
+}
+
+void FigureEight::calculatePositionToCenterNormalizedRotated(matrix::Vector2f &pos_to_center_normalized_rotated,
+		const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters &parameters) const
+{
+	matrix::Vector2f pos_to_center = curr_pos_local - parameters.center_pos_local;
+
+	// normalize position with respect to radius
+	matrix::Vector2f pos_to_center_normalized;
+	pos_to_center_normalized(0) = pos_to_center(0) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
+	pos_to_center_normalized(1) = pos_to_center(1) * NORMALIZED_MAJOR_RADIUS / parameters.loiter_radius;
+
+	// rotate position with respect to figure eight orientation and direction.
+	float yaw_rotation = calculateRotationAngle(parameters);
+	pos_to_center_normalized_rotated = pos_to_center_normalized;
+	pos_to_center_normalized_rotated.transform(yaw_rotation);
+}
+
+float FigureEight::calculateRotationAngle(const FigureEightPatternParameters &parameters) const
+{
+	// rotate position with respect to figure eight orientation and direction.
+	float yaw_rotation = parameters.loiter_orientation;
+
+	// figure eight pattern is symmetric, changing the direction is the same as a rotation by 180° around center
+	if (parameters.loiter_direction_counter_clockwise) {
+		yaw_rotation += M_PI_F;
+	}
+
+	return yaw_rotation;
+}
+
+void FigureEight::applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset,
+			      const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+			      const FigureEightPatternParameters &parameters,
+			      float target_airspeed, const bool use_npfg)
+{
+	float yaw_rotation = calculateRotationAngle(parameters);
+	const matrix::Matrix<float, 2, 2> rotation_matrix = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f,
+			yaw_rotation)).slice<2, 2>(0, 0);
+
+	matrix::Vector2f circle_offset = normalized_circle_offset * (parameters.loiter_radius / NORMALIZED_MAJOR_RADIUS);
+
+	matrix::Vector2f circle_offset_rotated = rotation_matrix * circle_offset;
+
+	matrix::Vector2f circle_center = parameters.center_pos_local + circle_offset_rotated;
+
+	if (use_npfg) {
+		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+		_npfg.navigateLoiter(circle_center, curr_pos_local, parameters.loiter_minor_radius,
+				     loiter_direction_counter_clockwise, ground_speed, _wind_vel);
+		_roll_setpoint = _npfg.getRollSetpoint();
+		_indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas;
+
+	} else {
+		_l1_control.navigate_loiter(circle_center, curr_pos_local, parameters.loiter_minor_radius,
+					    loiter_direction_counter_clockwise, ground_speed);
+		_roll_setpoint = _l1_control.get_roll_setpoint();
+		_indicated_airspeed_setpoint = target_airspeed;
+	}
+}
+
+void FigureEight::applyLine(const matrix::Vector2f &normalized_line_start_offset,
+			    const matrix::Vector2f &normalized_line_end_offset, const matrix::Vector2f &curr_pos_local,
+			    const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters, float target_airspeed,
+			    const bool use_npfg)
+{
+	float yaw_rotation{calculateRotationAngle(parameters)};
+	const matrix::Matrix<float, 2, 2> rotation_matrix = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f,
+			yaw_rotation)).slice<2, 2>(0, 0);
+
+	/* Calculate start offset depending on radius */
+	matrix::Vector2f start_offset = normalized_line_start_offset * (parameters.loiter_radius / NORMALIZED_MAJOR_RADIUS);
+
+	// rotate start point
+	matrix::Vector2f start_offset_rotated = rotation_matrix * start_offset;
+
+	matrix::Vector2f line_segment_start_position = parameters.center_pos_local + start_offset_rotated;
+
+	/* Calculate start offset depending on radius*/
+	matrix::Vector2f end_offset = normalized_line_end_offset * (parameters.loiter_radius / NORMALIZED_MAJOR_RADIUS);
+
+	// rotate start point
+	matrix::Vector2f end_offset_rotated = rotation_matrix * end_offset;
+
+	matrix::Vector2f line_segment_end_position = parameters.center_pos_local + end_offset_rotated;
+
+	if (use_npfg) {
+		_npfg.setAirspeedNom(target_airspeed * _eas2tas);
+		_npfg.setAirspeedMax(_param_fw_airspd_max.get() * _eas2tas);
+		_npfg.navigateWaypoints(line_segment_start_position, line_segment_end_position, curr_pos_local, ground_speed,
+					_wind_vel);
+		_roll_setpoint = _npfg.getRollSetpoint();
+		_indicated_airspeed_setpoint = _npfg.getAirspeedRef() / _eas2tas;
+
+	} else {
+		_l1_control.navigate_waypoints(line_segment_start_position, line_segment_end_position, curr_pos_local, ground_speed);
+		_roll_setpoint = _l1_control.get_roll_setpoint();
+		_indicated_airspeed_setpoint = target_airspeed;
+	}
+}
diff --git a/src/modules/fw_pos_control_l1/figure_eight/FigureEight.hpp b/src/modules/fw_pos_control_l1/figure_eight/FigureEight.hpp
new file mode 100644
index 0000000000..26125446ca
--- /dev/null
+++ b/src/modules/fw_pos_control_l1/figure_eight/FigureEight.hpp
@@ -0,0 +1,270 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+/**
+ * @file FigureEight.hpp
+ * Helper class for lateral fixed wing position controller when flying a figure 8 loiter pattern.
+ *
+ */
+
+#ifndef FIGUREEIGHT_HPP_
+#define FIGUREEIGHT_HPP_
+
+#include <cstdint>
+
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/param.h>
+
+#include "lib/l1/ECL_L1_Pos_Controller.hpp"
+#include "lib/matrix/matrix/Vector2.hpp"
+#include "lib/npfg/npfg.hpp"
+
+class FigureEight : public ModuleParams
+{
+public:
+	/**
+	 * @brief Figure eight pattern points strust
+	 *
+	 * Struct defining all relevant points for the figure eight pattern.
+	 *
+	 */
+	struct FigureEightPatternPoints {
+		matrix::Vector2f normalized_north_circle_offset;
+		matrix::Vector2f normalized_north_entry_offset;
+		matrix::Vector2f normalized_north_exit_offset;
+		matrix::Vector2f normalized_south_circle_offset;
+		matrix::Vector2f normalized_south_entry_offset;
+		matrix::Vector2f normalized_south_exit_offset;
+	};
+	struct FigureEightPatternParameters {
+		matrix::Vector2f center_pos_local;
+		float loiter_radius;
+		float loiter_minor_radius;
+		float loiter_orientation;
+		bool loiter_direction_counter_clockwise;
+	};
+	/**
+	 * @brief Construct a new Figure Eight object
+	 *
+	 * @param[in] npfg is the reference to the parent npfg object.
+	 * @param[in] l1_control is the reference to the parent l1_control object.
+	 * @param[in] wind_vel is the reference to the parent wind velocity [m/s].
+	 * @param[in] eas2tas is the reference to the parent indicated airspeed to true airspeed conversion.
+	 */
+	FigureEight(NPFG &npfg, ECL_L1_Pos_Controller &l1_control, matrix::Vector2f &wind_vel, float &eas2tas);
+	/**
+	 * @brief Initialize the figure eight pattern.
+	 *
+	 * Initialize the figure eight pattern by determining the current active segment.
+	 *
+	 * @param[in] curr_pos_local is the current local position of the vehicle in [m].
+	 * @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
+	 * @param[in] parameters is the parameter set defining the figure eight shape.
+	 */
+	void initializePattern(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+			       const FigureEightPatternParameters &parameters);
+	/**
+	 * @brief reset the figure eight pattern.
+	 *
+	 * Reset the figure eight pattern such that it can be properly initialized on a new figure eight pattern.
+	 *
+	 */
+	void resetPattern();
+	/**
+	 * @brief Update roll and airspeed setpoint.
+	 *
+	 * @param[in] curr_pos_local is the current local position of the vehicle in [m].
+	 * @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
+	 * @param[in] parameters is the parameter set defining the figure eight shape.
+	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
+	 * @param[in] use_npfg flag if the npfg lateral control logic should be used.
+	 */
+	void updateSetpoint(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+			    const FigureEightPatternParameters &parameters, float target_airspeed, const bool use_npfg);
+	/**
+	 * @brief Get the roll setpoint
+	 *
+	 * @return the roll setpoint in [rad].
+	 */
+	float getRollSetpoint() const {return _roll_setpoint;};
+	/**
+	 * @brief Get the indicated airspeed setpoint
+	 *
+	 * @return the indicated airspeed setpoint in [m/s].
+	 */
+	float getAirspeedSetpoint() const {return _indicated_airspeed_setpoint;};
+
+
+private:
+	/**
+	 * @brief Calculate figure eight pattern points
+	 *
+	 * @param[out] 	pattern_points is the output with the calculated points for the figure eight.
+	 * @param[in] parameters is the parameter set defining the figure eight shape.
+	 */
+	void calculateFigureEightPoints(FigureEightPatternPoints &pattern_points,
+					const FigureEightPatternParameters &parameters);
+	/**
+	 * @brief Apply lateral control logic
+	 *
+	 *
+	 * @param[in] curr_pos_local is the current local position of the vehicle in [m].
+	 * @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
+	 * @param[in] parameters is the parameter set defining the figure eight shape.
+	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
+	 * @param[in] use_npfg flag if the npfg lateral control logic should be used.
+	 * @param[in] pattern_points are the relevant points defining the figure eight pattern.
+	 */
+	void applyControl(const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+			  const FigureEightPatternParameters &parameters, float target_airspeed, const bool use_npfg,
+			  const FigureEightPatternPoints &pattern_points);
+	/**
+	 * @brief Update active segment.
+	 *
+	 * @param[in] curr_pos_local is the current local position of the vehicle in [m].
+	 * @param[in] parameters is the parameter set defining the figure eight shape.
+	 * @param[in] use_npfg flag if the npfg lateral control logic should be used.
+	 * @param[in] pattern_points are the relevant points defining the figure eight pattern.
+	 */
+	void updateSegment(const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters &parameters,
+			   const bool use_npfg,
+			   const FigureEightPatternPoints &pattern_points);
+	/**
+	 * @brief calculate normalized and rotated relative vehicle position to pattern center.
+	 *
+	 * @param[out] pos_to_center_normalized_rotated is the calculated normalized and rotated relative vehicle position to pattern center.
+	 * @param[in] curr_pos_local is the current local position of the vehicle in [m].
+	 * @param[in] parameters is the parameter set defining the figure eight shape.
+	 */
+	void calculatePositionToCenterNormalizedRotated(matrix::Vector2f &pos_to_center_normalized_rotated,
+			const matrix::Vector2f &curr_pos_local, const FigureEightPatternParameters &parameters) const;
+	/**
+	 * @brief Calculate rotation angle.
+	 *
+	 * @param[in] parameters is the parameter set defining the figure eight shape.
+	 *
+	 * @return is the rotation angle of the major axis compared to north in [rad].
+	 */
+	float calculateRotationAngle(const FigureEightPatternParameters &parameters) const;
+	/**
+	 * @brief Apply circular lateral control
+	 *
+	 * @param[in] loiter_direction_counter_clockwise flag if the circle direction should be counter-clockwise.
+	 * @param[in] normalized_circle_offset is the normalized position offset of the circle compared to the pattern center.
+	 * @param[in] curr_pos_local is the current local position of the vehicle in [m].
+	 * @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
+	 * @param[in] parameters is the parameter set defining the figure eight shape.
+	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
+	 * @param[in] use_npfg flag if the npfg lateral control logic should be used.
+	 */
+	void applyCircle(bool loiter_direction_counter_clockwise, const matrix::Vector2f &normalized_circle_offset,
+			 const matrix::Vector2f &curr_pos_local,
+			 const matrix::Vector2f &ground_speed, const FigureEightPatternParameters &parameters, float target_airspeed,
+			 const bool use_npfg);
+	/**
+	 * @brief Apply path lateral control
+	 *
+	 * @param[in] normalized_line_start_offset is the normalized position offset of the start point of the path compared to the pattern center.
+	 * @param[in] normalized_line_end_offset is the normalized position offset of the end point of the path compared to the pattern center.
+	 * @param[in] curr_pos_local is the current local position of the vehicle in [m].
+	 * @param[in] ground_speed is the current ground speed of the vehicle in [m/s].
+	 * @param[in] parameters is the parameter set defining the figure eight shape.
+	 * @param[in] target_airspeed is the current targeted indicated airspeed [m/s].
+	 * @param[in] use_npfg flag if the npfg lateral control logic should be used.
+	 */
+	void applyLine(const matrix::Vector2f &normalized_line_start_offset, const matrix::Vector2f &normalized_line_end_offset,
+		       const matrix::Vector2f &curr_pos_local, const matrix::Vector2f &ground_speed,
+		       const FigureEightPatternParameters &parameters,
+		       float target_airspeed, const bool use_npfg);
+
+private:
+	/**
+	 * @brief npfg lateral control object.
+	 *
+	 */
+	NPFG &_npfg;
+	/**
+	 * @brief lateral l1 control object.
+	 *
+	 */
+	ECL_L1_Pos_Controller &_l1_control;
+	/**
+	 * @brief Wind velocity in [m/s].
+	 *
+	 */
+	const matrix::Vector2f &_wind_vel;
+	/**
+	 * @brief Conversion factor from indicated to true airspeed.
+	 *
+	 */
+	const float &_eas2tas;
+	/**
+	 * @brief Roll setpoint in [rad].
+	 *
+	 */
+	float _roll_setpoint;
+	/**
+	 * @brief Indicated airspeed setpoint in [m/s].
+	 *
+	 */
+	float _indicated_airspeed_setpoint;
+	/**
+	 * @brief active figure eight position setpoint.
+	 *
+	 */
+	FigureEightPatternParameters _active_parameters;
+
+	enum class FigureEightSegment {
+		SEGMENT_UNDEFINED,
+		SEGMENT_CIRCLE_NORTH,
+		SEGMENT_NORTHEAST_SOUTHWEST,
+		SEGMENT_CIRCLE_SOUTH,
+		SEGMENT_SOUTHEAST_NORTHWEST
+	};
+
+	/**
+	 * @brief Current active segment of the figure eight pattern.
+	 *
+	 */
+	FigureEightSegment _current_segment{FigureEightSegment::SEGMENT_UNDEFINED};
+	/**
+	 * @brief flag if vehicle position passed circle center along major axis when on circle segment.
+	 *
+	 */
+	bool _pos_passed_circle_center_along_major_axis;
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::FW_AIRSPD_MAX>) _param_fw_airspd_max,
+		(ParamFloat<px4::params::NAV_LOITER_RAD>) _param_nav_loiter_rad
+	)
+};
+
+#endif // FIGUREEIGHT_HPP_
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 76ec5ce013..0a1ae4a1a2 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -80,6 +80,7 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
  *
  * @unit deg/s
  * @min 0
+ * @decimal 0
  * @increment 1
  * @group FW L1 Control
  */
@@ -93,7 +94,7 @@ PARAM_DEFINE_FLOAT(FW_L1_R_SLEW_MAX, 90.0f);
  * @boolean
  * @group FW NPFG Control
  */
-PARAM_DEFINE_INT32(FW_USE_NPFG, 0);
+PARAM_DEFINE_INT32(FW_USE_NPFG, 1);
 
 /**
  * NPFG period
@@ -230,36 +231,18 @@ PARAM_DEFINE_FLOAT(NPFG_SW_DST_MLT, 0.32f);
 PARAM_DEFINE_FLOAT(NPFG_PERIOD_SF, 1.5f);
 
 /**
- * Cruise throttle
+ * Trim throttle
  *
- * This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7.
+ * This is the throttle setting required to achieve FW_AIRSPD_TRIM during level flight. Most airframes have a value of 0.5-0.7.
  *
  * @unit norm
  * @min 0.0
  * @max 1.0
  * @decimal 2
  * @increment 0.01
- * @group FW L1 Control
+ * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.6f);
-
-/**
- * Scale throttle by pressure change
- *
- * Automatically adjust throttle to account for decreased air density at higher altitudes.
- * Start with a scale factor of 1.0 and adjust for different propulsion systems.
- *
- * When flying without airspeed sensor this will help to keep a constant performance over large altitude ranges.
- *
- * The default value of 0 will disable scaling.
- *
- * @min 0.0
- * @max 10.0
- * @decimal 1
- * @increment 0.1
- * @group FW L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_THR_ALT_SCL, 0.0f);
+PARAM_DEFINE_FLOAT(FW_THR_TRIM, 0.6f);
 
 /**
  * Throttle max slew rate
@@ -268,7 +251,9 @@ PARAM_DEFINE_FLOAT(FW_THR_ALT_SCL, 0.0f);
  *
  * @min 0.0
  * @max 1.0
- * @group FW L1 Control
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f);
 
@@ -282,9 +267,9 @@ PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f);
  * @max 0.0
  * @decimal 1
  * @increment 0.5
- * @group FW L1 Control
+ * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
+PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -30.0f);
 
 /**
  * Maximum pitch angle
@@ -296,9 +281,9 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
  * @max 60.0
  * @decimal 1
  * @increment 0.5
- * @group FW L1 Control
+ * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
+PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 30.0f);
 
 /**
  * Maximum roll angle
@@ -326,7 +311,7 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 50.0f);
  * @max 1.0
  * @decimal 2
  * @increment 0.01
- * @group FW L1 Control
+ * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
 
@@ -347,7 +332,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
  * @max 1.0
  * @decimal 2
  * @increment 0.01
- * @group FW L1 Control
+ * @group FW TECS
  */
 PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
 
@@ -356,32 +341,20 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
  *
  * This is the minimum throttle while on the ground
  *
- * For aircraft with internal combustion engine this parameter should be set
- * above desired idle rpm.
+ * For aircraft with internal combustion engines, this parameter should be set
+ * above the desired idle rpm. For electric motors, idle should typically be set
+ * to zero.
+ *
+ * Note that in automatic modes, "landed" conditions will engage idle throttle.
  *
  * @unit norm
  * @min 0.0
  * @max 0.4
  * @decimal 2
  * @increment 0.01
- * @group FW L1 Control
+ * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_THR_IDLE, 0.15f);
-
-/**
- * Throttle limit during landing below throttle limit altitude
- *
- * During the flare of the autonomous landing process, this value will be set
- * as throttle limit when the aircraft altitude is below FW_LND_TLALT.
- *
- * @unit norm
- * @min 0.0
- * @max 1.0
- * @decimal 2
- * @increment 0.01
- * @group FW L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(FW_THR_IDLE, 0.0f);
 
 /**
  * Climbout Altitude difference
@@ -401,14 +374,17 @@ PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
 PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 10.0f);
 
 /**
- * Landing slope angle
+ * Maximum landing slope angle
+ *
+ * Typically the desired landing slope angle when landing configuration (flaps, airspeed) is enabled.
+ * Set this value within the vehicle's performance limits.
  *
  * @unit deg
  * @min 1.0
  * @max 15.0
  * @decimal 1
  * @increment 0.5
- * @group FW L1 Control
+ * @group FW Auto Landing
  */
 PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
 
@@ -425,68 +401,51 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
 PARAM_DEFINE_FLOAT(FW_TKO_PITCH_MIN, 10.0f);
 
 /**
+ * Takeoff Airspeed
  *
+ * The calibrated airspeed setpoint TECS will stabilize to during the takeoff climbout.
  *
- * @unit m
- * @min 1.0
- * @max 15.0
+ * If set <= 0.0, FW_AIRSPD_MIN will be set by default.
+ *
+ * @unit m/s
+ * @min -1.0
  * @decimal 1
- * @increment 0.5
- * @group FW L1 Control
+ * @increment 0.1
+ * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f);
+PARAM_DEFINE_FLOAT(FW_TKO_AIRSPD, -1.0f);
 
 /**
  * Landing flare altitude (relative to landing altitude)
  *
+ * NOTE: max(FW_LND_FLALT, FW_LND_FL_TIME * |z-velocity|) is taken as the flare altitude
+ *
  * @unit m
  * @min 0.0
- * @max 25.0
  * @decimal 1
  * @increment 0.5
- * @group FW L1 Control
+ * @group FW Auto Landing
  */
-PARAM_DEFINE_FLOAT(FW_LND_FLALT, 3.0f);
+PARAM_DEFINE_FLOAT(FW_LND_FLALT, 0.5f);
 
 /**
- * Landing throttle limit altitude (relative landing altitude)
+ * Use terrain estimation during landing. This is critical for detecting when to flare, and should be enabled if possible.
  *
- * Default of -1.0 lets the system default to applying throttle
- * limiting at 2/3 of the flare altitude.
+ * NOTE: terrain estimate is currently solely derived from a distance sensor.
  *
- * @unit m
- * @min -1.0
- * @max 30.0
- * @decimal 1
- * @increment 0.5
- * @group FW L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
-
-/**
- * Landing heading hold horizontal distance.
+ * If enabled and no measurement is found within a given timeout, the landing waypoint altitude will be used OR the landing
+ * will be aborted, depending on the criteria set in FW_LND_ABORT.
  *
- * Set to 0 to disable heading hold.
+ * If disabled, FW_LND_ABORT terrain based criteria are ignored.
  *
- * @unit m
  * @min 0
- * @max 30.0
- * @decimal 1
- * @increment 0.5
- * @group FW L1 Control
+ * @max 2
+ * @value 0 Disable the terrain estimate
+ * @value 1 Use the terrain estimate to trigger the flare (only)
+ * @value 2 Calculate landing glide slope relative to the terrain estimate
+ * @group FW Auto Landing
  */
-PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
-
-/**
- * Use terrain estimate during landing.
- *
- * This is turned off by default and a waypoint or return altitude is normally used
- * (or sea level for an arbitrary land position).
- *
- * @boolean
- * @group FW L1 Control
- */
-PARAM_DEFINE_INT32(FW_LND_USETER, 0);
+PARAM_DEFINE_INT32(FW_LND_USETER, 1);
 
 /**
  * Early landing configuration deployment
@@ -500,7 +459,7 @@ PARAM_DEFINE_INT32(FW_LND_USETER, 0);
  *
  * @boolean
  *
- * @group FW L1 Control
+ * @group FW Auto Landing
  */
 PARAM_DEFINE_INT32(FW_LND_EARLYCFG, 0);
 
@@ -508,14 +467,14 @@ PARAM_DEFINE_INT32(FW_LND_EARLYCFG, 0);
  * Flare, minimum pitch
  *
  * Minimum pitch during flare, a positive sign means nose up
- * Applied once FW_LND_FLALT is reached
+ * Applied once flaring is triggered
  *
  * @unit deg
  * @min 0
  * @max 15.0
  * @decimal 1
  * @increment 0.5
- * @group FW L1 Control
+ * @group FW Auto Landing
  */
 PARAM_DEFINE_FLOAT(FW_LND_FL_PMIN, 2.5f);
 
@@ -523,14 +482,14 @@ PARAM_DEFINE_FLOAT(FW_LND_FL_PMIN, 2.5f);
  * Flare, maximum pitch
  *
  * Maximum pitch during flare, a positive sign means nose up
- * Applied once FW_LND_FLALT is reached
+ * Applied once flaring is triggered
  *
  * @unit deg
  * @min 0
  * @max 45.0
  * @decimal 1
  * @increment 0.5
- * @group FW L1 Control
+ * @group FW Auto Landing
  */
 PARAM_DEFINE_FLOAT(FW_LND_FL_PMAX, 15.0f);
 
@@ -546,7 +505,7 @@ PARAM_DEFINE_FLOAT(FW_LND_FL_PMAX, 15.0f);
  * @max 1.5
  * @decimal 2
  * @increment 0.01
- * @group FW L1 Control
+ * @group FW Auto Landing
  */
 PARAM_DEFINE_FLOAT(FW_LND_AIRSPD_SC, 1.3f);
 
@@ -562,7 +521,7 @@ PARAM_DEFINE_FLOAT(FW_LND_AIRSPD_SC, 1.3f);
  * @min 0.2
  * @max 1.0
  * @increment 0.1
- * @group FW L1 Control
+ * @group FW Auto Landing
  */
 PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f);
 
@@ -580,6 +539,10 @@ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f);
  * The minimal airspeed (calibrated airspeed) the user is able to command.
  * Further, if the airspeed falls below this value, the TECS controller will try to
  * increase airspeed more aggressively.
+ * Has to be set according to the vehicle's stall speed (which should be set in FW_AIRSPD_STALL),
+ * with some margin between the stall speed and minimum airspeed.
+ * This value corresponds to the desired minimum speed with the default load factor (level flight, default weight),
+ * and is automatically adpated to the current load factor (calculated from roll setpoint and WEIGHT_GROSS/WEIGHT_BASE).
  *
  * @unit m/s
  * @min 0.5
@@ -593,7 +556,8 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
 /**
  * Maximum Airspeed (CAS)
  *
- * If the CAS (calibrated airspeed) is above this value, the TECS controller will try to decrease
+ * The maximal airspeed (calibrated airspeed) the user is able to command.
+ * Further, if the airspeed is above this value, the TECS controller will try to decrease
  * airspeed more aggressively.
  *
  * @unit m/s
@@ -605,22 +569,6 @@ PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 10.0f);
  */
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 20.0f);
 
-/**
- * Cruise Airspeed (CAS)
- *
- * The trim CAS (calibrated airspeed) of the vehicle. If an airspeed controller is active,
- * this is the default airspeed setpoint that the controller will try to achieve if
- * no other airspeed setpoint sources are present (e.g. through non-centered RC sticks).
- *
- * @unit m/s
- * @min 0.5
- * @max 40
- * @decimal 1
- * @increment 0.5
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 15.0f);
-
 /**
  * Stall Airspeed (CAS)
  *
@@ -998,3 +946,268 @@ PARAM_DEFINE_INT32(FW_GPSF_LT, 30);
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f);
+
+/**
+ * Eco Mode: Throttle limit max
+ *
+ * APX4 custom parameter.
+ *
+ * @unit norm
+ * @min 0.0
+ * @max 1.0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_THR_MAX_E, 1.0f);
+
+/**
+ * Eco target climbrate.
+ *
+ * The rate at which the vehicle will climb in autonomous modes to achieve altitude setpoints in Eco mode.
+ * APX4 custom parameter.
+ *
+ * @unit m/s
+ * @min 0.5
+ * @max 15
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP_E, 3.0f);
+
+/**
+ * Eco Mode: Speed <--> Altitude priority
+ *
+ * APX4 custom parameter.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @decimal 1
+ * @increment 1.0
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT_E, 1.8f);
+
+/**
+ * Eco Mode: Altitude error time constant.
+ *
+ * Normally larger than FW_T_ALT_TC to have looser altitude control and in turn less throttle changes.
+ * APX4 custom parameter.
+ *
+ * @min 2.0
+ * @decimal 1
+ * @increment 0.5
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_ALT_TC_E, 10.0f);
+
+/**
+ * Eco TECS mode max altitude undershoot
+ *
+ * Eco mode is disabled if the current altitude is more than this value below the setpoint.
+ * APX4 custom parameter.
+ *
+ * @min 1.0
+ * @max 50.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_ECO_ALT_ERR_U, 10.f);
+
+/**
+ * Eco TECS mode max altitude overshoot
+ *
+ * Eco mode is disabled if the current altitude is more than this value above the setpoint.
+ * APX4 custom parameter.
+ *
+ * @min 1.0
+ * @max 50.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_ECO_ALT_ERR_O, 20.f);
+
+/**
+ * Eco TECS mode min altitude
+ *
+ * Eco mode is disabled if the current altitude is below this relative altitude to home.
+ * APX4 custom parameter.
+ *
+ * @min -200.0
+ * @max 200.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_ECO_ALT_MIN, 50.f);
+
+/**
+ * Airspeed threshold for Eco
+ *
+ * Eco mode is disabled if the airspeed setpoint is above this value.
+ * APX4 custom parameter.
+ *
+ * @min -1.0
+ * @max 50.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_ECO_AD_THRLD, -1.f);
+
+/**
+ * Vehicle base weight.
+ *
+ * This is the weight of the vehicle at which it's performance limits were derived. A zero or negative value
+ * disables trim throttle and minimum airspeed compensation based on weight.
+ *
+ * @unit kg
+ * @decimal 1
+ * @increment 0.5
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(WEIGHT_BASE, -1.0f);
+
+/**
+ * Vehicle gross weight.
+ *
+ * This is the actual weight of the vehicle at any time. This value will differ from WEIGHT_BASE in case weight was added
+ * or removed from the base weight. Examples are the addition of payloads or larger batteries. A zero or negative value
+ * disables trim throttle and minimum airspeed compensation based on weight.
+ *
+ * @unit kg
+ * @decimal 1
+ * @increment 0.1
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(WEIGHT_GROSS, -1.0f);
+
+/**
+ * The aircraft's wing span (length from tip to tip).
+ *
+ * This is used for limiting the roll setpoint near the ground. (if multiple wings, take the longest span)
+ *
+ * @unit m
+ * @min 0.1
+ * @decimal 1
+ * @increment 0.1
+ * @group FW Geometry
+ */
+PARAM_DEFINE_FLOAT(FW_WING_SPAN, 3.0);
+
+/**
+ * Height (AGL) of the wings when the aircraft is on the ground.
+ *
+ * This is used to constrain a minimum altitude below which we keep wings level to avoid wing tip strike. It's safer
+ * to give a slight margin here (> 0m)
+ *
+ * @unit m
+ * @min 0.0
+ * @decimal 1
+ * @increment 1
+ * @group FW Geometry
+ */
+PARAM_DEFINE_FLOAT(FW_WING_HEIGHT, 0.5);
+
+/**
+ * Landing flare time
+ *
+ * Multiplied by the descent rate to calculate a dynamic altitude at which
+ * to trigger the flare.
+ *
+ * NOTE: max(FW_LND_FLALT, FW_LND_FL_TIME * descent rate) is taken as the flare altitude
+ *
+ * @unit s
+ * @min 0.1
+ * @max 5.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW Auto Landing
+ */
+PARAM_DEFINE_FLOAT(FW_LND_FL_TIME, 1.0f);
+
+/**
+ * Landing flare sink rate
+ *
+ * TECS will attempt to control the aircraft to this sink rate via pitch angle (throttle killed during flare)
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 1.0
+ * @decimal 1
+ * @increment 0.1
+ * @group FW Auto Landing
+ */
+PARAM_DEFINE_FLOAT(FW_LND_FL_SINK, 0.25f);
+
+/**
+ * Maximum lateral position offset for the touchdown point
+ *
+ * @unit m
+ * @min 0.0
+ * @max 10.0
+ * @decimal 1
+ * @increment 1
+ * @group FW Auto Landing
+ */
+PARAM_DEFINE_FLOAT(FW_LND_TD_OFF, 3.0);
+
+/**
+ * Landing touchdown nudging option.
+ *
+ * Approach angle nudging: shifts the touchdown point laterally while keeping the approach entrance point constant
+ * Approach path nudging: shifts the touchdown point laterally along with the entire approach path
+ *
+ * This is useful for manually adjusting the landing point in real time when map or GNSS errors cause an offset from the
+ * desired landing vector. Nuding is done with yaw stick, constrained to FW_LND_TD_OFF (in meters) and the direction is
+ * relative to the vehicle heading (stick deflection to the right = land point moves to the right as seen by the vehicle).
+ *
+ * @min 0
+ * @max 2
+ * @value 0 Disable nudging
+ * @value 1 Nudge approach angle
+ * @value 2 Nudge approach path
+ * @group FW Auto Landing
+ */
+PARAM_DEFINE_INT32(FW_LND_NUDGE, 2);
+
+/**
+ * Bit mask to set the automatic landing abort conditions.
+ *
+ * Terrain estimation:
+ * bit 0: Abort if terrain is not found
+ * bit 1: Abort if terrain times out (after a first successful measurement)
+ *
+ * The last estimate is always used as ground, whether the last valid measurement or the land waypoint, depending on the
+ * selected abort criteria, until an abort condition is entered. If FW_LND_USETER == 0, these bits are ignored.
+ *
+ * TODO: Extend automatic abort conditions
+ * e.g. glide slope tracking error (horizontal and vertical)
+ *
+ * @min 0
+ * @max 3
+ * @bit 0 Abort if terrain is not found
+ * @bit 1 Abort if terrain times out (after a first successful measurement)
+ * @group FW Auto Landing
+ */
+PARAM_DEFINE_INT32(FW_LND_ABORT, 3);
+
+/**
+ * Wind-based airspeed scaling factor
+ *
+ * Multiplying this factor with the current absolute wind estimate gives the airspeed offset
+ * added to the minimum airspeed setpoint limit. This helps to make the
+ * system more robust against disturbances (turbulence) in high wind.
+ * Only applies to AUTO flight mode.
+ *
+ * airspeed_min_adjusted = FW_AIRSPD_MIN + FW_WIND_ARSP_SC * wind.length()
+ *
+ * @min 0
+ * @decimal 2
+ * @increment 0.01
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.f);
diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp
index d6aa863ce0..455b78dd30 100644
--- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp
+++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.cpp
@@ -52,210 +52,175 @@ using namespace time_literals;
 namespace runwaytakeoff
 {
 
-RunwayTakeoff::RunwayTakeoff(ModuleParams *parent) :
-	ModuleParams(parent),
-	_state(),
-	_initialized(false),
-	_initialized_time(0),
-	_init_yaw(0),
-	_climbout(false)
+void RunwayTakeoff::init(const hrt_abstime &time_now, const float initial_yaw, const matrix::Vector2d &start_pos_global)
 {
+	initial_yaw_ = initial_yaw;
+	start_pos_global_ = start_pos_global;
+	takeoff_state_ = RunwayTakeoffState::THROTTLE_RAMP;
+	initialized_ = true;
+	time_initialized_ = time_now;
+	takeoff_time_ = 0;
 }
 
-void RunwayTakeoff::init(const hrt_abstime &now, float yaw, double current_lat, double current_lon)
+void RunwayTakeoff::update(const hrt_abstime &time_now, const float takeoff_airspeed, const float calibrated_airspeed,
+			   const float vehicle_altitude, const float clearance_altitude, orb_advert_t *mavlink_log_pub)
 {
-	_init_yaw = yaw;
-	_initialized = true;
-	_state = RunwayTakeoffState::THROTTLE_RAMP;
-	_initialized_time = now;
-	_climbout = true; // this is true until climbout is finished
-	_start_wp(0) = current_lat;
-	_start_wp(1) = current_lon;
-}
-
-void RunwayTakeoff::update(const hrt_abstime &now, float airspeed, float alt_agl,
-			   double current_lat, double current_lon, orb_advert_t *mavlink_log_pub)
-{
-	switch (_state) {
+	switch (takeoff_state_) {
 	case RunwayTakeoffState::THROTTLE_RAMP:
-		if (((now - _initialized_time) > (_param_rwto_ramp_time.get() * 1_s))
-		    || (airspeed > (_param_fw_airspd_min.get() * _param_rwto_airspd_scl.get() * 0.9f))) {
-
-			_state = RunwayTakeoffState::CLAMPED_TO_RUNWAY;
+		if ((time_now - time_initialized_) > (param_rwto_ramp_time_.get() * 1_s)) {
+			takeoff_state_ = RunwayTakeoffState::CLAMPED_TO_RUNWAY;
 		}
 
 		break;
 
+	case RunwayTakeoffState::CLAMPED_TO_RUNWAY: {
+			const float rotation_airspeed = (param_rwto_rot_airspd_.get() > FLT_EPSILON) ? math::min(param_rwto_rot_airspd_.get(),
+							takeoff_airspeed) : 0.9f * takeoff_airspeed;
+
+			if (calibrated_airspeed > rotation_airspeed) {
+				takeoff_time_ = time_now;
+				takeoff_state_ = RunwayTakeoffState::CLIMBOUT;
+				mavlink_log_info(mavlink_log_pub, "Takeoff airspeed reached, climbout\t");
+				events::send(events::ID("runway_takeoff_reached_airspeed"), events::Log::Info,
+					     "Takeoff airspeed reached, climbout");
+			}
+
+			break;
+		}
+
+	case RunwayTakeoffState::CLIMBOUT:
+		if (vehicle_altitude > clearance_altitude) {
+			takeoff_state_ = RunwayTakeoffState::FLY;
+			mavlink_log_info(mavlink_log_pub, "Reached clearance altitude\t");
+			events::send(events::ID("runway_takeoff_reached_clearance_altitude"), events::Log::Info, "Reached clearance altitude");
+		}
+
+		break;
+
+	default:
+		break;
+	}
+}
+
+bool RunwayTakeoff::controlYaw()
+{
+	// keep controlling yaw directly until we start navigation
+	return takeoff_state_ < RunwayTakeoffState::CLIMBOUT;
+}
+
+float RunwayTakeoff::getPitch(float external_pitch_setpoint)
+{
+	if (takeoff_state_ <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
+		return math::radians(param_rwto_psp_.get());
+	}
+
+	return external_pitch_setpoint;
+}
+
+float RunwayTakeoff::getRoll(float external_roll_setpoint)
+{
+	// until we have enough ground clearance, set roll to 0
+	if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
+		return 0.0f;
+	}
+
+	return external_roll_setpoint;
+}
+
+float RunwayTakeoff::getYaw(float external_yaw_setpoint)
+{
+	if (param_rwto_hdg_.get() == 0 && takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
+		return initial_yaw_;
+
+	} else {
+		return external_yaw_setpoint;
+	}
+}
+
+float RunwayTakeoff::getThrottle(const float idle_throttle, const float external_throttle_setpoint) const
+{
+	float throttle = idle_throttle;
+
+	switch (takeoff_state_) {
+	case RunwayTakeoffState::THROTTLE_RAMP:
+
+		throttle = interpolateValuesOverAbsoluteTime(idle_throttle, param_rwto_max_thr_.get(), time_initialized_,
+				param_rwto_ramp_time_.get());
+
+		break;
+
 	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
-		if (airspeed > _param_fw_airspd_min.get() * _param_rwto_airspd_scl.get()) {
-			_state = RunwayTakeoffState::TAKEOFF;
-			mavlink_log_info(mavlink_log_pub, "#Takeoff airspeed reached\t");
-			events::send(events::ID("runway_takeoff_reached_airspeed"), events::Log::Info,
-				     "Takeoff airspeed reached");
-		}
-
-		break;
-
-	case RunwayTakeoffState::TAKEOFF:
-		if (alt_agl > _param_rwto_nav_alt.get()) {
-			_state = RunwayTakeoffState::CLIMBOUT;
-
-			/*
-			 * If we started in heading hold mode, move the navigation start WP to the current location now.
-			 * The navigator will take this as starting point to navigate towards the takeoff WP.
-			 */
-			if (_param_rwto_hdg.get() == 0) {
-				_start_wp(0) = current_lat;
-				_start_wp(1) = current_lon;
-			}
-
-			mavlink_log_info(mavlink_log_pub, "#Climbout\t");
-			events::send(events::ID("runway_takeoff_climbout"), events::Log::Info, "Climbout");
-		}
+		throttle = param_rwto_max_thr_.get();
 
 		break;
 
 	case RunwayTakeoffState::CLIMBOUT:
-		if (alt_agl > _param_fw_clmbout_diff.get()) {
-			_climbout = false;
-			_state = RunwayTakeoffState::FLY;
-			mavlink_log_info(mavlink_log_pub, "#Navigating to waypoint\t");
-			events::send(events::ID("runway_takeoff_nav_to_wp"), events::Log::Info, "Navigating to waypoint");
-		}
+		// ramp in throttle setpoint over takeoff rotation transition time
+		throttle = interpolateValuesOverAbsoluteTime(param_rwto_max_thr_.get(), external_throttle_setpoint, takeoff_time_,
+				param_rwto_rot_time_.get());
 
 		break;
 
-	default:
-		break;
-	}
-}
-
-/*
- * Returns true as long as we're below navigation altitude
- */
-bool RunwayTakeoff::controlYaw()
-{
-	// keep controlling yaw directly until we start navigation
-	return _state < RunwayTakeoffState::CLIMBOUT;
-}
-
-/*
- * Returns pitch setpoint to use.
- *
- * Limited (parameter) as long as the plane is on runway. Otherwise
- * use the one from TECS
- */
-float RunwayTakeoff::getPitch(float tecsPitch)
-{
-	if (_state <= RunwayTakeoffState::CLAMPED_TO_RUNWAY) {
-		return math::radians(_param_rwto_psp.get());
+	case RunwayTakeoffState::FLY:
+		throttle = external_throttle_setpoint;
 	}
 
-	return tecsPitch;
-}
-
-/*
- * Returns the roll setpoint to use.
- */
-float RunwayTakeoff::getRoll(float navigatorRoll)
-{
-	// until we have enough ground clearance, set roll to 0
-	if (_state < RunwayTakeoffState::CLIMBOUT) {
-		return 0.0f;
-	}
-
-	// allow some roll during climbout
-	else if (_state < RunwayTakeoffState::FLY) {
-		return math::constrain(navigatorRoll,
-				       math::radians(-_param_rwto_max_roll.get()),
-				       math::radians(_param_rwto_max_roll.get()));
-	}
-
-	return navigatorRoll;
-}
-
-/*
- * Returns the yaw setpoint to use.
- *
- * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the
- * runway. When it has enough ground clearance we start navigation towards WP.
- */
-float RunwayTakeoff::getYaw(float navigatorYaw)
-{
-	if (_param_rwto_hdg.get() == 0 && _state < RunwayTakeoffState::CLIMBOUT) {
-		return _init_yaw;
-
-	} else {
-		return navigatorYaw;
-	}
-}
-
-/*
- * Returns the throttle setpoint to use.
- *
- * Ramps up in the beginning, until it lifts off the runway it is set to
- * parameter value, then it returns the TECS throttle.
- */
-float RunwayTakeoff::getThrottle(const hrt_abstime &now, float tecsThrottle)
-{
-	switch (_state) {
-	case RunwayTakeoffState::THROTTLE_RAMP: {
-			float throttle = ((now - _initialized_time) / (_param_rwto_ramp_time.get() * 1_s)) * _param_rwto_max_thr.get();
-			return math::min(throttle, _param_rwto_max_thr.get());
-		}
-
-	case RunwayTakeoffState::CLAMPED_TO_RUNWAY:
-		return _param_rwto_max_thr.get();
-
-	default:
-		return tecsThrottle;
-	}
+	return throttle;
 }
 
 bool RunwayTakeoff::resetIntegrators()
 {
 	// reset integrators if we're still on runway
-	return _state < RunwayTakeoffState::TAKEOFF;
+	return takeoff_state_ < RunwayTakeoffState::CLIMBOUT;
 }
 
-/*
- * Returns the minimum pitch for TECS to use.
- *
- * In climbout we either want what was set on the waypoint (sp_min) but at least
- * the climbtout minimum pitch (parameter).
- * Otherwise use the minimum that is enforced generally (parameter).
- */
-float RunwayTakeoff::getMinPitch(float climbout_min, float min)
+float RunwayTakeoff::getMinPitch(float min_pitch_in_climbout, float min_pitch) const
 {
-	if (_state < RunwayTakeoffState::FLY) {
-		return climbout_min;
+	if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
+		// constrain to the taxi pitch setpoint
+		return math::radians(param_rwto_psp_.get() - 0.01f);
+
+	} else if (takeoff_state_ < RunwayTakeoffState::FLY) {
+		// ramp in the climbout pitch constraint over the rotation transition time
+		const float taxi_pitch_min = math::radians(param_rwto_psp_.get() - 0.01f);
+		return interpolateValuesOverAbsoluteTime(taxi_pitch_min, min_pitch_in_climbout, takeoff_time_,
+				param_rwto_rot_time_.get());
 
 	} else {
-		return min;
+		return min_pitch;
 	}
 }
 
-/*
- * Returns the maximum pitch for TECS to use.
- *
- * Limited by parameter (if set) until climbout is done.
- */
-float RunwayTakeoff::getMaxPitch(float max)
+float RunwayTakeoff::getMaxPitch(const float max_pitch) const
 {
-	// use max pitch from parameter if set (> 0.1)
-	if (_state < RunwayTakeoffState::FLY && _param_rwto_max_pitch.get() > 0.1f) {
-		return _param_rwto_max_pitch.get();
-	}
+	if (takeoff_state_ < RunwayTakeoffState::CLIMBOUT) {
+		// constrain to the taxi pitch setpoint
+		return math::radians(param_rwto_psp_.get() + 0.01f);
 
-	else {
-		return max;
+	} else if (takeoff_state_ < RunwayTakeoffState::FLY) {
+		// ramp in the climbout pitch constraint over the rotation transition time
+		const float taxi_pitch_max = math::radians(param_rwto_psp_.get() + 0.01f);
+		return interpolateValuesOverAbsoluteTime(taxi_pitch_max, max_pitch, takeoff_time_, param_rwto_rot_time_.get());
+
+	} else {
+		return max_pitch;
 	}
 }
 
 void RunwayTakeoff::reset()
 {
-	_initialized = false;
-	_state = RunwayTakeoffState::THROTTLE_RAMP;
+	initialized_ = false;
+	takeoff_state_ = RunwayTakeoffState::THROTTLE_RAMP;
+	takeoff_time_ = 0;
 }
 
+float RunwayTakeoff::interpolateValuesOverAbsoluteTime(const float start_value, const float end_value,
+		const hrt_abstime &start_time, const float interpolation_time) const
+{
+	const float seconds_since_start = hrt_elapsed_time(&start_time) / float(1_s);
+	const float interpolator = math::constrain(seconds_since_start / interpolation_time, 0.0f, 1.0f);
+
+	return interpolator * end_value + (1.0f - interpolator) * start_value;
 }
+
+} // namespace runwaytakeoff
diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h
index 796967d682..532a1dd33b 100644
--- a/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h
+++ b/src/modules/fw_pos_control_l1/runway_takeoff/RunwayTakeoff.h
@@ -55,68 +55,189 @@ namespace runwaytakeoff
 {
 
 enum RunwayTakeoffState {
-	THROTTLE_RAMP = 0, /**< ramping up throttle */
-	CLAMPED_TO_RUNWAY = 1, /**< clamped to runway, controlling yaw directly (wheel or rudder) */
-	TAKEOFF = 2, /**< taking off, get ground clearance, roll 0 */
-	CLIMBOUT = 3, /**< climbout to safe height before navigation, roll limited */
-	FLY = 4 /**< fly towards takeoff waypoint */
+	THROTTLE_RAMP = 0, // ramping up throttle
+	CLAMPED_TO_RUNWAY, // clamped to runway, controlling yaw directly (wheel or rudder)
+	CLIMBOUT, // climbout to safe height before navigation
+	FLY // navigate freely
 };
 
 class __EXPORT RunwayTakeoff : public ModuleParams
 {
 public:
-	RunwayTakeoff(ModuleParams *parent);
+	RunwayTakeoff(ModuleParams *parent) : ModuleParams(parent) {}
 	~RunwayTakeoff() = default;
 
-	void init(const hrt_abstime &now, float yaw, double current_lat, double current_lon);
-	void update(const hrt_abstime &now, float airspeed, float alt_agl, double current_lat, double current_lon,
-		    orb_advert_t *mavlink_log_pub);
+	/**
+	 * @brief Initializes the state machine.
+	 *
+	 * @param time_now Absolute time since system boot [us]
+	 * @param initial_yaw Vehicle yaw angle at time of initialization [us]
+	 * @param start_pos_global Vehicle global (lat, lon) position at time of initialization [deg]
+	 */
+	void init(const hrt_abstime &time_now, const float initial_yaw, const matrix::Vector2d &start_pos_global);
 
-	RunwayTakeoffState getState() { return _state; }
-	bool isInitialized() { return _initialized; }
+	/**
+	 * @brief Updates the state machine based on the current vehicle condition.
+	 *
+	 * @param time_now Absolute time since system boot [us]
+	 * @param takeoff_airspeed Calibrated airspeed setpoint for the takeoff climbout [m/s]
+	 * @param calibrated_airspeed Vehicle calibrated airspeed [m/s]
+	 * @param vehicle_altitude Vehicle altitude (AGL) [m]
+	 * @param clearance_altitude Altitude (AGL) above which we have cleared all occlusions in the runway path [m]
+	 * @param mavlink_log_pub
+	 */
+	void update(const hrt_abstime &time_now, const float takeoff_airspeed, const float calibrated_airspeed,
+		    const float vehicle_altitude, const float clearance_altitude, orb_advert_t *mavlink_log_pub);
 
-	bool runwayTakeoffEnabled() { return _param_rwto_tkoff.get(); }
-	float getMinAirspeedScaling() { return _param_rwto_airspd_scl.get(); }
-	float getInitYaw() { return _init_yaw; }
+	/**
+	 * @return Current takeoff state
+	 */
+	RunwayTakeoffState getState() { return takeoff_state_; }
 
+	/**
+	 * @return The state machine is initialized
+	 */
+	bool isInitialized() { return initialized_; }
+
+	/**
+	 * @return Runway takeoff is enabled
+	 */
+	bool runwayTakeoffEnabled() { return param_rwto_tkoff_.get(); }
+
+	/**
+	 * @return Initial vehicle yaw angle [rad]
+	 */
+	float getInitYaw() { return initial_yaw_; }
+
+	/**
+	 * @return The vehicle should control yaw via rudder or nose gear
+	 */
 	bool controlYaw();
-	bool climbout() { return _climbout; }
-	float getPitch(float tecsPitch);
-	float getRoll(float navigatorRoll);
-	float getYaw(float navigatorYaw);
-	float getThrottle(const hrt_abstime &now, float tecsThrottle);
-	bool resetIntegrators();
-	float getMinPitch(float climbout_min, float min);
-	float getMaxPitch(float max);
-	const matrix::Vector2d &getStartWP() const { return _start_wp; };
 
+	/**
+	 * @param external_pitch_setpoint Externally commanded pitch angle setpoint (usually from TECS) [rad]
+	 * @return Pitch angle setpoint (limited while plane is on runway) [rad]
+	 */
+	float getPitch(float external_pitch_setpoint);
+
+	/**
+	 * @param external_roll_setpoint Externally commanded roll angle setpoint (usually from L1) [rad]
+	 * @return Roll angle setpoint [rad]
+	 */
+	float getRoll(float external_roll_setpoint);
+
+	/**
+	 * @brief Returns the appropriate yaw angle setpoint.
+	 *
+	 * In heading hold mode (_heading_mode == 0), it returns initial yaw as long as it's on the runway.
+	 * When it has enough ground clearance we start navigation towards WP.
+	 *
+	 * @param external_yaw_setpoint Externally commanded yaw angle setpoint [rad]
+	 * @return Yaw angle setpoint [rad]
+	 */
+	float getYaw(float external_yaw_setpoint);
+
+	/**
+	 * @brief Returns the throttle setpoint.
+	 *
+	 * Ramps up over RWTO_RAMP_TIME to RWTO_MAX_THR until the aircraft lifts off the runway and then
+	 * ramps from RWTO_MAX_THR to the externally defined throttle setting over the takeoff rotation time
+	 *
+	 * @param idle_throttle normalized [0,1]
+	 * @param external_throttle_setpoint Externally commanded throttle setpoint (usually from TECS), normalized [0,1]
+	 * @return Throttle setpoint, normalized [0,1]
+	 */
+	float getThrottle(const float idle_throttle, const float external_throttle_setpoint) const;
+
+	/**
+	 * @param min_pitch_in_climbout Minimum pitch angle during climbout [rad]
+	 * @param min_pitch Externally commanded minimum pitch angle [rad]
+	 * @return Minimum pitch angle [rad]
+	 */
+	float getMinPitch(const float min_pitch_in_climbout, const float min_pitch) const;
+
+	/**
+	 * @param max_pitch Externally commanded maximum pitch angle [rad]
+	 * @return Maximum pitch angle [rad]
+	 */
+	float getMaxPitch(const float max_pitch) const;
+
+	/**
+	 * @return Runway takeoff starting position in global frame (lat, lon) [deg]
+	 */
+	const matrix::Vector2d &getStartPosition() const { return start_pos_global_; };
+
+	// NOTE: this is only to be used for mistaken mode transitions to takeoff while already in air
+	void forceSetFlyState() { takeoff_state_ = RunwayTakeoffState::FLY; }
+
+	/**
+	 * @return If the attitude / rate control integrators should be continually reset.
+	 * This is the case during ground roll.
+	 */
+	bool resetIntegrators();
+
+	/**
+	 * @brief Reset the state machine.
+	 */
 	void reset();
 
+	/**
+	 * @brief Linearly interpolates between a start and end value over an absolute time span.
+	 *
+	 * @param start_value
+	 * @param end_value
+	 * @param start_time Absolute start time [us]
+	 * @param interpolation_time The time span to interpolate over [s]
+	 * @return interpolated value
+	 */
+	float interpolateValuesOverAbsoluteTime(const float start_value, const float end_value, const hrt_abstime &start_time,
+						const float interpolation_time) const;
+
 private:
-	/** state variables **/
-	RunwayTakeoffState _state{THROTTLE_RAMP};
-	bool _initialized{false};
-	hrt_abstime _initialized_time{0};
-	float _init_yaw{0.f};
-	bool _climbout{false};
-	matrix::Vector2d _start_wp;
+	/**
+	 * Current state of runway takeoff procedure
+	 */
+	RunwayTakeoffState takeoff_state_{THROTTLE_RAMP};
+
+	/**
+	 * True if the runway state machine is initialized
+	 */
+	bool initialized_{false};
+
+	/**
+	 * The absolute time since system boot at which the state machine was intialized [us]
+	 */
+	hrt_abstime time_initialized_{0};
+
+	/**
+	 * The absolute time the takeoff state transitions from CLAMPED_TO_RUNWAY -> CLIMBOUT [us]
+	 */
+	hrt_abstime takeoff_time_{0};
+
+	/**
+	 * Initial yaw of the vehicle on first pass through the runway takeoff state machine.
+	 * used for heading hold mode. [rad]
+	 */
+	float initial_yaw_{0.f};
+
+	/**
+	 * The global (lat, lon) position of the vehicle on first pass through the runway takeoff state machine. The
+	 * takeoff path emanates from this point to correct for any GNSS uncertainty from the planned takeoff point. The
+	 * vehicle should accordingly be set on the center of the runway before engaging the mission. [deg]
+	 */
+	matrix::Vector2d start_pos_global_{};
 
 	DEFINE_PARAMETERS(
-		(ParamBool<px4::params::RWTO_TKOFF>) _param_rwto_tkoff,
-		(ParamInt<px4::params::RWTO_HDG>) _param_rwto_hdg,
-		(ParamFloat<px4::params::RWTO_NAV_ALT>) _param_rwto_nav_alt,
-		(ParamFloat<px4::params::RWTO_MAX_THR>) _param_rwto_max_thr,
-		(ParamFloat<px4::params::RWTO_PSP>) _param_rwto_psp,
-		(ParamFloat<px4::params::RWTO_MAX_PITCH>) _param_rwto_max_pitch,
-		(ParamFloat<px4::params::RWTO_MAX_ROLL>) _param_rwto_max_roll,
-		(ParamFloat<px4::params::RWTO_AIRSPD_SCL>) _param_rwto_airspd_scl,
-		(ParamFloat<px4::params::RWTO_RAMP_TIME>) _param_rwto_ramp_time,
-		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
-		(ParamFloat<px4::params::FW_CLMBOUT_DIFF>) _param_fw_clmbout_diff
+		(ParamBool<px4::params::RWTO_TKOFF>) param_rwto_tkoff_,
+		(ParamInt<px4::params::RWTO_HDG>) param_rwto_hdg_,
+		(ParamFloat<px4::params::RWTO_MAX_THR>) param_rwto_max_thr_,
+		(ParamFloat<px4::params::RWTO_PSP>) param_rwto_psp_,
+		(ParamFloat<px4::params::RWTO_RAMP_TIME>) param_rwto_ramp_time_,
+		(ParamFloat<px4::params::RWTO_ROT_AIRSPD>) param_rwto_rot_airspd_,
+		(ParamFloat<px4::params::RWTO_ROT_TIME>) param_rwto_rot_time_
 	)
-
 };
 
-}
+} // namespace runwaytakeoff
 
 #endif // RUNWAYTAKEOFF_H
diff --git a/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c b/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c
index 5f165d06ca..7f791a01c6 100644
--- a/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c
+++ b/src/modules/fw_pos_control_l1/runway_takeoff/runway_takeoff_params.c
@@ -48,34 +48,20 @@
 PARAM_DEFINE_INT32(RWTO_TKOFF, 0);
 
 /**
- * Specifies which heading should be held during runnway takeoff.
+ * Specifies which heading should be held during the runway takeoff ground roll.
  *
- * 0: airframe heading, 1: heading towards takeoff waypoint
+ * 0: airframe heading when takeoff is initiated
+ * 1: position control along runway direction (bearing defined from vehicle position on takeoff initiation to MAV_CMD_TAKEOFF
+ *    position defined by operator)
  *
  * @value 0 Airframe
- * @value 1 Waypoint
+ * @value 1 Runway
  * @min 0
  * @max 1
  * @group Runway Takeoff
  */
 PARAM_DEFINE_INT32(RWTO_HDG, 0);
 
-/**
- * Altitude AGL at which we have enough ground clearance to allow some roll.
- *
- * Until RWTO_NAV_ALT is reached the plane is held level and only
- * rudder is used to keep the heading (see RWTO_HDG). This should be below
- * FW_CLMBOUT_DIFF if FW_CLMBOUT_DIFF > 0.
- *
- * @unit m
- * @min 0.0
- * @max 100.0
- * @decimal 1
- * @increment 1
- * @group Runway Takeoff
- */
-PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0);
-
 /**
  * Max throttle during runway takeoff.
  *
@@ -91,7 +77,7 @@ PARAM_DEFINE_FLOAT(RWTO_NAV_ALT, 5.0);
 PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0);
 
 /**
- * Pitch setpoint during taxi / before takeoff airspeed is reached.
+ * Pitch setpoint during taxi / before takeoff rotation airspeed is reached.
  *
  * A taildragger with steerable wheel might need to pitch up
  * a little to keep its wheel on the ground before airspeed
@@ -106,51 +92,6 @@ PARAM_DEFINE_FLOAT(RWTO_MAX_THR, 1.0);
  */
 PARAM_DEFINE_FLOAT(RWTO_PSP, 0.0);
 
-/**
- * Max pitch during takeoff.
- *
- * Fixed-wing settings are used if set to 0. Note that there is also a minimum
- * pitch of 10 degrees during takeoff, so this must be larger if set.
- *
- * @unit deg
- * @min 0.0
- * @max 60.0
- * @decimal 1
- * @increment 0.5
- * @group Runway Takeoff
- */
-PARAM_DEFINE_FLOAT(RWTO_MAX_PITCH, 20.0);
-
-/**
- * Max roll during climbout.
- *
- * Roll is limited during climbout to ensure enough lift and prevents aggressive
- * navigation before we're on a safe height.
- *
- * @unit deg
- * @min 0.0
- * @max 60.0
- * @decimal 1
- * @increment 0.5
- * @group Runway Takeoff
- */
-PARAM_DEFINE_FLOAT(RWTO_MAX_ROLL, 25.0);
-
-/**
- * Min airspeed scaling factor for takeoff.
- *
- * Pitch up will be commanded when the following airspeed is reached:
- * FW_AIRSPD_MIN * RWTO_AIRSPD_SCL
- *
- * @unit norm
- * @min 0.0
- * @max 2.0
- * @decimal 2
- * @increment 0.01
- * @group Runway Takeoff
- */
-PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3);
-
 /**
  * Throttle ramp up time for runway takeoff
  *
@@ -162,3 +103,55 @@ PARAM_DEFINE_FLOAT(RWTO_AIRSPD_SCL, 1.3);
  * @group Runway Takeoff
  */
 PARAM_DEFINE_FLOAT(RWTO_RAMP_TIME, 2.0f);
+
+/**
+ * L1 period while steering on runway
+ *
+ * @unit s
+ * @min 1.0
+ * @max 100.0
+ * @decimal 1
+ * @increment 0.1
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_L1_PERIOD, 5.0f);
+
+/**
+ * Enable use of yaw stick for nudging the wheel during runway ground roll
+ *
+ * This is useful when map, GNSS, or yaw errors on ground are misaligned with what the operator intends for takeoff course.
+ * Particularly useful for skinny runways or if the wheel servo is a bit off trim.
+ *
+ * @boolean
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_INT32(RWTO_NUDGE, 1);
+
+/**
+ * Takeoff rotation airspeed
+ *
+ * The calibrated airspeed threshold during the takeoff ground roll when the plane should start rotating (pitching up).
+ * Must be less than the takeoff airspeed, will otherwise be capped at the takeoff airpeed (see FW_TKO_AIRSPD).
+ *
+ * If set <= 0.0, defaults to 0.9 * takeoff airspeed (see FW_TKO_AIRSPD)
+ *
+ * @unit m/s
+ * @min -1.0
+ * @decimal 1
+ * @increment 0.1
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_ROT_AIRSPD, -1.0f);
+
+/**
+ * Takeoff rotation time
+ *
+ * This is the time desired to linearly ramp in takeoff pitch constraints during the takeoff rotation
+ *
+ * @unit s
+ * @min 0.1
+ * @decimal 1
+ * @increment 0.1
+ * @group Runway Takeoff
+ */
+PARAM_DEFINE_FLOAT(RWTO_ROT_TIME, 1.0f);
diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp
index 0276b433f5..7f40e5c983 100644
--- a/src/modules/gimbal/gimbal.cpp
+++ b/src/modules/gimbal/gimbal.cpp
@@ -250,11 +250,7 @@ static int gimbal_thread_main(int argc, char *argv[])
 					break;
 
 				case InputBase::UpdateResult::UpdatedNotActive:
-					if (already_active) {
-						// No longer active
-						thread_data.last_input_active = -1;
-					}
-
+					// Ignore, input not active
 					break;
 				}
 
diff --git a/src/modules/gimbal/input_mavlink.cpp b/src/modules/gimbal/input_mavlink.cpp
index 4418b9a9fc..58c83298f9 100644
--- a/src/modules/gimbal/input_mavlink.cpp
+++ b/src/modules/gimbal/input_mavlink.cpp
@@ -360,7 +360,7 @@ void InputMavlinkCmdMount::_ack_vehicle_command(const vehicle_command_s &cmd)
 
 	vehicle_command_ack.timestamp = hrt_absolute_time();
 	vehicle_command_ack.command = cmd.command;
-	vehicle_command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+	vehicle_command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 	vehicle_command_ack.target_system = cmd.source_system;
 	vehicle_command_ack.target_component = cmd.source_component;
 
@@ -729,7 +729,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_
 			break;
 		}
 
-		_ack_vehicle_command(vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+		_ack_vehicle_command(vehicle_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 		return update_result;
 
 	} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {
@@ -763,7 +763,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_
 			}
 		}
 
-		_ack_vehicle_command(vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+		_ack_vehicle_command(vehicle_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 		return UpdateResult::UpdatedActive;
 
 	} else if (vehicle_command.command ==
@@ -828,7 +828,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_
 			}
 		}();
 
-		_ack_vehicle_command(vehicle_command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+		_ack_vehicle_command(vehicle_command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 		if (new_sysid_primary_control != control_data.sysid_primary_control ||
 		    new_compid_primary_control != control_data.compid_primary_control) {
@@ -861,7 +861,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_
 
 			_set_control_data_from_set_attitude(control_data, flags, q, angular_velocity);
 			_ack_vehicle_command(vehicle_command,
-					     vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+					     vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 			return UpdateResult::UpdatedActive;
 
@@ -871,7 +871,7 @@ InputMavlinkGimbalV2::_process_command(ControlData &control_data, const vehicle_
 				 vehicle_command.source_component,
 				 control_data.sysid_primary_control, control_data.compid_primary_control);
 			_ack_vehicle_command(vehicle_command,
-					     vehicle_command_s::VEHICLE_CMD_RESULT_DENIED);
+					     vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
 
 			return UpdateResult::UpdatedNotActive;
 		}
diff --git a/src/modules/gyro_calibration/GyroCalibration.cpp b/src/modules/gyro_calibration/GyroCalibration.cpp
index 51cb743628..279284c554 100644
--- a/src/modules/gyro_calibration/GyroCalibration.cpp
+++ b/src/modules/gyro_calibration/GyroCalibration.cpp
@@ -153,6 +153,7 @@ void GyroCalibration::Run()
 			}
 
 			if (_gyro_calibration[gyro].device_id() == sensor_gyro.device_id) {
+				_gyro_calibration[gyro].SensorCorrectionsUpdate();
 				const Vector3f val{Vector3f{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z} - _gyro_calibration[gyro].thermal_offset()};
 				_gyro_mean[gyro].update(val);
 				_gyro_last_update[gyro] = sensor_gyro.timestamp;
diff --git a/src/modules/gyro_calibration/GyroCalibration.hpp b/src/modules/gyro_calibration/GyroCalibration.hpp
index 9d1047d6b3..7d931a0663 100644
--- a/src/modules/gyro_calibration/GyroCalibration.hpp
+++ b/src/modules/gyro_calibration/GyroCalibration.hpp
@@ -93,7 +93,7 @@ private:
 	uORB::SubscriptionMultiArray<sensor_gyro_s, MAX_SENSORS>  _sensor_gyro_subs{ORB_ID::sensor_gyro};
 
 	calibration::Gyroscope _gyro_calibration[MAX_SENSORS] {};
-	math::WelfordMean<matrix::Vector3f> _gyro_mean[MAX_SENSORS] {};
+	math::WelfordMean<float, 3> _gyro_mean[MAX_SENSORS] {};
 	float _temperature[MAX_SENSORS] {};
 	hrt_abstime _gyro_last_update[MAX_SENSORS] {};
 
diff --git a/src/modules/gyro_calibration/parameters.c b/src/modules/gyro_calibration/parameters.c
index dee9df9b74..a936249c37 100644
--- a/src/modules/gyro_calibration/parameters.c
+++ b/src/modules/gyro_calibration/parameters.c
@@ -38,4 +38,4 @@
 * @reboot_required true
 * @group Sensors
 */
-PARAM_DEFINE_INT32(IMU_GYRO_CAL_EN, 1);
+PARAM_DEFINE_INT32(IMU_GYRO_CAL_EN, 0);
diff --git a/src/modules/land_detector/LandDetector.cpp b/src/modules/land_detector/LandDetector.cpp
index f16e8ed383..4ae554f1b8 100644
--- a/src/modules/land_detector/LandDetector.cpp
+++ b/src/modules/land_detector/LandDetector.cpp
@@ -57,6 +57,7 @@ LandDetector::LandDetector() :
 	_land_detected.has_low_throttle = false;
 	_land_detected.vertical_movement = false;
 	_land_detected.horizontal_movement = false;
+	_land_detected.rotational_movement = false;
 	_land_detected.close_to_ground_or_skipped_check = true;
 	_land_detected.at_rest = true;
 }
@@ -174,6 +175,7 @@ void LandDetector::Run()
 		_land_detected.has_low_throttle = _get_has_low_throttle();
 		_land_detected.horizontal_movement = _get_horizontal_movement();
 		_land_detected.vertical_movement = _get_vertical_movement();
+		_land_detected.rotational_movement = _get_rotational_movement();
 		_land_detected.close_to_ground_or_skipped_check = _get_close_to_ground_or_skipped_check();
 		_land_detected.at_rest = at_rest;
 		_land_detected.timestamp = hrt_absolute_time();
diff --git a/src/modules/land_detector/LandDetector.h b/src/modules/land_detector/LandDetector.h
index 26e74eacd4..6c8247b84e 100644
--- a/src/modules/land_detector/LandDetector.h
+++ b/src/modules/land_detector/LandDetector.h
@@ -138,6 +138,7 @@ protected:
 	virtual bool _get_has_low_throttle() { return false; }
 	virtual bool _get_horizontal_movement() { return false; }
 	virtual bool _get_vertical_movement() { return false; }
+	virtual bool _get_rotational_movement() { return false; }
 	virtual bool _get_close_to_ground_or_skipped_check() {  return false; }
 	virtual void _set_hysteresis_factor(const int factor) = 0;
 
diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp
index eb21bb3bbb..9786a48f46 100644
--- a/src/modules/land_detector/MulticopterLandDetector.cpp
+++ b/src/modules/land_detector/MulticopterLandDetector.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -75,12 +75,13 @@ namespace land_detector
 
 MulticopterLandDetector::MulticopterLandDetector()
 {
-	_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
 	_paramHandle.minManThrottle = param_find("MPC_MANTHR_MIN");
 	_paramHandle.minThrottle = param_find("MPC_THR_MIN");
 	_paramHandle.useHoverThrustEstimate = param_find("MPC_USE_HTE");
 	_paramHandle.hoverThrottle = param_find("MPC_THR_HOVER");
+	_paramHandle.landSpeed = param_find("MPC_LAND_SPEED");
 	_paramHandle.crawlSpeed = param_find("MPC_LAND_CRWL");
+	_minimum_thrust_8s_hysteresis.set_hysteresis_time_from(false, 8_s);
 }
 
 void MulticopterLandDetector::_update_topics()
@@ -122,11 +123,14 @@ void MulticopterLandDetector::_update_params()
 	param_get(_paramHandle.landSpeed, &_params.landSpeed);
 	param_get(_paramHandle.crawlSpeed, &_params.crawlSpeed);
 
-	if (_param_lndmc_z_vel_max.get() > _params.landSpeed) {
-		PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_SPEED, updating %.3f -> %.3f",
-			(double)_param_lndmc_z_vel_max.get(), (double)_params.landSpeed);
+	// 1.2 corresponds to the margin between the default parameters LNDMC_Z_VEL_MAX = MPC_LAND_CRWL / 1.2
+	const float lndmc_upper_threshold = math::min(_params.crawlSpeed, _params.landSpeed) / 1.2f;
 
-		_param_lndmc_z_vel_max.set(_params.landSpeed);
+	if (_param_lndmc_z_vel_max.get() > lndmc_upper_threshold) {
+		PX4_ERR("LNDMC_Z_VEL_MAX > MPC_LAND_CRWL or MPC_LAND_SPEED, updating %.3f -> %.3f",
+			(double)_param_lndmc_z_vel_max.get(), (double)(lndmc_upper_threshold));
+
+		_param_lndmc_z_vel_max.set(lndmc_upper_threshold);
 		_param_lndmc_z_vel_max.commit_no_notification();
 	}
 
@@ -158,22 +162,19 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 
 	const bool lpos_available = ((time_now_us - _vehicle_local_position.timestamp) < 1_s);
 
-	// land speed threshold, 90% of MPC_LAND_SPEED
-	const float crawl_speed_threshold = 0.9f * math::max(_params.crawlSpeed, 0.1f);
-
 	if (lpos_available && _vehicle_local_position.v_z_valid) {
-		// Check if we are moving vertically - this might see a spike after arming due to
-		// throttle-up vibration. If accelerating fast the throttle thresholds will still give
-		// an accurate in-air indication.
-		float max_climb_rate = math::min(crawl_speed_threshold * 0.5f, _param_lndmc_z_vel_max.get());
+		// Check if we are moving vertically.
+		// Use wider threshold if currently in "maybe landed" state, as estimation for
+		// vertical speed is often deteriorated when on the ground or due to propeller
+		// up/down throttling.
 
-		if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
-			// Widen acceptance thresholds for landed state right after arming
-			// so that motor spool-up and other effects do not trigger false negatives.
-			max_climb_rate = _param_lndmc_z_vel_max.get() * 2.5f;
+		float vertical_velocity_threshold = _param_lndmc_z_vel_max.get();
+
+		if (_landed_hysteresis.get_state()) {
+			vertical_velocity_threshold *= 2.5f;
 		}
 
-		_vertical_movement = (fabsf(_vehicle_local_position.vz) > max_climb_rate);
+		_vertical_movement = (fabsf(_vehicle_local_position.vz) > vertical_velocity_threshold);
 
 	} else {
 		_vertical_movement = true;
@@ -212,12 +213,12 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	// if we have a valid velocity setpoint and the vehicle is demanded to go down but no vertical movement present,
 	// we then can assume that the vehicle hit ground
 	if (_flag_control_climb_rate_enabled) {
-		vehicle_local_position_setpoint_s trajectory_setpoint;
+		trajectory_setpoint_s trajectory_setpoint;
 
 		if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
 			// Setpoints can be NAN
-			_in_descend = PX4_ISFINITE(trajectory_setpoint.vz)
-				      && (trajectory_setpoint.vz >= crawl_speed_threshold);
+			_in_descend = PX4_ISFINITE(trajectory_setpoint.velocity[2])
+				      && (trajectory_setpoint.velocity[2] >= 1.1f * _param_lndmc_z_vel_max.get());
 		}
 
 		// ground contact requires commanded descent until landed
@@ -235,93 +236,55 @@ bool MulticopterLandDetector::_get_ground_contact_state()
 	const bool skip_close_to_ground_check = !_dist_bottom_is_observable || !_vehicle_local_position.dist_bottom_valid;
 	_close_to_ground_or_skipped_check = _is_close_to_ground() || skip_close_to_ground_check;
 
-	// When not armed, consider to have ground-contact
-	if (!_armed) {
-		return true;
-	}
-
 	// TODO: we need an accelerometer based check for vertical movement for flying without GPS
-	return _close_to_ground_or_skipped_check && ground_contact && !_horizontal_movement
-	       && !_vertical_movement;
+	return !_armed ||
+	       (_close_to_ground_or_skipped_check && ground_contact
+		&& !_horizontal_movement && !_vertical_movement);
 }
 
 bool MulticopterLandDetector::_get_maybe_landed_state()
 {
-	// When not armed, consider to be maybe-landed
-	if (!_armed) {
-		return true;
-	}
+	hrt_abstime now = hrt_absolute_time();
 
-	const hrt_abstime time_now_us = hrt_absolute_time();
+	float minimum_thrust_threshold{0.f};
 
-	// minimal throttle: initially 10% of throttle range between min and hover
-	float sys_min_throttle = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;
-
-	// Determine the system min throttle based on flight mode
-	if (!_flag_control_climb_rate_enabled) {
-		sys_min_throttle = (_params.minManThrottle + 0.01f);
-	}
-
-	// Check if thrust output is less than the minimum throttle.
-	if (_actuator_controls_throttle <= sys_min_throttle) {
-		if (_min_thrust_start == 0) {
-			_min_thrust_start = time_now_us;
-		}
+	if (_flag_control_climb_rate_enabled) {
+		// 10% of throttle range between min and hover
+		minimum_thrust_threshold = _params.minThrottle + (_params.hoverThrottle - _params.minThrottle) * 0.1f;
 
 	} else {
-		_min_thrust_start = 0;
-		return false;
+		minimum_thrust_threshold = (_params.minManThrottle + 0.01f);
 	}
 
+	const bool minimum_thrust_now = _actuator_controls_throttle <= minimum_thrust_threshold;
+	_minimum_thrust_8s_hysteresis.set_state_and_update(minimum_thrust_now, now);
 
-	if (_freefall_hysteresis.get_state()) {
-		return false;
+	// Next look if vehicle is not rotating (do not consider yaw)
+	float max_rotation_threshold = math::radians(_param_lndmc_rot_max.get());
+
+	// Widen max rotation thresholds if either in landed state, thus making it harder
+	// to trigger a false positive !landed e.g. due to propeller throttling up/down.
+	if (_landed_hysteresis.get_state()) {
+		max_rotation_threshold *= 2.5f;
 	}
 
-
-	float landThresholdFactor = 1.f;
-
-	// Widen acceptance thresholds for landed state right after landed
-	if ((time_now_us - _landed_time) < LAND_DETECTOR_LAND_PHASE_TIME_US) {
-		landThresholdFactor = 2.5f;
-	}
-
-	// Next look if all rotation angles are not moving.
-	const float max_rotation_scaled = math::radians(_param_lndmc_rot_max.get()) * landThresholdFactor;
-
-	matrix::Vector2f angular_velocity{_angular_velocity(0), _angular_velocity(1)};
-
-	if (angular_velocity.norm() > max_rotation_scaled) {
-		return false;
-	}
+	_rotational_movement = _angular_velocity.xy().norm() > max_rotation_threshold;
 
 	// If vertical velocity is available: ground contact, no thrust, no movement -> landed
-	if (((time_now_us - _vehicle_local_position.timestamp) < 1_s) && _vehicle_local_position.v_z_valid) {
-		return _ground_contact_hysteresis.get_state();
-	}
+	const bool local_position_updated = (now - _vehicle_local_position.timestamp) < 1_s;
+	const bool vertical_velocity_valid = _vehicle_local_position.v_z_valid;
+	const bool vertical_estimate = local_position_updated && vertical_velocity_valid;
 
-	// Otherwise, landed if the system has minimum thrust (manual or in failsafe) and no rotation for at least 8 seconds
-	return (_min_thrust_start > 0) && ((time_now_us - _min_thrust_start) > 8_s);
+	return !_armed ||
+	       (minimum_thrust_now && !_freefall_hysteresis.get_state() && !_rotational_movement
+		&& ((vertical_estimate && _ground_contact_hysteresis.get_state())
+		    || (!vertical_estimate && _minimum_thrust_8s_hysteresis.get_state())));
 }
 
 bool MulticopterLandDetector::_get_landed_state()
 {
-	// reset the landed_time
-	if (!_maybe_landed_hysteresis.get_state()) {
-		_landed_time = 0;
-
-	} else if (_landed_time == 0) {
-		_landed_time = hrt_absolute_time();
-	}
-
-	// When not armed, consider to be landed
-	if (!_armed) {
-		return true;
-	}
-
-	// if we have maybe_landed, the mc_pos_control goes into idle (thrust_sp = 0.0)
-	// therefore check if all other condition of the landed state remain true
-	return _maybe_landed_hysteresis.get_state();
+	// all maybe_landed conditions need to hold longer
+	return !_armed || _maybe_landed_hysteresis.get_state();
 }
 
 bool MulticopterLandDetector::_get_ground_effect_state()
diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h
index 855d6ff52d..785a6ada3f 100644
--- a/src/modules/land_detector/MulticopterLandDetector.h
+++ b/src/modules/land_detector/MulticopterLandDetector.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -46,7 +46,6 @@
 #include <uORB/topics/hover_thrust_estimate.h>
 #include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/takeoff_status.h>
 
 #include "LandDetector.h"
@@ -75,6 +74,7 @@ protected:
 	bool _get_has_low_throttle() override { return _has_low_throttle; }
 	bool _get_horizontal_movement() override { return _horizontal_movement; }
 	bool _get_vertical_movement() override { return _vertical_movement; }
+	bool _get_rotational_movement() override { return _rotational_movement; }
 	bool _get_close_to_ground_or_skipped_check() override { return _close_to_ground_or_skipped_check; }
 
 	void _set_hysteresis_factor(const int factor) override;
@@ -84,29 +84,25 @@ private:
 	/** Time in us that freefall has to hold before triggering freefall */
 	static constexpr hrt_abstime FREEFALL_TRIGGER_TIME_US = 300_ms;
 
-	/** Time interval in us in which wider acceptance thresholds are used after landed. */
-	static constexpr hrt_abstime LAND_DETECTOR_LAND_PHASE_TIME_US = 2_s;
-
 	/** Distance above ground below which entering ground contact state is possible when distance to ground is available. */
 	static constexpr float DIST_FROM_GROUND_THRESHOLD = 1.0f;
 
-	/** Handles for interesting parameters. **/
 	struct {
 		param_t minThrottle;
 		param_t hoverThrottle;
 		param_t minManThrottle;
+		param_t useHoverThrustEstimate;
 		param_t landSpeed;
 		param_t crawlSpeed;
-		param_t useHoverThrustEstimate;
 	} _paramHandle{};
 
 	struct {
 		float minThrottle;
 		float hoverThrottle;
 		float minManThrottle;
+		bool useHoverThrustEstimate;
 		float landSpeed;
 		float crawlSpeed;
-		bool useHoverThrustEstimate;
 	} _params{};
 
 	uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)};
@@ -126,12 +122,12 @@ private:
 
 	uint8_t _takeoff_state{takeoff_status_s::TAKEOFF_STATE_DISARMED};
 
-	hrt_abstime _min_thrust_start{0};	///< timestamp when minimum trust was applied first
-	hrt_abstime _landed_time{0};
+	systemlib::Hysteresis _minimum_thrust_8s_hysteresis{false};
 
 	bool _in_descend{false};		///< vehicle is commanded to desend
 	bool _horizontal_movement{false};	///< vehicle is moving horizontally
 	bool _vertical_movement{false};
+	bool _rotational_movement{false};
 	bool _has_low_throttle{false};
 	bool _close_to_ground_or_skipped_check{false};
 	bool _below_gnd_effect_hgt{false};	///< vehicle height above ground is below height where ground effect occurs
diff --git a/src/modules/land_detector/VtolLandDetector.cpp b/src/modules/land_detector/VtolLandDetector.cpp
index a3a4ae3380..22f859c13c 100644
--- a/src/modules/land_detector/VtolLandDetector.cpp
+++ b/src/modules/land_detector/VtolLandDetector.cpp
@@ -69,31 +69,7 @@ bool VtolLandDetector::_get_landed_state()
 		return !_armed;
 	}
 
-	// this is returned from the mutlicopter land detector
-	bool landed = MulticopterLandDetector::_get_landed_state();
-
-	// for vtol we additionally consider airspeed
-	airspeed_validated_s airspeed_validated{};
-	_airspeed_validated_sub.copy(&airspeed_validated);
-
-	if (hrt_elapsed_time(&airspeed_validated.timestamp) < 1_s && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)) {
-
-		_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * airspeed_validated.true_airspeed_m_s;
-
-	} else {
-		// if airspeed does not update, set it to zero and rely on multicopter land detector
-		_airspeed_filtered = 0.0f;
-	}
-
-	// only consider airspeed if we have been in air before to avoid false
-	// detections in the case of wind on the ground
-	if (_was_in_air && (_airspeed_filtered > _param_lndfw_airspd_max.get())) {
-		landed = false;
-	}
-
-	_was_in_air = !landed;
-
-	return landed;
+	return MulticopterLandDetector::_get_landed_state();
 }
 
 bool VtolLandDetector::_get_freefall_state()
diff --git a/src/modules/land_detector/VtolLandDetector.h b/src/modules/land_detector/VtolLandDetector.h
index 16b34b6ce7..e6141cf882 100644
--- a/src/modules/land_detector/VtolLandDetector.h
+++ b/src/modules/land_detector/VtolLandDetector.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -33,7 +33,9 @@
 
 /**
  * @file VtolLandDetector.h
- * Land detection implementation for VTOL also called hybrids.
+ * Land detection implementation for VTOLs.
+ * It uses the MC land detector in hover, while land detection in FW
+ * is 1:1 linked to the (boolean) armed state.
  *
  * @author Roman Bapst <bapstr@gmail.com>
  * @author Julian Oes <julian@oes.ch>
@@ -41,8 +43,6 @@
 
 #pragma once
 
-#include <uORB/topics/airspeed_validated.h>
-
 #include "MulticopterLandDetector.h"
 
 namespace land_detector
@@ -59,17 +59,6 @@ protected:
 	bool _get_landed_state() override;
 	bool _get_maybe_landed_state() override;
 	bool _get_freefall_state() override;
-
-private:
-	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
-
-	bool _was_in_air{false}; /**< indicates whether the vehicle was in the air in the previous iteration */
-	float _airspeed_filtered{0.0f}; /**< low pass filtered airspeed */
-
-	DEFINE_PARAMETERS_CUSTOM_PARENT(
-		MulticopterLandDetector,
-		(ParamFloat<px4::params::LNDFW_AIRSPD_MAX>) _param_lndfw_airspd_max
-	);
 };
 
 } // namespace land_detector
diff --git a/src/modules/land_detector/land_detector_params_mc.c b/src/modules/land_detector/land_detector_params_mc.c
index 3c979ac767..b68b1255bf 100644
--- a/src/modules/land_detector/land_detector_params_mc.c
+++ b/src/modules/land_detector/land_detector_params_mc.c
@@ -48,16 +48,20 @@
 PARAM_DEFINE_FLOAT(LNDMC_TRIG_TIME, 1.0f);
 
 /**
- * Multicopter max climb rate
+ * Multicopter vertical velocity threshold
  *
- * Maximum vertical velocity allowed in the landed state
+ * Vertical velocity threshold to detect landing.
+ * Has to be set lower than the expected minimal speed for landing,
+ * which is either MPC_LAND_SPEED or MPC_LAND_CRWL.
+ * This is enforced by an automatic check.
  *
  * @unit m/s
- * @decimal 1
+ * @min 0
+ * @decimal 2
  *
  * @group Land Detector
  */
-PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.50f);
+PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.25f);
 
 /**
  * Multicopter max horizontal velocity
diff --git a/src/modules/local_position_estimator/params.c b/src/modules/local_position_estimator/params.c
index daaed04a25..da199ca246 100644
--- a/src/modules/local_position_estimator/params.c
+++ b/src/modules/local_position_estimator/params.c
@@ -52,7 +52,6 @@ PARAM_DEFINE_FLOAT(LPE_FLW_RR, 7.0f);
  * @group Local Position Estimator
  * @min 0
  * @max 255
- * @decimal 0
  */
 PARAM_DEFINE_INT32(LPE_FLW_QMIN, 150);
 
diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp
index 1a0840006d..13bc588044 100644
--- a/src/modules/logger/logged_topics.cpp
+++ b/src/modules/logger/logged_topics.cpp
@@ -60,15 +60,20 @@ void LoggedTopics::add_default_topics()
 	add_topic("cellular_status", 200);
 	add_topic("commander_state");
 	add_topic("cpuload");
+	add_topic("custom_action_status");
 	add_optional_topic("esc_status", 250);
 	add_topic("failure_detector_status", 100);
+	add_topic("follow_target_status", 100);
 	add_optional_topic("follow_target", 500);
+	add_topic("follow_target_estimator", 100);
 	add_optional_topic("generator_status");
 	add_optional_topic("gps_dump");
+	add_optional_topic("gripper");
 	add_optional_topic("heater_status");
 	add_topic("home_position");
 	add_topic("hover_thrust_estimate", 100);
 	add_topic("input_rc", 500);
+	add_topic("log_message_incoming");
 	add_optional_topic("internal_combustion_engine_status", 10);
 	add_optional_topic("irlock_report", 1000);
 	add_optional_topic("landing_target_pose", 1000);
@@ -79,12 +84,15 @@ void LoggedTopics::add_default_topics()
 	add_topic("navigator_mission_item");
 	add_topic("npfg_status", 100);
 	add_topic("offboard_control_mode", 100);
-	add_topic("onboard_computer_status", 10);
 	add_topic("parameter_update");
+	add_topic("onboard_computer_status", 10);
+	add_topic("autotune_attitude_control_status", 100);
 	add_topic("position_controller_status", 500);
+	add_topic("position_controller_landing_status", 100);
 	add_topic("position_setpoint_triplet", 200);
 	add_optional_topic("px4io_status");
 	add_topic("radio_status");
+	add_topic("radio_status_extensions");
 	add_topic("rtl_time_estimate", 1000);
 	add_topic("sensor_combined");
 	add_optional_topic("sensor_correction");
@@ -102,6 +110,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_attitude", 50);
 	add_topic("vehicle_attitude_setpoint", 50);
 	add_topic("vehicle_command");
+	add_topic("vehicle_command_ack");
 	add_topic("vehicle_constraints", 1000);
 	add_topic("vehicle_control_mode");
 	add_topic("vehicle_global_position", 200);
@@ -176,12 +185,12 @@ void LoggedTopics::add_default_topics()
 	add_optional_topic_multi("sensor_baro", 1000, 4);
 	add_topic_multi("sensor_gps", 1000, 2);
 	add_topic_multi("sensor_gnss_relative", 1000, 1);
-	add_optional_topic("pps_capture", 1000);
 	add_optional_topic_multi("sensor_gyro", 1000, 4);
 	add_optional_topic_multi("sensor_mag", 1000, 4);
 	add_topic_multi("vehicle_imu", 500, 4);
 	add_topic_multi("vehicle_imu_status", 1000, 4);
 	add_optional_topic_multi("vehicle_magnetometer", 500, 4);
+	add_optional_topic("pps_capture");
 
 	// SYS_CTRL_ALLOC: additional dynamic control allocation logging when enabled
 	int32_t sys_ctrl_alloc = 0;
@@ -239,6 +248,20 @@ void LoggedTopics::add_default_topics()
 	add_topic("wind");
 	add_topic("yaw_estimator_status");
 #endif /* CONFIG_ARCH_BOARD_PX4_SITL */
+
+	int32_t mount_input_mode = -1;
+	param_get(param_find("MNT_MODE_IN"), &mount_input_mode);
+
+	if (mount_input_mode != -1) {
+		// Gimbal general topics
+		add_topic("gimbal_device_set_attitude", 100);
+		add_topic("gimbal_manager_set_attitude");  // ~ 5Hz
+		add_topic("gimbal_manager_set_manual_control"); // ~ 5Hz
+		add_topic("gimbal_device_attitude_status", 100);
+		add_topic("gimbal_device_information", 1000);
+		add_topic("gimbal_manager_information");
+		add_topic("gimbal_manager_status"); // ~ 5Hz
+	}
 }
 
 void LoggedTopics::add_high_rate_topics()
@@ -315,6 +338,8 @@ void LoggedTopics::add_vision_and_avoidance_topics()
 	add_topic("collision_constraints");
 	add_topic("obstacle_distance_fused");
 	add_topic("vehicle_mocap_odometry", 30);
+	add_topic("vehicle_odometry", 30);
+	add_topic("vehicle_trajectory_bezier", 200);
 	add_topic("vehicle_trajectory_waypoint", 200);
 	add_topic("vehicle_trajectory_waypoint_desired", 200);
 	add_topic("vehicle_visual_odometry", 30);
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 054c7e4522..a3d5212560 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -601,7 +601,7 @@ void Logger::run()
 		max_msg_size = _event_subscription.get_topic()->o_size;
 	}
 
-	max_msg_size += sizeof(ulog_message_data_header_s);
+	max_msg_size += sizeof(ulog_message_data_s);
 
 	if (sizeof(ulog_message_logging_s) > (size_t)max_msg_size) {
 		max_msg_size = sizeof(ulog_message_logging_s);
@@ -745,9 +745,9 @@ void Logger::run()
 				 */
 				const bool try_to_subscribe = (sub_idx == next_subscribe_topic_index);
 
-				if (copy_if_updated(sub_idx, _msg_buffer + sizeof(ulog_message_data_header_s), try_to_subscribe)) {
+				if (copy_if_updated(sub_idx, _msg_buffer + sizeof(ulog_message_data_s), try_to_subscribe)) {
 					// each message consists of a header followed by an orb data object
-					const size_t msg_size = sizeof(ulog_message_data_header_s) + sub.get_topic()->o_size_no_padding;
+					const size_t msg_size = sizeof(ulog_message_data_s) + sub.get_topic()->o_size_no_padding;
 					const uint16_t write_msg_size = static_cast<uint16_t>(msg_size - ULOG_MSG_HEADER_LEN);
 					const uint16_t write_msg_id = sub.msg_id;
 
@@ -964,7 +964,7 @@ bool Logger::handle_event_updates(uint32_t &total_bytes)
 	bool data_written = false;
 
 	while (_event_subscription.updated()) {
-		event_s *orb_event = (event_s *)(_msg_buffer + sizeof(ulog_message_data_header_s));
+		event_s *orb_event = (event_s *)(_msg_buffer + sizeof(ulog_message_data_s));
 		_event_subscription.copy(orb_event);
 
 		// Important: we can only access single-byte values in orb_event (it's not necessarily aligned)
@@ -978,7 +978,7 @@ bool Logger::handle_event_updates(uint32_t &total_bytes)
 			updated_sequence -= _event_sequence_offset;
 			memcpy(&orb_event->event_sequence, &updated_sequence, sizeof(updated_sequence));
 
-			size_t msg_size = sizeof(ulog_message_data_header_s) + _event_subscription.get_topic()->o_size_no_padding;
+			size_t msg_size = sizeof(ulog_message_data_s) + _event_subscription.get_topic()->o_size_no_padding;
 			uint16_t write_msg_size = static_cast<uint16_t>(msg_size - ULOG_MSG_HEADER_LEN);
 			uint16_t write_msg_id = _event_subscription.msg_id;
 			//write one byte after another (because of alignment)
@@ -1160,23 +1160,23 @@ void Logger::handle_vehicle_command_update()
 		if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_START) {
 
 			if ((int)(command.param1 + 0.5f) != 0) {
-				ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
+				ack_vehicle_command(&command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED);
 
 			} else if (can_start_mavlink_log()) {
-				ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				ack_vehicle_command(&command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 				start_log_mavlink();
 
 			} else {
-				ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
+				ack_vehicle_command(&command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED);
 			}
 
 		} else if (command.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP) {
 			if (_writer.is_started(LogType::Full, LogWriter::BackendMavlink)) {
-				ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_IN_PROGRESS);
+				ack_vehicle_command(&command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS);
 				stop_log_mavlink();
 			}
 
-			ack_vehicle_command(&command, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+			ack_vehicle_command(&command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 		}
 	}
 }
@@ -1834,14 +1834,14 @@ void Logger::write_add_logged_msg(LogType type, LoggerSubscription &subscription
 void Logger::write_info(LogType type, const char *name, const char *value)
 {
 	_writer.lock();
-	ulog_message_info_header_s msg = {};
+	ulog_message_info_s msg = {};
 	uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
 	msg.msg_type = static_cast<uint8_t>(ULogMessageType::INFO);
 
 	/* construct format key (type and name) */
 	size_t vlen = strlen(value);
-	msg.key_len = snprintf(msg.key, sizeof(msg.key), "char[%zu] %s", vlen, name);
-	size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;
+	msg.key_len = snprintf(msg.key_value_str, sizeof(msg.key_value_str), "char[%zu] %s", vlen, name);
+	size_t msg_size = sizeof(msg) - sizeof(msg.key_value_str) + msg.key_len;
 
 	/* copy string value directly to buffer */
 	if (vlen < (sizeof(msg) - msg_size)) {
@@ -1859,15 +1859,15 @@ void Logger::write_info(LogType type, const char *name, const char *value)
 void Logger::write_info_multiple(LogType type, const char *name, const char *value, bool is_continued)
 {
 	_writer.lock();
-	ulog_message_info_multiple_header_s msg;
+	ulog_message_info_multiple_s msg;
 	uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
 	msg.msg_type = static_cast<uint8_t>(ULogMessageType::INFO_MULTIPLE);
 	msg.is_continued = is_continued;
 
 	/* construct format key (type and name) */
 	size_t vlen = strlen(value);
-	msg.key_len = snprintf(msg.key, sizeof(msg.key), "char[%zu] %s", vlen, name);
-	size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;
+	msg.key_len = snprintf(msg.key_value_str, sizeof(msg.key_value_str), "char[%zu] %s", vlen, name);
+	size_t msg_size = sizeof(msg) - sizeof(msg.key_value_str) + msg.key_len;
 
 	/* copy string value directly to buffer */
 	if (vlen < (sizeof(msg) - msg_size)) {
@@ -1879,7 +1879,7 @@ void Logger::write_info_multiple(LogType type, const char *name, const char *val
 		write_message(type, buffer, msg_size);
 
 	} else {
-		PX4_ERR("info_multiple str too long (%" PRIu8 "), key=%s", msg.key_len, msg.key);
+		PX4_ERR("info_multiple str too long (%" PRIu8 "), key=%s", msg.key_len, msg.key_value_str);
 	}
 
 	_writer.unlock();
@@ -1900,13 +1900,13 @@ template<typename T>
 void Logger::write_info_template(LogType type, const char *name, T value, const char *type_str)
 {
 	_writer.lock();
-	ulog_message_info_header_s msg = {};
+	ulog_message_info_s msg = {};
 	uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
 	msg.msg_type = static_cast<uint8_t>(ULogMessageType::INFO);
 
 	/* construct format key (type and name) */
-	msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, name);
-	size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;
+	msg.key_len = snprintf(msg.key_value_str, sizeof(msg.key_value_str), "%s %s", type_str, name);
+	size_t msg_size = sizeof(msg) - sizeof(msg.key_value_str) + msg.key_len;
 
 	/* copy string value directly to buffer */
 	memcpy(&buffer[msg_size], &value, sizeof(T));
@@ -2044,7 +2044,7 @@ void Logger::write_version(LogType type)
 void Logger::write_parameter_defaults(LogType type)
 {
 	_writer.lock();
-	ulog_message_parameter_default_header_s msg = {};
+	ulog_message_parameter_default_s msg = {};
 	uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
 
 	msg.msg_type = static_cast<uint8_t>(ULogMessageType::PARAMETER_DEFAULT);
@@ -2092,8 +2092,8 @@ void Logger::write_parameter_defaults(LogType type)
 			}
 
 			// format parameter key (type and name)
-			msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, param_name(param));
-			size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;
+			msg.key_len = snprintf(msg.key_value_str, sizeof(msg.key_value_str), "%s %s", type_str, param_name(param));
+			size_t msg_size = sizeof(msg) - sizeof(msg.key_value_str) + msg.key_len;
 
 			if (param_get_default_value(param, &default_value) != 0) {
 				continue;
@@ -2138,7 +2138,7 @@ void Logger::write_parameter_defaults(LogType type)
 void Logger::write_parameters(LogType type)
 {
 	_writer.lock();
-	ulog_message_parameter_header_s msg = {};
+	ulog_message_parameter_s msg = {};
 	uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
 
 	msg.msg_type = static_cast<uint8_t>(ULogMessageType::PARAMETER);
@@ -2175,8 +2175,8 @@ void Logger::write_parameters(LogType type)
 			}
 
 			// format parameter key (type and name)
-			msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, param_name(param));
-			size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;
+			msg.key_len = snprintf(msg.key_value_str, sizeof(msg.key_value_str), "%s %s", type_str, param_name(param));
+			size_t msg_size = sizeof(msg) - sizeof(msg.key_value_str) + msg.key_len;
 
 			// copy parameter value directly to buffer
 			switch (ptype) {
@@ -2207,7 +2207,7 @@ void Logger::write_parameters(LogType type)
 void Logger::write_changed_parameters(LogType type)
 {
 	_writer.lock();
-	ulog_message_parameter_header_s msg = {};
+	ulog_message_parameter_s msg = {};
 	uint8_t *buffer = reinterpret_cast<uint8_t *>(&msg);
 
 	msg.msg_type = static_cast<uint8_t>(ULogMessageType::PARAMETER);
@@ -2245,8 +2245,8 @@ void Logger::write_changed_parameters(LogType type)
 			}
 
 			// format parameter key (type and name)
-			msg.key_len = snprintf(msg.key, sizeof(msg.key), "%s %s", type_str, param_name(param));
-			size_t msg_size = sizeof(msg) - sizeof(msg.key) + msg.key_len;
+			msg.key_len = snprintf(msg.key_value_str, sizeof(msg.key_value_str), "%s %s", type_str, param_name(param));
+			size_t msg_size = sizeof(msg) - sizeof(msg.key_value_str) + msg.key_len;
 
 			// copy parameter value directly to buffer
 			switch (ptype) {
diff --git a/src/modules/logger/logger.h b/src/modules/logger/logger.h
index 2b42e02b2a..232e47b7e5 100644
--- a/src/modules/logger/logger.h
+++ b/src/modules/logger/logger.h
@@ -353,7 +353,7 @@ private:
 	float						_rate_factor{1.0f};
 	const orb_metadata				*_polling_topic_meta{nullptr}; ///< if non-null, poll on this topic instead of sleeping
 	orb_advert_t					_mavlink_log_pub{nullptr};
-	uint8_t						_next_topic_id{0}; ///< id of next subscribed ulog topic
+	uint8_t						_next_topic_id{0}; ///< Logger's internal id (first topic is 0, then 1, and so on) it will assign to the next subscribed ulog topic, used for ulog_message_add_logged_s
 	char						*_replay_file_name{nullptr};
 	bool						_should_stop_file_log{false}; /**< if true _next_load_print is set and file logging
 											will be stopped after load printing (for the full log) */
diff --git a/src/modules/logger/messages.h b/src/modules/logger/messages.h
index 59e56339e4..9b71d0fe67 100644
--- a/src/modules/logger/messages.h
+++ b/src/modules/logger/messages.h
@@ -88,103 +88,175 @@ struct ulog_key_header_s {
 	uint8_t	data[0];
 };
 
-#define ULOG_MSG_HEADER_LEN 3 //accounts for msg_size and msg_type
+
+/**
+ * @brief Message Header for the ULog
+ *
+ * This header components that is in the beginning of every ULog messages that gets written into
+ * Definitions section as well as the Data section of the ULog file.
+ */
 struct ulog_message_header_s {
-	uint16_t msg_size;
-	uint8_t msg_type;
+	uint16_t msg_size; ///< Size of the message excluding the header size
+	uint8_t msg_type; ///< Message type, which is one of the ASCII alphabet, defined in ULogMessageType
 };
 
+#define ULOG_MSG_HEADER_LEN 3 // Length of the header in bytes: accounts for msg_size (2 bytes) and msg_type (1 byte)
+
+/**
+ * @brief Format Message
+ *
+ * This message describes a single ULog topic's name and it's inner fields. The inner fields can have the type
+ * as defined in the uORB message file (e.g. msg/action_request.msg). Including other uORB topics, which is the
+ * nested type case.
+ *
+ * @param format Format of the uORB topic in the format: "message_name:field0;field1;"
+ * Example: "action_request:uint64_t timestamp;uint8_t action;uint8_t source;uint8_t mode;uint8_t[5] _padding0;"
+ */
 struct ulog_message_format_s {
-	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+	uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::FORMAT);
 
 	char format[1500];
 };
 
+/**
+ * @brief Subscription Message
+ *
+ * This message describes which uORB topic the logger has subscribed to.
+ *
+ * Used for indicating which msg_id corresponds to which uORB topic name (message_name)
+ */
 struct ulog_message_add_logged_s {
-	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+	uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::ADD_LOGGED_MSG);
 
-	uint8_t multi_id;
-	uint16_t msg_id;
-	char message_name[255];
+	uint8_t multi_id; ///< Multi instance id, if the topic is one of a multi instance uORB topic
+	uint16_t msg_id; ///< Message ID, an internally tracked id in the logger, which matches with the msg_id in ulog_message_data_s message
+	char message_name[255]; ///< Name of the uORB topic
 };
 
+/**
+ * @brief Unsubscription Message
+ *
+ * This message describes which uORB topic the logger has unsubscribed from.
+ */
 struct ulog_message_remove_logged_s {
-	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+	uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::REMOVE_LOGGED_MSG);
 
 	uint16_t msg_id;
 };
 
+/**
+ * @brief Sync Message
+ */
 struct ulog_message_sync_s {
-	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+	uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::SYNC);
 
 	uint8_t sync_magic[8];
 };
 
 struct ulog_message_dropout_s {
-	uint16_t msg_size = sizeof(uint16_t); //size of message - ULOG_MSG_HEADER_LEN
+	uint16_t msg_size = sizeof(uint16_t); ///< size of message - ULOG_MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::DROPOUT);
 
-	uint16_t duration; //in ms
+	uint16_t duration; ///< in ms
 };
 
-struct ulog_message_data_header_s {
-	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+/**
+ * @brief Logged Data Message
+ *
+ * Includes the binary data of the uORB topic with the corresponding logger's internal msg_id.
+ *
+ * The uint8_t data[] section follows after the msg_id part, but the message struct only defines up until the msg_id,
+ * since the data length varies per each uORB topic (the struct size of the uORB message).
+ */
+struct ulog_message_data_s {
+	uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::DATA);
 
 	uint16_t msg_id;
 };
 
-struct ulog_message_info_header_s {
-	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+/**
+ * @brief Information Message
+ *
+ * Writes the dictionary type key:value relationship of any kind of information.
+ *
+ * Example: key_value_str[] = "char[5] sys_toolchain_ver9.4.0"
+ */
+struct ulog_message_info_s {
+	uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::INFO);
 
-	uint8_t key_len;
-	char key[255];
+	uint8_t key_len; ///<  Length of the 'key'
+	char key_value_str[255]; ///<  String with the key and value information
 };
 
-struct ulog_message_info_multiple_header_s {
-	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+/**
+ * @brief Multiple Information Message
+ *
+ * Writes the dictionary type key:value relationship of any kind of information, but
+ * for the ones which has a long value that can't be contained in a single Information Message.
+ */
+struct ulog_message_info_multiple_s {
+	uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::INFO_MULTIPLE);
 
-	uint8_t is_continued; ///< can be used for arrays: set to 1, if this message is part of the previous with the same key
-	uint8_t key_len;
-	char key[255];
+	uint8_t is_continued; ///< Can be used for arrays: set to 1, if this message is part of the previous with the same key
+	uint8_t key_len;      ///< Length of the 'key'
+	char key_value_str[255]; ///< String with the key and value information
 };
 
+/**
+ * @brief Logged String Message
+ *
+ * Logged string, either from PX4_INFO() macro calls or MAVLink information messages, etc.
+ * Useful for Debugging.
+ */
 struct ulog_message_logging_s {
-	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+	uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::LOGGING);
 
-	uint8_t log_level; //same levels as in the linux kernel
+	uint8_t log_level; ///< same levels as in the linux kernel
 	uint64_t timestamp;
-	char message[128]; //defines the maximum length of a logged message string
+	char message[128]; ///< defines the maximum length of a logged message string
 };
 
 struct ulog_message_logging_tagged_s {
-	uint16_t msg_size; //size of message - ULOG_MSG_HEADER_LEN
+	uint16_t msg_size; ///< size of message - ULOG_MSG_HEADER_LEN
 	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::LOGGING_TAGGED);
 
-	uint8_t log_level; //same levels as in the linux kernel
+	uint8_t log_level; ///< same levels as in the linux kernel
 	uint16_t tag;
 	uint64_t timestamp;
-	char message[128]; //defines the maximum length of a logged message string
+	char message[128]; ///< defines the maximum length of a logged message string
 };
 
-struct ulog_message_parameter_header_s {
+/**
+ * @brief Parameter Message
+ *
+ * Includes a parameter value information in the format "<TYPE> <PARAMETER_NAME> <VALUE>"
+ */
+struct ulog_message_parameter_s {
 	uint16_t msg_size;
 	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::PARAMETER);
 
 	uint8_t key_len;
-	char key[255];
+	char key_value_str[255]; ///<  String with the key and value information
 };
 
+/**
+ * @brief ULog default parameter type flags
+ *
+ * These flags indicate whether the default parameter defined in the ulog_message_parameter_default_s message
+ * is System's default or the User setup's default value. As user can set a custom default parameter for each
+ * vehicle setup that overrides the system's default value.
+ */
 enum class ulog_parameter_default_type_t : uint8_t {
-	system = (1 << 0),
-	current_setup = (1 << 1) //airframe default
+	system = (1 << 0), ///< System wide default parameter
+	current_setup = (1 << 1) ///<  Custom setup default parameter
 };
 
 inline ulog_parameter_default_type_t operator|(ulog_parameter_default_type_t a, ulog_parameter_default_type_t b)
@@ -192,13 +264,20 @@ inline ulog_parameter_default_type_t operator|(ulog_parameter_default_type_t a,
 	return static_cast<ulog_parameter_default_type_t>(static_cast<uint8_t>(a) | static_cast<uint8_t>(b));
 }
 
-struct ulog_message_parameter_default_header_s {
+/**
+ * @brief Default Parameter Message
+ *
+ * This message defines a parameter name and it's default value. Which is useful for figuring out in ULog analyzer
+ * which Parameters are non-default (User modified), which can give a quick insight into how the vehicle is configured,
+ * since the default parameter values are well known & understood (e.g. PID Tuning values)
+ */
+struct ulog_message_parameter_default_s {
 	uint16_t msg_size;
 	uint8_t msg_type = static_cast<uint8_t>(ULogMessageType::PARAMETER_DEFAULT);
 
 	ulog_parameter_default_type_t default_types;
 	uint8_t key_len;
-	char key[255];
+	char key_value_str[255]; ///<  String with the key and value information
 };
 
 
diff --git a/src/modules/manual_control/ManualControl.cpp b/src/modules/manual_control/ManualControl.cpp
index 056f6cbe5e..1ac96085f3 100644
--- a/src/modules/manual_control/ManualControl.cpp
+++ b/src/modules/manual_control/ManualControl.cpp
@@ -87,6 +87,7 @@ void ManualControl::Run()
 
 		_stick_arm_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms);
 		_stick_disarm_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms);
+		_stick_kill_hysteresis.set_hysteresis_time_from(false, 10_s);
 		_button_hysteresis.set_hysteresis_time_from(false, _param_com_rc_arm_hyst.get() * 1_ms);
 
 		_selector.setRcInMode(_param_com_rc_in_mode.get());
@@ -157,7 +158,7 @@ void ManualControl::Run()
 	if (_selector.setpoint().valid) {
 		_published_invalid_once = false;
 
-		processStickArming(_selector.setpoint());
+		processStickGestures(_selector.setpoint());
 
 		// User override by stick
 		const float dt_s = (now - _last_time) / 1e6f;
@@ -322,6 +323,7 @@ void ManualControl::Run()
 		_r_diff.reset();
 		_stick_arm_hysteresis.set_state_and_update(false, now);
 		_stick_disarm_hysteresis.set_state_and_update(false, now);
+		_stick_kill_hysteresis.set_state_and_update(false, now);
 		_button_hysteresis.set_state_and_update(false, now);
 	}
 
@@ -333,7 +335,7 @@ void ManualControl::Run()
 	perf_end(_loop_perf);
 }
 
-void ManualControl::processStickArming(const manual_control_setpoint_s &input)
+void ManualControl::processStickGestures(const manual_control_setpoint_s &input)
 {
 	// Arm gesture
 	const bool right_stick_centered = (fabsf(input.x) < 0.1f) && (fabsf(input.y) < 0.1f);
@@ -355,6 +357,16 @@ void ManualControl::processStickArming(const manual_control_setpoint_s &input)
 	if (_param_man_arm_gesture.get() && !previous_stick_disarm_hysteresis && _stick_disarm_hysteresis.get_state()) {
 		sendActionRequest(action_request_s::ACTION_DISARM, action_request_s::SOURCE_RC_STICK_GESTURE);
 	}
+
+	// Kill gesture
+	const bool right_stick_lower_right = (input.x < -0.9f) && (input.y > 0.9f);
+
+	const bool previous_stick_kill_hysteresis = _stick_kill_hysteresis.get_state();
+	_stick_kill_hysteresis.set_state_and_update(left_stick_lower_left && right_stick_lower_right, input.timestamp);
+
+	if (!previous_stick_kill_hysteresis && _stick_kill_hysteresis.get_state()) {
+		sendActionRequest(action_request_s::ACTION_KILL, action_request_s::SOURCE_RC_STICK_GESTURE);
+	}
 }
 
 void ManualControl::evaluateModeSlot(uint8_t mode_slot)
diff --git a/src/modules/manual_control/ManualControl.hpp b/src/modules/manual_control/ManualControl.hpp
index 1ddf25c46e..77747538b8 100644
--- a/src/modules/manual_control/ManualControl.hpp
+++ b/src/modules/manual_control/ManualControl.hpp
@@ -78,7 +78,7 @@ private:
 	static constexpr int MAX_MANUAL_INPUT_COUNT = 3;
 
 	void Run() override;
-	void processStickArming(const manual_control_setpoint_s &input);
+	void processStickGestures(const manual_control_setpoint_s &input);
 
 	void evaluateModeSlot(uint8_t mode_slot);
 	void sendActionRequest(int8_t action, int8_t source, int8_t mode = 0);
@@ -110,6 +110,7 @@ private:
 
 	systemlib::Hysteresis _stick_arm_hysteresis{false};
 	systemlib::Hysteresis _stick_disarm_hysteresis{false};
+	systemlib::Hysteresis _stick_kill_hysteresis{false};
 	systemlib::Hysteresis _button_hysteresis{false};
 
 	ManualControlSelector _selector;
diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt
index e8e4c4bd39..8774cce504 100644
--- a/src/modules/mavlink/CMakeLists.txt
+++ b/src/modules/mavlink/CMakeLists.txt
@@ -31,7 +31,7 @@
 #
 ############################################################################
 
-set(MAVLINK_DIALECT "development") # standard, development, etc
+set(MAVLINK_DIALECT "auterion") # standard, development, etc
 
 # force mavlink dialect to standard if flash constrained
 if(px4_constrained_flash_build)
@@ -67,6 +67,31 @@ add_library(mavlink_c INTERFACE)
 target_sources(mavlink_c INTERFACE ${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT}/${MAVLINK_DIALECT}.h)
 set_source_files_properties(${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT}/${MAVLINK_DIALECT}.h PROPERTIES GENERATED true)
 target_include_directories(mavlink_c INTERFACE ${MAVLINK_LIBRARY_DIR} ${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT})
+
+set(MAVLINK_DIALECT_UAVIONIX "uAvionix")
+
+add_custom_command(
+	OUTPUT ${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT_UAVIONIX}/${MAVLINK_DIALECT_UAVIONIX}.h
+	COMMAND
+		${PYTHON_EXECUTABLE} ${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py
+			--lang C --wire-protocol 2.0
+			#--no-validate
+			#--strict-units
+			--output ${MAVLINK_LIBRARY_DIR}
+			${MAVLINK_GIT_DIR}/message_definitions/v1.0/${MAVLINK_DIALECT_UAVIONIX}.xml
+	DEPENDS
+		git_mavlink_v2
+		${MAVLINK_GIT_DIR}/pymavlink/tools/mavgen.py
+		${MAVLINK_GIT_DIR}/message_definitions/v1.0/${MAVLINK_DIALECT_UAVIONIX}.xml
+	COMMENT "Generating Mavlink ${MAVLINK_DIALECT_UAVIONIX}: ${MAVLINK_GIT_DIR_RELATIVE}/message_definitions/v1.0/${MAVLINK_DIALECT_UAVIONIX}.xml"
+	USES_TERMINAL
+)
+
+add_custom_target(mavlink_c_generate_uavionix DEPENDS ${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT_UAVIONIX}/${MAVLINK_DIALECT_UAVIONIX}.h)
+target_sources(mavlink_c INTERFACE ${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT_UAVIONIX}/${MAVLINK_DIALECT_UAVIONIX}.h)
+set_source_files_properties(${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT_UAVIONIX}/${MAVLINK_DIALECT_UAVIONIX}.h PROPERTIES GENERATED true)
+target_include_directories(mavlink_c INTERFACE ${MAVLINK_LIBRARY_DIR} ${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT_UAVIONIX})
+
 target_compile_options(mavlink_c INTERFACE -Wno-address-of-packed-member -Wno-cast-align)
 
 
@@ -94,10 +119,13 @@ px4_add_module(
 		mavlink_timesync.cpp
 		mavlink_ulog.cpp
 		MavlinkStatustextHandler.cpp
+		mavlink_terrain.cpp
 		tune_publisher.cpp
+		terrain.cpp
 	MODULE_CONFIG
 		module.yaml
 	DEPENDS
+		adsb
 		airspeed
 		component_general_json # for checksums.h
 		drivers_accelerometer
@@ -109,6 +137,7 @@ px4_add_module(
 		mavlink_c
 		tunes
 		version
+		terrain
 	UNITY_BUILD
 	)
 
@@ -116,9 +145,19 @@ if(PX4_TESTING)
 	add_subdirectory(mavlink_tests)
 endif()
 
+px4_add_functional_gtest(SRC mavlink_terrain_test.cpp
+	EXTRA_SRCS terrain.cpp
+	INCLUDES ${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT}
+	LINKLIBS terrain geo
+	COMPILE_FLAGS
+		-DMODULE_NAME=\"mavlink\"
+		-Wno-address-of-packed-member # TODO: fix in c_library_v2
+	)
+
 px4_add_unit_gtest(SRC MavlinkStatustextHandlerTest.cpp
 	INCLUDES
 		${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT}
+		${MAVLINK_LIBRARY_DIR}/${MAVLINK_DIALECT_UAVIONIX}
 	COMPILE_FLAGS
 		-Wno-address-of-packed-member # TODO: fix in c_library_v2
 		-Wno-cast-align # TODO: fix
diff --git a/src/modules/mavlink/mavlink b/src/modules/mavlink/mavlink
index 05864e218e..01de37f374 160000
--- a/src/modules/mavlink/mavlink
+++ b/src/modules/mavlink/mavlink
@@ -1 +1 @@
-Subproject commit 05864e218e204f1ebeee5555988150fcddbd873e
+Subproject commit 01de37f374752c2bae9399af211a6b44e991c4f0
diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index cc735329e6..ae32bb8ed0 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -93,6 +93,9 @@ extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
 extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
 
 #include <mavlink.h>
+#if !MAVLINK_FTP_UNIT_TEST && !functional_mavlink_terrain_test_EXPORTS
+#include <uAvionix.h>
+#endif
 
 __END_DECLS
 
diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 5e62c0ee9e..c1bd45ff8a 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -44,6 +44,7 @@
 
 #include "mavlink_ftp.h"
 #include "mavlink_tests/mavlink_ftp_test.h"
+#include <px4_platform_common/defines.h>
 
 #ifndef MAVLINK_FTP_UNIT_TEST
 #include "mavlink_main.h"
@@ -411,13 +412,7 @@ MavlinkFTP::_workList(PayloadHeader *payload)
 
 		// Determine the directory entry type
 		switch (result->d_type) {
-#ifdef __PX4_NUTTX
-
-		case DTYPE_FILE: {
-#else
-
-		case DT_REG: {
-#endif
+		case DIRENT_REGULAR_FILE: {
 				// For files we get the file size as well
 				direntType = kDirentFile;
 				int ret = snprintf(_work_buffer2, _work_buffer2_len, "%s/%s", _work_buffer1, result->d_name);
@@ -434,12 +429,7 @@ MavlinkFTP::_workList(PayloadHeader *payload)
 				break;
 			}
 
-#ifdef __PX4_NUTTX
-
-		case DTYPE_DIRECTORY:
-#else
-		case DT_DIR:
-#endif
+		case DIRENT_DIRECTORY:
 			if (strcmp(result->d_name, ".") == 0 || strcmp(result->d_name, "..") == 0) {
 				// Don't bother sending these back
 				direntType = kDirentSkip;
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 43c87f612e..dadddb6d29 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1022,6 +1022,9 @@ Mavlink::handle_message(const mavlink_message_t *msg)
 	// Special case for gimbals that need to forward GIMBAL_DEVICE_ATTITUDE_STATUS.
 	else if (msg->msgid == MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS) {
 		Mavlink::forward_message(msg, this);
+
+	} else if (msg->msgid == MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION) {
+		Mavlink::forward_message(msg, this);
 	}
 }
 
@@ -1061,9 +1064,11 @@ Mavlink::send_autopilot_capabilities()
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET;
+		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_FENCE;
 		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_MISSION_RALLY;
+		msg.capabilities |= MAV_PROTOCOL_CAPABILITY_TERRAIN;
 		msg.flight_sw_version = px4_firmware_version();
 		msg.middleware_sw_version = px4_firmware_version();
 		msg.os_sw_version = px4_os_version();
@@ -1494,6 +1499,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("COMMAND_CANCEL", unlimited_rate);
 		configure_stream_local("DISTANCE_SENSOR", 0.5f);
 		configure_stream_local("EFI_STATUS", 2.0f);
 		configure_stream_local("ESC_INFO", 1.0f);
@@ -1505,12 +1511,13 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
 		configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
 		configure_stream_local("GPS2_RAW", 1.0f);
-		configure_stream_local("GPS_GLOBAL_ORIGIN", 0.1f);
+		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 		configure_stream_local("GPS_RAW_INT", 1.0f);
 		configure_stream_local("GPS_STATUS", 1.0f);
 		configure_stream_local("HOME_POSITION", 0.5f);
 		configure_stream_local("HYGROMETER_SENSOR", 0.1f);
 		configure_stream_local("LOCAL_POSITION_NED", 1.0f);
+		configure_stream_local("MOUNT_ORIENTATION", 5.0f);
 		configure_stream_local("NAV_CONTROLLER_OUTPUT", 1.0f);
 		configure_stream_local("OBSTACLE_DISTANCE", 1.0f);
 		configure_stream_local("ORBIT_EXECUTION_STATUS", 2.0f);
@@ -1521,7 +1528,10 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("RC_CHANNELS", 5.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
 		configure_stream_local("SYS_STATUS", 1.0f);
+		configure_stream_local("TIME_ESTIMATE_TO_TARGET", 1.0f);
 		configure_stream_local("UTM_GLOBAL_POSITION", 0.5f);
+		configure_stream_local("OPEN_DRONE_ID_BASIC_ID", 1.f);
+		configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f);
 		configure_stream_local("VFR_HUD", 4.0f);
 		configure_stream_local("VIBRATION", 0.1f);
 		configure_stream_local("WIND_COV", 0.5f);
@@ -1532,6 +1542,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("DEBUG_VECT", 1.0f);
 		configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
 		configure_stream_local("LINK_NODE_STATUS", 1.0f);
+		configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 2.0f);
 #endif // !CONSTRAINED_FLASH
 
 		break;
@@ -1558,6 +1569,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("COMMAND_CANCEL", unlimited_rate);
 		configure_stream_local("EFI_STATUS", 2.0f);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 5.0f);
@@ -1587,6 +1599,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("VFR_HUD", 10.0f);
 		configure_stream_local("VIBRATION", 0.5f);
 		configure_stream_local("WIND_COV", 10.0f);
+		configure_stream_local("OPEN_DRONE_ID_BASIC_ID", 1.f);
+		configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f);
 
 #if !defined(CONSTRAINED_FLASH)
 		configure_stream_local("DEBUG", 10.0f);
@@ -1594,6 +1608,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("DEBUG_VECT", 10.0f);
 		configure_stream_local("NAMED_VALUE_FLOAT", 10.0f);
 		configure_stream_local("LINK_NODE_STATUS", 1.0f);
+		configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f);
 #endif // !CONSTRAINED_FLASH
 
 		break;
@@ -1625,6 +1640,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("COMMAND_CANCEL", unlimited_rate);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
 		configure_stream_local("GLOBAL_POSITION_INT", 5.0f);
@@ -1654,6 +1670,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("DEBUG_VECT", 1.0f);
 		configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
 		configure_stream_local("LINK_NODE_STATUS", 1.0f);
+		configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f);
 #endif // !CONSTRAINED_FLASH
 
 		break;
@@ -1703,6 +1720,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("COMMAND_CANCEL", unlimited_rate);
 		configure_stream_local("EFI_STATUS", 10.0f);
 		configure_stream_local("ESC_INFO", 10.0f);
 		configure_stream_local("ESC_STATUS", 10.0f);
@@ -1736,6 +1754,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("VFR_HUD", 20.0f);
 		configure_stream_local("VIBRATION", 2.5f);
 		configure_stream_local("WIND_COV", 10.0f);
+		configure_stream_local("OPEN_DRONE_ID_BASIC_ID", 1.f);
+		configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f);
 
 #if !defined(CONSTRAINED_FLASH)
 		configure_stream_local("DEBUG", 50.0f);
@@ -1743,6 +1763,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("DEBUG_VECT", 50.0f);
 		configure_stream_local("NAMED_VALUE_FLOAT", 50.0f);
 		configure_stream_local("LINK_NODE_STATUS", 1.0f);
+		configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f);
 #endif // !CONSTRAINED_FLASH
 
 		break;
@@ -1773,13 +1794,16 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		// Note: streams requiring low latency come first
 		configure_stream_local("TIMESYNC", 10.0f);
 		configure_stream_local("CAMERA_TRIGGER", unlimited_rate);
+		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
 		configure_stream_local("LOCAL_POSITION_NED", 30.0f);
 		configure_stream_local("ATTITUDE", 20.0f);
 		configure_stream_local("ALTITUDE", 10.0f);
 		configure_stream_local("DISTANCE_SENSOR", 10.0f);
 		configure_stream_local("MOUNT_ORIENTATION", 10.0f);
 		configure_stream_local("OBSTACLE_DISTANCE", 10.0f);
-		configure_stream_local("ODOMETRY", 30.0f);
+		// currently odometry is disabled as mavlink router forwards this to the groundstation which
+		// causes unnecessary waste of bandwidth
+		//configure_stream_local("ODOMETRY", 30.0f);
 		configure_stream_local("GIMBAL_DEVICE_ATTITUDE_STATUS", 1.0f);
 		configure_stream_local("GIMBAL_MANAGER_STATUS", 0.5f);
 		configure_stream_local("GIMBAL_DEVICE_SET_ATTITUDE", 5.0f);
@@ -1790,9 +1814,11 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ATTITUDE_TARGET", 2.0f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("COLLISION", unlimited_rate);
+		configure_stream_local("COMMAND_CANCEL", unlimited_rate);
 		configure_stream_local("ESTIMATOR_STATUS", 1.0f);
 		configure_stream_local("EXTENDED_SYS_STATE", 1.0f);
 		configure_stream_local("GLOBAL_POSITION_INT", 10.0f);
+		configure_stream_local("GPS_GLOBAL_ORIGIN", 1.0f);
 		configure_stream_local("GPS2_RAW", unlimited_rate);
 		configure_stream_local("GPS_RAW_INT", unlimited_rate);
 		configure_stream_local("HOME_POSITION", 0.5f);
@@ -1802,7 +1828,7 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("PING", 0.1f);
 		configure_stream_local("POSITION_TARGET_GLOBAL_INT", 1.5f);
 		configure_stream_local("POSITION_TARGET_LOCAL_NED", 1.5f);
-		configure_stream_local("RC_CHANNELS", 5.0f);
+		configure_stream_local("RC_CHANNELS", 20.0f);
 		configure_stream_local("SERVO_OUTPUT_RAW_0", 1.0f);
 		configure_stream_local("SYS_STATUS", 5.0f);
 		configure_stream_local("TRAJECTORY_REPRESENTATION_WAYPOINTS", 5.0f);
@@ -1810,15 +1836,24 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("VFR_HUD", 4.0f);
 		configure_stream_local("VIBRATION", 0.5f);
 		configure_stream_local("WIND_COV", 1.0f);
+		configure_stream_local("SYSTEM_TIME", 2.0f);
+		configure_stream_local("OPEN_DRONE_ID_BASIC_ID", 1.f);
+		configure_stream_local("OPEN_DRONE_ID_LOCATION", 1.f);
 
 #if !defined(CONSTRAINED_FLASH)
 		configure_stream_local("DEBUG", 1.0f);
 		configure_stream_local("DEBUG_FLOAT_ARRAY", 1.0f);
 		configure_stream_local("DEBUG_VECT", 1.0f);
 		configure_stream_local("NAMED_VALUE_FLOAT", 1.0f);
+		configure_stream_local("FIGURE_EIGHT_EXECUTION_STATUS", 5.0f);
 #endif // !CONSTRAINED_FLASH
 		break;
 
+	case MAVLINK_MODE_UAVIONIX:
+		configure_stream_local("UAVIONIX_ADSB_OUT_CFG", 0.1f);
+		configure_stream_local("UAVIONIX_ADSB_OUT_DYNAMIC", 5.0f);
+		break;
+
 	default:
 		ret = -1;
 		break;
@@ -2032,6 +2067,9 @@ Mavlink::task_main(int argc, char *argv[])
 					} else if (strcmp(myoptarg, "onboard_low_bandwidth") == 0) {
 						_mode = MAVLINK_MODE_ONBOARD_LOW_BANDWIDTH;
 
+					} else if (strcmp(myoptarg, "uavionix") == 0) {
+						_mode = MAVLINK_MODE_UAVIONIX;
+
 					} else {
 						PX4_ERR("invalid mode");
 						err_flag = true;
@@ -2339,7 +2377,7 @@ Mavlink::task_main(int argc, char *argv[])
 						// send positive command ack
 						vehicle_command_ack_s command_ack{};
 						command_ack.command = vehicle_cmd.command;
-						command_ack.result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 						command_ack.from_external = !vehicle_cmd.from_external;
 						command_ack.target_system = vehicle_cmd.source_system;
 						command_ack.target_component = vehicle_cmd.source_component;
@@ -2386,7 +2424,7 @@ Mavlink::task_main(int argc, char *argv[])
 							cmd_logging_start_acknowledgement = true;
 
 						} else if (command_ack.command == vehicle_command_s::VEHICLE_CMD_LOGGING_STOP
-							   && command_ack.result == vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) {
+							   && command_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
 							cmd_logging_stop_acknowledgement = true;
 						}
 					}
@@ -2427,6 +2465,8 @@ Mavlink::task_main(int argc, char *argv[])
 				msg.timeout = 0;
 				msg.device = SERIAL_CONTROL_DEV_SHELL;
 				msg.count = _mavlink_shell->read(msg.data, sizeof(msg.data));
+				msg.target_system = _mavlink_shell->targetSysid();
+				msg.target_component = _mavlink_shell->targetCompid();
 				mavlink_msg_serial_control_send_struct(get_channel(), &msg);
 			}
 		}
@@ -3288,7 +3328,7 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 	PRINT_MODULE_USAGE_PARAM_INT('o', 14550, 0, 65536, "Select UDP Network Port (remote)", true);
 	PRINT_MODULE_USAGE_PARAM_STRING('t', "127.0.0.1", nullptr, "Partner IP (broadcasting can be enabled via -p flag)", true);
 #endif
-	PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvision|extvisionmin|gimbal",
+	PRINT_MODULE_USAGE_PARAM_STRING('m', "normal", "custom|camera|onboard|osd|magic|config|iridium|minimal|extvision|extvisionmin|gimbal|uavionix",
 					"Mode: sets default streams and rates", true);
 	PRINT_MODULE_USAGE_PARAM_STRING('n', nullptr, "<interface_name>", "wifi/ethernet interface name", true);
 #if defined(CONFIG_NET_IGMP) && defined(CONFIG_NET_ROUTE)
@@ -3311,6 +3351,7 @@ $ mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 
 	PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print status for all instances");
 	PRINT_MODULE_USAGE_ARG("streams", "Print all enabled streams", true);
+	PRINT_MODULE_USAGE_COMMAND_DESCR("terrain_status", "Print terrain storage info");
 
 	PRINT_MODULE_USAGE_COMMAND_DESCR("stream", "Configure the sending rate of a stream for a running instance");
 #if defined(CONFIG_NET) || defined(__PX4_POSIX)
@@ -3345,6 +3386,10 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "stop")) {
 		return Mavlink::stop_command(argc, argv);
 
+	} else if (!strcmp(argv[1], "terrain_status")) {
+		terrain::MavlinkTerrainUploader::printStatus();
+		return 0;
+
 	} else if (!strcmp(argv[1], "stream")) {
 		return Mavlink::stream_command(argc, argv);
 
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 23c2fd5e4b..63d6e70bcb 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -179,6 +179,8 @@ public:
 	void			check_events_enable() { _should_check_events.store(true); }
 	void			check_events_disable() { _should_check_events.store(false); }
 
+	bool			is_primary_instance() const { return check_events(); /* TODO: rename when this goes upstream */ }
+
 	int			get_uart_fd() const { return _uart_fd; }
 
 	/**
@@ -210,6 +212,7 @@ public:
 		MAVLINK_MODE_EXTVISIONMIN,
 		MAVLINK_MODE_GIMBAL,
 		MAVLINK_MODE_ONBOARD_LOW_BANDWIDTH,
+		MAVLINK_MODE_UAVIONIX,
 		MAVLINK_MODE_COUNT
 	};
 
@@ -264,6 +267,9 @@ public:
 		case MAVLINK_MODE_ONBOARD_LOW_BANDWIDTH:
 			return "OnboardLowBandwidth";
 
+		case MAVLINK_MODE_UAVIONIX:
+			return "uAvionix";
+
 		default:
 			return "Unknown";
 		}
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index b974495099..1f63e83444 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -54,6 +54,8 @@
 
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionMultiArray.hpp>
+
+#include <uORB/topics/vehicle_command_cancel.h>
 #include <uORB/topics/vehicle_status.h>
 
 #include "streams/ACTUATOR_CONTROL_TARGET.hpp"
@@ -67,6 +69,7 @@
 #include "streams/CAMERA_IMAGE_CAPTURED.hpp"
 #include "streams/CAMERA_TRIGGER.hpp"
 #include "streams/COLLISION.hpp"
+#include "streams/COMMAND_CANCEL.hpp"
 #include "streams/COMMAND_LONG.hpp"
 #include "streams/COMPONENT_INFORMATION.hpp"
 #include "streams/COMPONENT_METADATA.hpp"
@@ -112,11 +115,14 @@
 #include "streams/STORAGE_INFORMATION.hpp"
 #include "streams/SYS_STATUS.hpp"
 #include "streams/SYSTEM_TIME.hpp"
+#include "streams/TIME_ESTIMATE_TO_TARGET.hpp"
 #include "streams/TIMESYNC.hpp"
 #include "streams/TRAJECTORY_REPRESENTATION_WAYPOINTS.hpp"
 #include "streams/VFR_HUD.hpp"
 #include "streams/VIBRATION.hpp"
 #include "streams/WIND_COV.hpp"
+#include "streams/OPEN_DRONE_ID_BASIC_ID.hpp"
+#include "streams/OPEN_DRONE_ID_LOCATION.hpp"
 
 #if !defined(CONSTRAINED_FLASH)
 # include "streams/ADSB_VEHICLE.hpp"
@@ -129,6 +135,7 @@
 # include "streams/GIMBAL_DEVICE_SET_ATTITUDE.hpp"
 # include "streams/GIMBAL_MANAGER_INFORMATION.hpp"
 # include "streams/GIMBAL_MANAGER_STATUS.hpp"
+# include "streams/GIMBAL_DEVICE_INFORMATION.hpp"
 # include "streams/GPS2_RAW.hpp"
 # include "streams/HIGH_LATENCY2.hpp"
 # include "streams/LINK_NODE_STATUS.hpp"
@@ -137,7 +144,10 @@
 # include "streams/SCALED_PRESSURE2.hpp"
 # include "streams/SCALED_PRESSURE3.hpp"
 # include "streams/SMART_BATTERY_INFO.hpp"
+# include "streams/UAVIONIX_ADSB_OUT_CFG.hpp"
+# include "streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp"
 # include "streams/UTM_GLOBAL_POSITION.hpp"
+#include "streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp"
 #endif // !CONSTRAINED_FLASH
 
 // ensure PX4 rotation enum and MAV_SENSOR_ROTATION align
@@ -224,7 +234,7 @@ static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_68_YAW_293 == static_cast<MAV_SE
 static_assert(MAV_SENSOR_ROTATION_PITCH_315 == static_cast<MAV_SENSOR_ORIENTATION>(ROTATION_PITCH_315), "Pitch: 315");
 static_assert(MAV_SENSOR_ROTATION_ROLL_90_PITCH_315 == static_cast<MAV_SENSOR_ORIENTATION>(ROTATION_ROLL_90_PITCH_315),
 	      "Roll: 90, Pitch: 315");
-static_assert(41 == ROTATION_MAX, "Keep MAV_SENSOR_ROTATION and PX4 Rotation in sync");
+static_assert(41 + 3 == ROTATION_MAX, "Keep MAV_SENSOR_ROTATION and PX4 Rotation in sync");
 
 
 union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
@@ -250,6 +260,7 @@ union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
 		break;
 
+	/* fallthrough */
 	case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
@@ -310,6 +321,10 @@ union px4_custom_mode get_px4_custom_mode(uint8_t nav_state)
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 		custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF;
 		break;
+
+	case vehicle_status_s::NAVIGATION_STATE_INIT:
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_INIT;
+		break;
 	}
 
 	return custom_mode;
@@ -322,6 +337,9 @@ static const StreamListItem streams_list[] = {
 #if defined(STATUSTEXT_HPP)
 	create_stream_list_item<MavlinkStreamStatustext>(),
 #endif // STATUSTEXT_HPP
+#if defined(COMMAND_CANCEL_HPP)
+	create_stream_list_item<MavlinkStreamCommandCancel>(),
+#endif // COMMAND_CANCEL_HPP
 #if defined(COMMAND_LONG_HPP)
 	create_stream_list_item<MavlinkStreamCommandLong>(),
 #endif // COMMAND_LONG_HPP
@@ -377,6 +395,9 @@ static const StreamListItem streams_list[] = {
 #if defined(SYSTEM_TIME_HPP)
 	create_stream_list_item<MavlinkStreamSystemTime>(),
 #endif // SYSTEM_TIME_HPP
+#if defined(TIME_ESTIMATE_TO_TARGET_HPP)
+	create_stream_list_item<MavlinkStreamTimeEstimateToTarget>(),
+#endif // TIME_ESTIMATE_TO_TARGET_HPP
 #if defined(TIMESYNC_HPP)
 	create_stream_list_item<MavlinkStreamTimesync>(),
 #endif // TIMESYNC_HPP
@@ -413,6 +434,9 @@ static const StreamListItem streams_list[] = {
 #if defined(GIMBAL_MANAGER_INFORMATION_HPP)
 	create_stream_list_item<MavlinkStreamGimbalManagerInformation>(),
 #endif // GIMBAL_MANAGER_INFORMATION_HPP
+#if defined(GIMBAL_DEVICE_INFORMATION_HPP)
+	create_stream_list_item<MavlinkStreamGimbalDeviceInformation>(),
+#endif // GIMBAL_DEVICE_INFORMATION_HPP
 #if defined(GIMBAL_MANAGER_STATUS_HPP)
 	create_stream_list_item<MavlinkStreamGimbalManagerStatus>(),
 #endif // GIMBAL_MANAGER_STATUS_HPP
@@ -514,6 +538,9 @@ static const StreamListItem streams_list[] = {
 #if defined(ORBIT_EXECUTION_STATUS_HPP)
 	create_stream_list_item<MavlinkStreamOrbitStatus>(),
 #endif // ORBIT_EXECUTION_STATUS_HPP
+#if defined(FIGURE_EIGHT_EXECUTION_STATUS_HPP)
+	create_stream_list_item<MavlinkStreamFigureEightStatus>(),
+#endif // FIGURE_EIGHT_EXECUTION_STATUS_HPP
 #if defined(OBSTACLE_DISTANCE_HPP)
 	create_stream_list_item<MavlinkStreamObstacleDistance>(),
 #endif // OBSTACLE_DISTANCE_HPP
@@ -554,10 +581,23 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamEfiStatus>(),
 #endif // EFI_STATUS_HPP
 #if defined(GPS_RTCM_DATA_HPP)
-	create_stream_list_item<MavlinkStreamGPSRTCMData>()
+	create_stream_list_item<MavlinkStreamGPSRTCMData>(),
 #endif // GPS_RTCM_DATA_HPP
+#if defined(UAVIONIX_ADSB_OUT_CFG_HPP)
+	create_stream_list_item<MavlinkStreamUavionixADSBOutCfg>(),
+#endif // UAVIONIX_ADSB_OUT_CFG_HPP
+#if defined(UAVIONIX_ADSB_OUT_DYNAMIC_HPP)
+	create_stream_list_item<MavlinkStreamUavionixADSBOutDynamic>(),
+#endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP
+#if defined(OPEN_DRONE_ID_BASIC_ID_HPP)
+	create_stream_list_item<MavlinkStreamOpenDroneIdBasicId>(),
+#endif // OPEN_DRONE_ID_BASIC_ID_HPP
+#if defined(OPEN_DRONE_ID_LOCATION)
+	create_stream_list_item<MavlinkStreamOpenDroneIdLocation>()
+#endif // OPEN_DRONE_ID_LOCATION
 };
 
+
 const char *get_stream_name(const uint16_t msg_id)
 {
 	// search for stream with specified msg id in supported streams list
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index 5f5a971102..64b7782efb 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -51,6 +51,7 @@
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 #include <navigator/navigation.h>
+#include <navigator/mission_block.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
@@ -103,6 +104,13 @@ MavlinkMissionManager::init_offboard_mission()
 			_count[MAV_MISSION_TYPE_MISSION] = mission_state.count;
 			_current_seq = mission_state.current_seq;
 
+			if (_current_seq >= _count[MAV_MISSION_TYPE_MISSION] &&
+			    !(_current_seq <= 0 && _count[MAV_MISSION_TYPE_MISSION] == 0)) {
+				PX4_ERR("mission corrupted, len %i, seq %i. Clearing..", (int)_count[MAV_MISSION_TYPE_MISSION], (int)_current_seq);
+				update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
+						      DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, true);
+			}
+
 		} else if (ret < 0) {
 			PX4_WARN("offboard mission init failed (%i)", ret);
 		}
@@ -148,7 +156,7 @@ MavlinkMissionManager::load_safepoint_stats()
  * Publish mission topic to notify navigator about changes.
  */
 int
-MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq)
+MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool clear_mission)
 {
 	// We want to make sure the whole struct is initialized including padding before getting written by dataman.
 	mission_s mission{};
@@ -156,6 +164,7 @@ MavlinkMissionManager::update_active_mission(dm_item_t dataman_id, uint16_t coun
 	mission.dataman_id = dataman_id;
 	mission.count = count;
 	mission.current_seq = seq;
+	mission.clear_mission = clear_mission;
 
 	/* update mission state in dataman */
 
@@ -331,15 +340,8 @@ MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t
 		break;
 
 	case MAV_MISSION_TYPE_RALLY: { // Read a safe point / rally point
-			mission_safe_point_s mission_safe_point;
-			bytes_read = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_safe_point, sizeof(mission_safe_point_s));
-			read_success = (bytes_read == sizeof(mission_safe_point_s));
-
-			mission_item.nav_cmd = MAV_CMD_NAV_RALLY_POINT;
-			mission_item.frame = mission_safe_point.frame;
-			mission_item.lat = mission_safe_point.lat;
-			mission_item.lon = mission_safe_point.lon;
-			mission_item.altitude = mission_safe_point.alt;
+			bytes_read = dm_read(DM_KEY_SAFE_POINTS, seq + 1, &mission_item, sizeof(mission_item_s));
+			read_success = (bytes_read == sizeof(mission_item_s));
 		}
 		break;
 
@@ -482,6 +484,12 @@ MavlinkMissionManager::send()
 
 	if (_mission_result_sub.update(&mission_result)) {
 
+		/* If an infeasible mission is uploaded and the old feasible mission exists item count could be wrong.
+		 * This is restoring item count from the feasible mission. */
+		if (_count[MAV_MISSION_TYPE_MISSION] != mission_result.seq_total) {
+			_count[MAV_MISSION_TYPE_MISSION] = mission_result.seq_total;
+		}
+
 		if (_current_seq != mission_result.seq_current) {
 
 			_current_seq = mission_result.seq_current;
@@ -547,9 +555,9 @@ MavlinkMissionManager::send()
 	} else if (_state != MAVLINK_WPM_STATE_IDLE && (_time_last_recv > 0)
 		   && hrt_elapsed_time(&_time_last_recv) > MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT) {
 
-		_mavlink->send_statustext_critical("Operation timeout\t");
+		_mavlink->send_statustext_info("Mission request timeout, transfer aborted\t");
 		events::send(events::ID("mavlink_mission_op_timeout"), events::Log::Error,
-			     "Operation timeout, aborting transfer");
+			     "Mission request timeout, aborting transfer");
 
 		PX4_DEBUG("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state);
 
@@ -952,8 +960,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 			_transfer_partner_sysid = msg->sysid;
 			_transfer_partner_compid = msg->compid;
 			_transfer_count = wpc.count;
-			_transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
-						DM_KEY_WAYPOINTS_OFFBOARD_0);	// use inactive storage for transmission
+			_transfer_dataman_id = next_transfer_dataman_id();
 			_transfer_current_seq = -1;
 
 			if (_mission_type == MAV_MISSION_TYPE_FENCE) {
@@ -1003,6 +1010,42 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg)
 	}
 }
 
+dm_item_t
+MavlinkMissionManager::next_transfer_dataman_id()
+{
+	dm_item_t transfer_dataman_id = _dataman_id;
+	/* lock MISSION_STATE item */
+	int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+
+	if (dm_lock_ret != 0) {
+		PX4_ERR("DM_KEY_MISSION_STATE lock failed");
+	}
+
+	mission_s mission_state;
+	int ret = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
+
+	/* unlock MISSION_STATE item */
+	if (dm_lock_ret == 0) {
+		dm_unlock(DM_KEY_MISSION_STATE);
+	}
+
+	if (ret == sizeof(mission_s)) {
+		_dataman_id = (dm_item_t)mission_state.dataman_id;
+		transfer_dataman_id = (_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
+				       DM_KEY_WAYPOINTS_OFFBOARD_0);	// use inactive storage for transmission
+
+	} else if (ret == 0) {
+		//dataman is empty (new or formatted SD card)
+		transfer_dataman_id = DM_KEY_WAYPOINTS_OFFBOARD_0;
+
+	} else {
+		PX4_ERR("Dataman can't read DM_KEY_MISSION_STATE. The actual size of readout is %d, expected size is %d.", ret,
+			static_cast<int>(sizeof(mission_s)));
+	}
+
+	return transfer_dataman_id;
+}
+
 void
 MavlinkMissionManager::switch_to_idle_state()
 {
@@ -1015,6 +1058,10 @@ MavlinkMissionManager::switch_to_idle_state()
 		PX4_DEBUG("unlocking geofence");
 	}
 
+	if (_state == MAVLINK_WPM_STATE_GETLIST) {
+		_had_upload_transfer = true;
+	}
+
 	_state = MAVLINK_WPM_STATE_IDLE;
 }
 
@@ -1065,7 +1112,15 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 			if (wp.seq != _transfer_seq) {
 				PX4_DEBUG("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq);
 
-				/* Item sequence not expected, ignore item */
+				// We should just ignore this as we will just re-request the item after a timeout.
+				//
+				// It seems like a good idea to immediately re-send a mission_request here, however,
+				// this can lead to a situation where we are "out of sync" and have a hard time
+				// recovering.
+				// What happens in that case is that we create more traffic by re-sending the request
+				// even though the correct mission item might already be "on the way" and arrive next.
+				// So by re-questing it yet again, we can cause this case to happen again in the next
+				// iteration, and even worse we add latency into the interation by having more traffic.
 				return;
 			}
 
@@ -1175,13 +1230,8 @@ MavlinkMissionManager::handle_mission_item_both(const mavlink_message_t *msg)
 			break;
 
 		case MAV_MISSION_TYPE_RALLY: { // Write a safe point / rally point
-				mission_safe_point_s mission_safe_point;
-				mission_safe_point.lat = mission_item.lat;
-				mission_safe_point.lon = mission_item.lon;
-				mission_safe_point.alt = mission_item.altitude;
-				mission_safe_point.frame = mission_item.frame;
-				write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, &mission_safe_point,
-							sizeof(mission_safe_point_s)) != sizeof(mission_safe_point_s);
+				write_failed = dm_write(DM_KEY_SAFE_POINTS, wp.seq + 1, &mission_item,
+							sizeof(mission_item)) != sizeof(mission_item);
 			}
 			break;
 
@@ -1281,7 +1331,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
 			switch (wpca.mission_type) {
 			case MAV_MISSION_TYPE_MISSION:
 				ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
-							    DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
+							    DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, true);
 				break;
 
 			case MAV_MISSION_TYPE_FENCE:
@@ -1294,7 +1344,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg)
 
 			case MAV_MISSION_TYPE_ALL:
 				ret = update_active_mission(_dataman_id == DM_KEY_WAYPOINTS_OFFBOARD_0 ? DM_KEY_WAYPOINTS_OFFBOARD_1 :
-							    DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0);
+							    DM_KEY_WAYPOINTS_OFFBOARD_0, 0, 0, true);
 				ret = update_geofence_count(0) || ret;
 				ret = update_safepoint_count(0) || ret;
 				break;
@@ -1331,6 +1381,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 	    mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT ||
 	    (_int_mode && (mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT ||
 			   mavlink_mission_item->frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT))) {
+		// This is a mission item with a global coordinate
 
 		// Switch to int mode if that is what we are receiving
 		if ((mavlink_mission_item->frame == MAV_FRAME_GLOBAL_INT ||
@@ -1363,6 +1414,8 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 			mission_item->altitude_is_relative = true;
 		}
 
+		// Depending on the received MAV_CMD_* (MAVLink Commands), assign the corresponding
+		// NAV_CMD value to the mission item's nav_cmd.
 		switch (mavlink_mission_item->command) {
 		case MAV_CMD_NAV_WAYPOINT:
 			mission_item->nav_cmd = NAV_CMD_WAYPOINT;
@@ -1438,11 +1491,11 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 			break;
 
 		case MAV_CMD_CONDITION_GATE:
-			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+			mission_item->nav_cmd = NAV_CMD_CONDITION_GATE;
 			break;
 
 		case MAV_CMD_NAV_FENCE_RETURN_POINT:
-			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+			mission_item->nav_cmd = NAV_CMD_FENCE_RETURN_POINT;
 			break;
 
 		case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION:
@@ -1459,11 +1512,11 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 
 		case MAV_CMD_NAV_RALLY_POINT:
 			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
+			mission_item->is_mission_rally_point = (mavlink_mission_item->param1 > 0.0f);
 			break;
 
 		default:
 			mission_item->nav_cmd = NAV_CMD_INVALID;
-
 			PX4_DEBUG("Unsupported command %d", mavlink_mission_item->command);
 
 			return MAV_MISSION_UNSUPPORTED;
@@ -1473,7 +1526,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 
 	} else if (mavlink_mission_item->frame == MAV_FRAME_MISSION) {
 
-		// this is a mission item with no coordinates
+		// This is a mission item with no coordinates
 
 		mission_item->params[0] = mavlink_mission_item->param1;
 		mission_item->params[1] = mavlink_mission_item->param2;
@@ -1517,6 +1570,12 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 			}
 			break;
 
+		case MAV_CMD_DO_WINCH:
+		case MAV_CMD_DO_GRIPPER:
+			mission_item->nav_cmd = mavlink_mission_item->command;
+			MavlinkMissionManager::copy_params_from_mavlink_to_mission_item(mission_item, mavlink_mission_item, 1, 2);
+			break;
+
 		case MAV_CMD_DO_CHANGE_SPEED:
 		case MAV_CMD_DO_SET_HOME:
 		case MAV_CMD_DO_SET_SERVO:
@@ -1545,6 +1604,7 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		case MAV_CMD_DO_SET_ROI_NONE:
 		case MAV_CMD_CONDITION_DELAY:
 		case MAV_CMD_CONDITION_DISTANCE:
+		case MAV_CMD_WAYPOINT_USER_1:
 			mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
 			break;
 
@@ -1637,6 +1697,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 		case NAV_CMD_SET_CAMERA_ZOOM:
 		case NAV_CMD_SET_CAMERA_FOCUS:
 		case NAV_CMD_DO_VTOL_TRANSITION:
+		case MAV_CMD_WAYPOINT_USER_1:
 			break;
 
 		default:
@@ -1737,6 +1798,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 			break;
 
 		case MAV_CMD_NAV_RALLY_POINT:
+			mavlink_mission_item->param1 = mission_item->is_mission_rally_point ? 1 : 0;
 			break;
 
 
@@ -1748,6 +1810,39 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 	return PX4_OK;
 }
 
+void MavlinkMissionManager::copy_params_from_mavlink_to_mission_item(struct mission_item_s *mission_item,
+		const mavlink_mission_item_t *mavlink_mission_item, int8_t start_idx, int8_t end_idx)
+{
+	// Copy each param1 ~ 7 if they are within the range specified
+	if (start_idx <= 1 && 1 <= end_idx) {
+		mission_item->params[0] = mavlink_mission_item->param1;
+	}
+
+	if (start_idx <= 2 && 2 <= end_idx) {
+		mission_item->params[1] = mavlink_mission_item->param2;
+	}
+
+	if (start_idx <= 3 && 3 <= end_idx) {
+		mission_item->params[2] = mavlink_mission_item->param3;
+	}
+
+	if (start_idx <= 4 && 4 <= end_idx) {
+		mission_item->params[3] = mavlink_mission_item->param4;
+	}
+
+	/* Param5, 6 and 7 are named x, y and z since it is used as position coordinates as well */
+	if (start_idx <= 5 && 5 <= end_idx) {
+		mission_item->params[4] = mavlink_mission_item->x;
+	}
+
+	if (start_idx <= 6 && 6 <= end_idx) {
+		mission_item->params[5] = mavlink_mission_item->y;
+	}
+
+	if (start_idx <= 7 && 7 <= end_idx) {
+		mission_item->params[6] = mavlink_mission_item->z;
+	}
+}
 
 void MavlinkMissionManager::check_active_mission()
 {
@@ -1764,3 +1859,86 @@ void MavlinkMissionManager::check_active_mission()
 				   MAV_MISSION_TYPE_MISSION);
 	}
 }
+
+
+bool MavlinkMissionManager::get_mission_area(double home_pos_lat, double home_pos_lon, bool home_pos_valid,
+		double &lat_sw, double &lon_sw, double &lat_ne, double &lon_ne)
+{
+	lat_sw = 1e9;
+	lon_sw = 1e9;
+	lat_ne = -1e9;
+	lon_ne = -1e9;
+	int num_points = 0;
+
+	auto extend_area = [&lat_sw, &lon_sw, &lat_ne, &lon_ne](double lat, double lon) {
+		// handle wrap-around: move new point as close as possible to existing area
+		if (lat < lat_sw - 180. / 2.) { lat += 180.; }
+
+		if (lon < lon_sw - 360. / 2.) { lon += 360.; }
+
+		if (lat > lat_ne + 180. / 2.) { lat -= 180.; }
+
+		if (lon > lon_ne + 360. / 2.) { lon -= 360.; }
+
+		if (lat < lat_sw) { lat_sw = lat; }
+
+		if (lat > lat_ne) { lat_ne = lat; }
+
+		if (lon < lon_sw) { lon_sw = lon; }
+
+		if (lon > lon_ne) { lon_ne = lon; }
+	};
+
+	// mission items
+	for (size_t i = 0; i < _count[MAV_MISSION_TYPE_MISSION]; i++) {
+
+		struct mission_item_s mission_item;
+
+		if (dm_read(_dataman_id, i, &mission_item, sizeof(mission_item_s)) != sizeof(mission_item_s)) {
+			PX4_ERR("dm_read failed");
+			return false;
+		}
+
+		if (!MissionBlock::item_contains_position(mission_item)) {
+			continue;
+		}
+
+		extend_area(mission_item.lat, mission_item.lon);
+		++num_points;
+	}
+
+	// safe points
+	for (int current_seq = 1; current_seq <= _count[MAV_MISSION_TYPE_RALLY]; ++current_seq) {
+		struct mission_item_s mission_item;
+
+		if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_item, sizeof(mission_item_s)) !=
+		    sizeof(mission_item_s)) {
+			PX4_ERR("dm_read failed");
+			return false;
+		}
+
+		extend_area(mission_item.lat, mission_item.lon);
+		++num_points;
+	}
+
+	if (home_pos_valid) {
+		extend_area(home_pos_lat, home_pos_lon);
+		++num_points;
+	}
+
+	// add a margin
+	const float margin_m = 200; // margin around the area in meters
+
+	MapProjection ref(lat_sw, lon_sw);
+	double lat, lon;
+	ref.reproject(margin_m, margin_m, lat, lon);
+	double margin_lat = matrix::wrap(lat, -90., 90.) - lat_sw;
+	double margin_lon = matrix::wrap(lon, -180., 180.) - lon_sw;
+
+	lat_sw -= margin_lat;
+	lat_ne += margin_lat;
+	lon_sw -= margin_lon;
+	lon_ne += margin_lon;
+
+	return num_points > 1;
+}
diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h
index 309f9d5812..b7222c342d 100644
--- a/src/modules/mavlink/mavlink_mission.h
+++ b/src/modules/mavlink/mavlink_mission.h
@@ -91,6 +91,21 @@ public:
 
 	void check_active_mission(void);
 
+	/**
+	 * Get the rectangular area around the current mission, safe points and home postion if valid (south-west and
+	 * north-east corner), with a margin.
+	 * @return true on success, false on failure, or not enough points
+	 */
+	bool get_mission_area(double home_pos_lat, double home_pos_lon, bool home_pos_valid,
+			      double &lat_sw, double &lon_sw, double &lat_ne, double &lon_ne);
+
+	bool had_upload_transfer()
+	{
+		bool ret = _had_upload_transfer;
+		_had_upload_transfer = false;
+		return ret;
+	}
+
 private:
 	enum MAVLINK_WPM_STATES _state {MAVLINK_WPM_STATE_IDLE};	///< Current state
 	enum MAV_MISSION_TYPE _mission_type {MAV_MISSION_TYPE_MISSION};	///< mission type of current transmission (only one at a time possible)
@@ -101,6 +116,7 @@ private:
 	uint8_t			_reached_sent_count{0};			///< last time when the vehicle reached a waypoint
 
 	bool			_int_mode{false};			///< Use accurate int32 instead of float
+	bool			_had_upload_transfer{false};		///< If there was an upload transfer completed
 
 	unsigned		_filesystem_errcount{0};		///< File system error count
 
@@ -143,7 +159,7 @@ private:
 	static constexpr uint16_t	MAX_COUNT[] = {
 		DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
 		DM_KEY_FENCE_POINTS_MAX - 1,
-		DM_KEY_SAFE_POINTS_MAX - 1
+		DM_KEY_SAFE_POINT_ITEMS_MAX - 1
 	};	/**< Maximum number of mission items for each type
 					(fence & safe points use the first item for the stats) */
 
@@ -159,7 +175,7 @@ private:
 
 	void init_offboard_mission();
 
-	int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq);
+	int update_active_mission(dm_item_t dataman_id, uint16_t count, int32_t seq, bool clear_mission = false);
 
 	/** store the geofence count to dataman */
 	int update_geofence_count(unsigned count);
@@ -216,6 +232,15 @@ private:
 
 	void handle_mission_count(const mavlink_message_t *msg);
 
+	/**
+	 * Helper function to determine next transfer dataman id.
+	 * It reads DM_KEY_MISSION_STATE which stores previous valid dataman_id.
+	 * Since mavlink_mission module and mission module communicate over dataman,
+	 * mission module is restoring dataman_id for invalid mission to previous valid mission
+	 * if valid mission exist.
+	 */
+	dm_item_t next_transfer_dataman_id();
+
 	void handle_mission_item(const mavlink_message_t *msg);
 	void handle_mission_item_int(const mavlink_message_t *msg);
 	void handle_mission_item_both(const mavlink_message_t *msg);
@@ -245,4 +270,19 @@ private:
 	 * set _state to idle (and do necessary cleanup)
 	 */
 	void switch_to_idle_state();
+
+	/**
+	 * Copies the specified range [1, 7] of param of MAVLink mission to params[] array of
+	 * the Mission item struct (Very useful for mission items for non-navigation
+	 * like MAV_CMD_DO*, as they use a parameter mapping 1 ~ 7, which directly
+	 * gets the value from MAVlink's Mission Item parameters.
+	 *
+	 * @param start_idx [1, 7] Start index of Param to copy from Mavlink Mission Item
+	 * @param end_idx [1, 7] End index of Param to copy from Mavlink Mission Item
+	 *
+	 * Note: The Index is in range [1, 7], so if you want to copy param2 ~ param5 into
+	 * params[1] and params[4], you need to call with 'start_idx = 2' and 'end_idx = 5'!
+	 */
+	void copy_params_from_mavlink_to_mission_item(struct mission_item_s *mission_item,
+			const mavlink_mission_item_t *mavlink_mission_item, int8_t start_idx = 1, int8_t end_idx = 7);
 };
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index 1ffafb796e..54f6ca985b 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -116,6 +116,18 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 					return;
 				}
 
+#ifdef PX4_RESTRICTED_BUILD
+
+				// only allow setting the following params
+				if (strcmp(name, "RTL_RETURN_ALT") != 0 &&
+				    strcmp(name, "GF_ACTION") != 0 &&
+				    strcmp(name, "GF_MAX_HOR_DIST") != 0 &&
+				    strcmp(name, "GF_MAX_VER_DIST") != 0) {
+					return;
+				}
+
+#endif /* PX4_RESTRICTED_BUILD */
+
 				/* attempt to find parameter, set and send it */
 				param_t param = param_find_no_notification(name);
 
@@ -228,6 +240,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 		}
 
 	case MAVLINK_MSG_ID_PARAM_MAP_RC: {
+#ifndef PX4_RESTRICTED_BUILD
 			/* map a rc channel to a parameter */
 			mavlink_param_map_rc_t map_rc;
 			mavlink_msg_param_map_rc_decode(msg, &map_rc);
@@ -267,6 +280,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 				_rc_param_map_pub.publish(_rc_param_map);
 			}
 
+#endif /* PX4_RESTRICTED_BUILD */
+
 			break;
 		}
 
diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c
index 59078c9525..0989c58a91 100644
--- a/src/modules/mavlink/mavlink_params.c
+++ b/src/modules/mavlink/mavlink_params.c
@@ -56,7 +56,7 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 1);
  * @value 1 Always use version 1
  * @value 2 Always use version 2
  */
-PARAM_DEFINE_INT32(MAV_PROTO_VER, 0);
+PARAM_DEFINE_INT32(MAV_PROTO_VER, 2);
 
 /**
  * MAVLink SiK Radio ID
@@ -165,4 +165,15 @@ PARAM_DEFINE_INT32(MAV_ODOM_LP, 0);
  * @min 1
  * @max 250
  */
-PARAM_DEFINE_INT32(MAV_RADIO_TOUT, 5);
+PARAM_DEFINE_INT32(MAV_RADIO_TOUT, 25);
+
+/**
+ * Enable MAVLink forwarding on TELEM2
+ *
+ * TELEM2 on Skynode only.
+ *
+ * @boolean
+ * @group MAVLink
+ * @reboot_required true
+ */
+PARAM_DEFINE_INT32(MAV_S_FORWARD, 0);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 5983c00170..f2d9082894 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -95,6 +95,11 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_handle_sens_flow_rot = param_find("SENS_FLOW_ROT");
 	_handle_ekf2_min_rng = param_find("EKF2_MIN_RNG");
 	_handle_ekf2_rng_a_hmax = param_find("EKF2_RNG_A_HMAX");
+	_handle_tf_terrain_en = param_find("TF_TERRAIN_EN");
+
+	if (_handle_tf_terrain_en != PARAM_INVALID) {
+		param_get(_handle_tf_terrain_en, &_param_tf_terrain_en);
+	}
 }
 
 void
@@ -176,6 +181,13 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_radio_status(msg);
 		break;
 
+#if !defined(CONSTRAINED_FLASH)
+
+	case MAVLINK_MSG_ID_RADIO_STATUS_EXTENSIONS:
+		handle_message_radio_status_extensions(msg);
+		break;
+#endif // !CONSTRAINED_FLASH
+
 	case MAVLINK_MSG_ID_MANUAL_CONTROL:
 		handle_message_manual_control(msg);
 		break;
@@ -307,6 +319,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_gimbal_device_attitude_status(msg);
 		break;
 
+	case MAVLINK_MSG_ID_TERRAIN_DATA:
+		_terrain_uploader.handleTerrainData(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -400,7 +416,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		// This looks suspicously like INT32_MAX was sent in a COMMAND_LONG instead of
 		// a COMMAND_INT.
 		PX4_ERR("param5/param6 invalid of command %" PRIu16, cmd_mavlink.command);
-		acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_DENIED);
+		acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED, 0);
 		return;
 	}
 
@@ -436,7 +452,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 	if (cmd_mavlink.x == 0x7ff80000 && cmd_mavlink.y == 0x7ff80000) {
 		// This looks like NAN was by accident sent as int.
 		PX4_ERR("x/y invalid of command %" PRIu16, cmd_mavlink.command);
-		acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_DENIED);
+		acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED, 0);
 		return;
 	}
 
@@ -474,14 +490,14 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 {
 	bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
 	bool send_ack = true;
-	uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+	uint8_t result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 	uint8_t progress = 0; // TODO: should be 255, 0 for backwards compatibility
 
 	if (!target_ok) {
 		// Reject alien commands only if there is no forwarding or we've never seen target component before
 		if (!_mavlink->get_forwarding_on()
 		    || !_mavlink->component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) {
-			acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_FAILED);
+			acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED, 0);
 		}
 
 		return;
@@ -506,7 +522,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 
 	} else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
 		if (set_message_interval((int)roundf(cmd_mavlink.param1), cmd_mavlink.param2, cmd_mavlink.param3)) {
-			result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
+			result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
 		}
 
 	} else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
@@ -547,7 +563,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 			send_ack = false;
 
 		} else {
-			result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+			result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 			send_ack = true;
 		}
 
@@ -633,7 +649,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 		if (has_module) {
 
 			// most are in progress
-			result = vehicle_command_ack_s::VEHICLE_RESULT_IN_PROGRESS;
+			result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS;
 
 			switch (status.state) {
 			case autotune_attitude_control_status_s::STATE_IDLE:
@@ -675,21 +691,38 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 			case autotune_attitude_control_status_s::STATE_COMPLETE:
 				progress = 100;
 				// ack it properly with an ACCEPTED once we're done
-				result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 				break;
 
 			case autotune_attitude_control_status_s::STATE_FAIL:
 				progress = 0;
-				result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
+				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
 				break;
 			}
 
 		} else {
-			result = vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
+			result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 		}
 
 		send_ack = true;
 
+	} else if (cmd_mavlink.command == MAV_CMD_WAYPOINT_USER_1) {
+		// Note for this case, the command is sent from an external entity
+		// to trigger a custom action on the Mission Computer, so a timeout
+		// check is not handled by the autopilot, but should rather be handled
+		// by the entity sending this command
+		vehicle_command_s custom_action_cmd = vehicle_command;
+		custom_action_cmd.target_system = 0;
+		custom_action_cmd.target_component = 193; // MAV_COMP_ID_ONBOARD_COMPUTER3 (arbitrarily selected)
+		custom_action_cmd.from_external = false;
+
+		PX4_DEBUG("receiving command %d from %d/%d", custom_action_cmd.command, custom_action_cmd.source_system,
+			  custom_action_cmd.source_component);
+
+		_cmd_pub.publish(custom_action_cmd);
+
+		send_ack = true;
+
 	} else {
 		send_ack = false;
 
@@ -704,7 +737,7 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 			// on a radio link
 			if (_mavlink->get_data_rate() < 5000) {
 				send_ack = true;
-				result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
+				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED;
 				_mavlink->send_statustext_critical("Not enough bandwidth to enable log streaming\t");
 				events::send<uint32_t>(events::ID("mavlink_log_not_enough_bw"), events::Log::Error,
 						       "Not enough bandwidth to enable log streaming ({1} \\< 5000)", _mavlink->get_data_rate());
@@ -759,7 +792,8 @@ uint8_t MavlinkReceiver::handle_request_message_command(uint16_t message_id, flo
 		}
 	}
 
-	return (message_sent ? vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED : vehicle_command_ack_s::VEHICLE_RESULT_DENIED);
+	return (message_sent ? vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED :
+		vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED);
 }
 
 
@@ -1639,6 +1673,27 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 	}
 }
 
+void
+MavlinkReceiver::handle_message_radio_status_extensions(mavlink_message_t *msg)
+{
+#if !defined(CONSTRAINED_FLASH)
+	mavlink_radio_status_extensions_t rstatus;
+	mavlink_msg_radio_status_extensions_decode(msg, &rstatus);
+
+	radio_status_extensions_s status{};
+
+	status.timestamp = hrt_absolute_time();
+	status.rssi = rstatus.rssi;
+	status.snr = rstatus.snr;
+	status.mcs_index = rstatus.mcs_index;
+	status.number_spatial_streams = rstatus.number_spatial_streams;
+	status.queue_size = rstatus.queue_size;
+	status.air_time = rstatus.air_time;
+
+	_radio_status_extensions_pub.publish(status);
+#endif // !CONSTRAINED_FLASH
+}
+
 void
 MavlinkReceiver::handle_message_ping(mavlink_message_t *msg)
 {
@@ -1778,6 +1833,7 @@ MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg)
 	if (shell) {
 		// we ignore the timeout, EXCLUSIVE & BLOCKING flags of the SERIAL_CONTROL message
 		if (serial_control_mavlink.count > 0) {
+			shell->setTargetID(msg->sysid, msg->compid);
 			shell->write(serial_control_mavlink.data, serial_control_mavlink.count);
 		}
 
@@ -2121,6 +2177,12 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 					(hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE);
 				break;
 
+			case MAV_TYPE_ODID:
+				_heartbeat_type_open_drone_id = now;
+				_mavlink->telemetry_status().open_drone_id_system_healthy =
+					(hb.system_status == MAV_STATE_STANDBY) || (hb.system_status == MAV_STATE_ACTIVE);
+				break;
+
 			default:
 				PX4_DEBUG("unhandled HEARTBEAT MAV_TYPE: %" PRIu8 " from SYSID: %" PRIu8 ", COMPID: %" PRIu8, hb.type, msg->sysid,
 					  msg->compid);
@@ -2923,6 +2985,7 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
 		tstatus.heartbeat_type_adsb                    = (t <= TIMEOUT + _heartbeat_type_adsb);
 		tstatus.heartbeat_type_camera                  = (t <= TIMEOUT + _heartbeat_type_camera);
 		tstatus.heartbeat_type_parachute               = (t <= TIMEOUT + _heartbeat_type_parachute);
+		tstatus.heartbeat_type_open_drone_id           = (t <= TIMEOUT + _heartbeat_type_open_drone_id);
 
 		tstatus.heartbeat_component_telemetry_radio    = (t <= TIMEOUT + _heartbeat_component_telemetry_radio);
 		tstatus.heartbeat_component_log                = (t <= TIMEOUT + _heartbeat_component_log);
@@ -3038,7 +3101,7 @@ MavlinkReceiver::handle_message_gimbal_device_attitude_status(mavlink_message_t
 	mavlink_msg_gimbal_device_attitude_status_decode(msg, &gimbal_device_attitude_status_msg);
 
 	gimbal_device_attitude_status_s gimbal_attitude_status{};
-	gimbal_attitude_status.timestamp = static_cast<uint64_t>(gimbal_device_attitude_status_msg.time_boot_ms) * 1000;
+	gimbal_attitude_status.timestamp = hrt_absolute_time();
 	gimbal_attitude_status.target_system = gimbal_device_attitude_status_msg.target_system;
 	gimbal_attitude_status.target_component = gimbal_device_attitude_status_msg.target_component;
 	gimbal_attitude_status.device_flags = gimbal_device_attitude_status_msg.flags;
@@ -3100,6 +3163,13 @@ MavlinkReceiver::run()
 
 #endif // MAVLINK_UDP
 
+	// let the main instance initialize the terrain uploader, if enabled
+	if (_mavlink->is_primary_instance() && _param_tf_terrain_en > 0) {
+		if (!_terrain_uploader.init(_param_tf_terrain_en)) {
+			PX4_ERR("Terrain uploader init failed");
+		}
+	}
+
 	ssize_t nread = 0;
 	hrt_abstime last_send_update = 0;
 
@@ -3261,6 +3331,9 @@ MavlinkReceiver::run()
 			}
 
 			_mavlink_log_handler.send();
+
+			update_terrain_uploader(t);
+
 			last_send_update = t;
 		}
 
@@ -3270,6 +3343,37 @@ MavlinkReceiver::run()
 	}
 }
 
+void MavlinkReceiver::update_terrain_uploader(const hrt_abstime &now)
+{
+	_terrain_uploader.update(_mavlink->get_channel(), *_mavlink->get_mavlink_log_pub());
+
+	bool home_position_changed = false;
+
+	// let only the main mavlink instance listen for home position changes
+	if (_mavlink->is_primary_instance() && _home_position_sub.updated()
+	    && now - _last_home_position_changed > 5_s) {
+		_last_home_position_changed = now;
+		home_position_changed = true;
+	}
+
+	if (_mission_manager.had_upload_transfer() || home_position_changed) {
+		actuator_armed_s armed{};
+		_actuator_armed_sub.copy(&armed);
+
+		if (!armed.armed && _terrain_uploader.valid()) {
+			home_position_s home_position{};
+			_home_position_sub.copy(&home_position);
+
+			double lat_sw, lon_sw, lat_ne, lon_ne;
+
+			if (_mission_manager.get_mission_area(home_position.lat, home_position.lon, home_position.valid_hpos,
+							      lat_sw, lon_sw, lat_ne, lon_ne)) {
+				_terrain_uploader.updateArea(lat_sw, lon_sw, lat_ne, lon_ne);
+			}
+		}
+	}
+}
+
 bool MavlinkReceiver::component_was_seen(int system_id, int component_id)
 {
 	// For system broadcast messages return true if at least one component was seen before
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 38cb1c5fa4..29f6b70ca1 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -33,7 +33,9 @@
 
 /**
  * @file mavlink_receiver.h
- * MAVLink receiver thread
+ *
+ * MAVLink receiver thread that converts the received MAVLink messages to the appropriate
+ * uORB topic publications, to decouple the uORB message and MAVLink message.
  *
  * @author Lorenz Meier <lorenz@px4.io>
  * @author Anton Babushkin <anton@px4.io>
@@ -46,6 +48,7 @@
 #include "mavlink_mission.h"
 #include "mavlink_parameters.h"
 #include "MavlinkStatustextHandler.hpp"
+#include "mavlink_terrain.h"
 #include "mavlink_timesync.h"
 #include "tune_publisher.h"
 
@@ -87,9 +90,11 @@
 #include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/onboard_computer_status.h>
 #include <uORB/topics/optical_flow.h>
+#include <uORB/topics/autotune_attitude_control_status.h>
 #include <uORB/topics/ping.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/radio_status.h>
+#include <uORB/topics/radio_status_extensions.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_gps.h>
@@ -104,7 +109,6 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
@@ -184,6 +188,7 @@ private:
 	void handle_message_play_tune(mavlink_message_t *msg);
 	void handle_message_play_tune_v2(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
+	void handle_message_radio_status_extensions(mavlink_message_t *msg);
 	void handle_message_rc_channels(mavlink_message_t *msg);
 	void handle_message_rc_channels_override(mavlink_message_t *msg);
 	void handle_message_serial_control(mavlink_message_t *msg);
@@ -232,6 +237,8 @@ private:
 
 	void schedule_tune(const char *tune);
 
+	void update_terrain_uploader(const hrt_abstime &now);
+
 	void update_message_statistics(const mavlink_message_t &message);
 	void update_rx_stats(const mavlink_message_t &message);
 
@@ -250,6 +257,7 @@ private:
 	MavlinkParametersManager	_parameters_manager;
 	MavlinkTimesync			_mavlink_timesync;
 	MavlinkStatustextHandler	_mavlink_statustext_handler;
+	terrain::MavlinkTerrainUploader _terrain_uploader;
 
 	mavlink_status_t		_status{}; ///< receiver status, used for mavlink_parse_char()
 
@@ -321,6 +329,7 @@ private:
 	uORB::Publication<vehicle_rates_setpoint_s>		_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
 	uORB::Publication<vehicle_trajectory_bezier_s>		_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)};
 	uORB::Publication<vehicle_trajectory_waypoint_s>	_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)};
+	uORB::Publication<radio_status_extensions_s>		_radio_status_extensions_pub{ORB_ID(radio_status_extensions)};
 
 #if !defined(CONSTRAINED_FLASH)
 	uORB::Publication<debug_array_s>			_debug_array_pub {ORB_ID(debug_array)};
@@ -369,6 +378,8 @@ private:
 	PX4Gyroscope *_px4_gyro{nullptr};
 	PX4Magnetometer *_px4_mag{nullptr};
 
+	hrt_abstime _last_home_position_changed{0};
+
 	float _global_local_alt0{NAN};
 	MapProjection _global_local_proj_ref{};
 
@@ -386,6 +397,7 @@ private:
 	hrt_abstime _heartbeat_type_adsb{0};
 	hrt_abstime _heartbeat_type_camera{0};
 	hrt_abstime _heartbeat_type_parachute{0};
+	hrt_abstime _heartbeat_type_open_drone_id{0};
 
 	hrt_abstime _heartbeat_component_telemetry_radio{0};
 	hrt_abstime _heartbeat_component_log{0};
@@ -402,6 +414,7 @@ private:
 	param_t _handle_sens_flow_rot{PARAM_INVALID};
 	param_t _handle_ekf2_min_rng{PARAM_INVALID};
 	param_t _handle_ekf2_rng_a_hmax{PARAM_INVALID};
+	param_t _handle_tf_terrain_en{PARAM_INVALID};
 
 	float _param_sens_flow_maxhgt{-1.0f};
 	float _param_sens_flow_maxr{-1.0f};
@@ -409,6 +422,7 @@ private:
 	int32_t _param_sens_flow_rot{0};
 	float _param_ekf2_min_rng{NAN};
 	float _param_ekf2_rng_a_hmax{NAN};
+	int32_t _param_tf_terrain_en{0};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::BAT_CRIT_THR>)     _param_bat_crit_thr,
diff --git a/src/modules/mavlink/mavlink_shell.h b/src/modules/mavlink/mavlink_shell.h
index 649b429b45..abb581f751 100644
--- a/src/modules/mavlink/mavlink_shell.h
+++ b/src/modules/mavlink/mavlink_shell.h
@@ -42,6 +42,7 @@
 #include <stddef.h>
 #include <stdint.h>
 #include <px4_platform_common/tasks.h>
+#include <px4_platform_common/atomic.h>
 
 #pragma once
 
@@ -77,8 +78,15 @@ public:
 	 */
 	size_t available();
 
+	void setTargetID(uint8_t sysid, uint8_t compid) { _target_sysid.store(sysid); _target_compid.store(compid); }
+
+	uint8_t targetSysid() const { return _target_sysid.load(); }
+	uint8_t targetCompid() const { return _target_compid.load(); }
 private:
 
+	px4::atomic<uint8_t> _target_sysid{};
+	px4::atomic<uint8_t> _target_compid{};
+
 	int _to_shell_fd = -1; /** fd to write to the shell */
 	int _from_shell_fd = -1; /** fd to read from the shell */
 	int _shell_fds[2] = { -1, -1}; /** stdin & out used by the shell */
diff --git a/src/modules/mavlink/mavlink_terrain.cpp b/src/modules/mavlink/mavlink_terrain.cpp
new file mode 100644
index 0000000000..5add35a91b
--- /dev/null
+++ b/src/modules/mavlink/mavlink_terrain.cpp
@@ -0,0 +1,148 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 "mavlink_terrain.h"
+
+#include <errno.h>
+
+using namespace terrain;
+
+TerrainUploader *MavlinkTerrainUploader::_instance = nullptr;
+pthread_mutex_t MavlinkTerrainUploader::_mtx = PTHREAD_MUTEX_INITIALIZER;
+
+bool MavlinkTerrainUploader::init(int grid_spacing_m)
+{
+	if (_instance) {
+		return true;
+	}
+
+	_instance = new TerrainUploader(grid_spacing_m);
+	return _instance != nullptr;
+}
+
+void MavlinkTerrainUploader::deinit()
+{
+	if (_instance) {
+		delete _instance;
+		_instance = nullptr;
+	}
+}
+
+int MavlinkTerrainUploader::updateArea(double lat_sw, double lon_sw, double lat_ne, double lon_ne)
+{
+	if (!_instance) {
+		return -EINVAL;
+	}
+
+	lock();
+	int ret = _instance->updateArea(hrt_absolute_time(), lat_sw, lon_sw, lat_ne, lon_ne);
+	unlock();
+	return ret;
+}
+
+void MavlinkTerrainUploader::update(const mavlink_channel_t &channel, orb_advert_t &mavlink_log_pub)
+{
+	if (!_instance) {
+		return;
+	}
+
+	hrt_abstime now = hrt_absolute_time();
+
+	lock();
+
+	if (now > _next_status_send) {
+		mavlink_terrain_report_t report{};
+		report.loaded = _instance->num4x4BlocksLoaded();
+		report.pending = _instance->num4x4BlocksPending();
+		report.spacing = _instance->gridSpacing();
+		mavlink_msg_terrain_report_send_struct(channel, &report);
+		_next_status_send = now + (report.pending > 0 ? 200_ms : 1_s);
+	}
+
+	_instance->update(now);
+
+	const mavlink_terrain_request_t *request;
+	int request_id = _instance->getLatestRequest(&request);
+
+	if (request && _current_request_id != request_id) {
+		_current_request_id = request_id;
+		mavlink_msg_terrain_request_send_struct(channel, request);
+		PX4_DEBUG("Sending request: lat=%.3f, lon=%.3f, grid_spacing=%i, ", request->lat * 1e-7, request->lon * 1e-7,
+			  request->grid_spacing);
+	}
+
+	TerrainUploader::Error latest_error = _instance->getLatestErrorAndClear();
+
+	unlock();
+
+	switch (latest_error) {
+	case TerrainUploader::Error::DirectoryCreationFailure:
+		mavlink_log_warning(&mavlink_log_pub, "terrain upload: directory creation failed");
+		break;
+
+	case TerrainUploader::Error::LatitudeExceeded:
+		mavlink_log_warning(&mavlink_log_pub, "terrain upload: latitude exceeded");
+		break;
+
+	case TerrainUploader::Error::StorageFailure:
+		mavlink_log_warning(&mavlink_log_pub, "terrain upload: storage failure (SD card full?)");
+		break;
+
+	case TerrainUploader::Error::None:
+		break;
+	}
+}
+
+void MavlinkTerrainUploader::handleTerrainData(mavlink_message_t *msg)
+{
+	if (!_instance) {
+		return;
+	}
+
+	mavlink_terrain_data_t terrain_data;
+	mavlink_msg_terrain_data_decode(msg, &terrain_data);
+	lock();
+	_instance->handleTerrainData(hrt_absolute_time(), terrain_data);
+	unlock();
+}
+
+void MavlinkTerrainUploader::printStatus()
+{
+	if (!_instance) {
+		return;
+	}
+
+	lock();
+	_instance->printStatus();
+	unlock();
+}
diff --git a/src/modules/mavlink/mavlink_terrain.h b/src/modules/mavlink/mavlink_terrain.h
new file mode 100644
index 0000000000..8e756f8296
--- /dev/null
+++ b/src/modules/mavlink/mavlink_terrain.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 <pthread.h>
+
+#include "mavlink_bridge_header.h"
+#include "terrain.h"
+
+#include <drivers/drv_hrt.h>
+#include <systemlib/mavlink_log.h>
+
+namespace terrain
+{
+
+/**
+ * @class MavlinkTerrainUploader
+ * Wrapper around a static TerrainUploader instance, so it can be used concurrently.
+ */
+class MavlinkTerrainUploader
+{
+public:
+	MavlinkTerrainUploader() = default;
+	~MavlinkTerrainUploader() = default;
+
+	bool init(int grid_spacing_m);
+	void deinit();
+
+	bool valid() const { return _instance != nullptr; }
+
+	int updateArea(double lat_sw, double lon_sw, double lat_ne, double lon_ne);
+
+	void update(const mavlink_channel_t &channel, orb_advert_t &mavlink_log_pub);
+	void handleTerrainData(mavlink_message_t *msg);
+
+	static void printStatus();
+
+private:
+	static void lock()
+	{
+		pthread_mutex_lock(&_mtx);
+	}
+
+	static void unlock()
+	{
+		pthread_mutex_unlock(&_mtx);
+	}
+
+	static TerrainUploader *_instance;
+	static pthread_mutex_t _mtx;
+
+	hrt_abstime _next_status_send{0};
+	int _current_request_id{0};
+};
+
+}
diff --git a/src/modules/mavlink/mavlink_terrain_test.cpp b/src/modules/mavlink/mavlink_terrain_test.cpp
new file mode 100644
index 0000000000..91d74e5340
--- /dev/null
+++ b/src/modules/mavlink/mavlink_terrain_test.cpp
@@ -0,0 +1,485 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 "mavlink_terrain.h"
+#include <lib/terrain/terrain_provider.h>
+
+#include <dirent.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <errno.h>
+
+#include <gtest/gtest.h>
+#include <array>
+
+using namespace terrain;
+// to run: make tests TESTFILTER=mavlink_terrain
+
+class TestData
+{
+public:
+	TestData(int grid_spacing, double lat_sw_conf, double lon_sw_conf, double lat_ne_conf, double lon_ne_conf);
+	~TestData();
+
+	int16_t lookup(double lat, double lon) const;
+
+	// configured area (w/o margin)
+	const double lat_sw;
+	const double lon_sw;
+	const double lat_ne;
+	const double lon_ne;
+private:
+	// add some margin to the test area, as the uploader might request some data outside the area
+	static constexpr double margin = util::delta_lat_lon_deg * 4.;
+
+	double _lat_sw_data;
+	double _lon_sw_data;
+	double _lat_ne_data;
+	double _lon_ne_data;
+	int16_t *_data{nullptr};
+	int _x_count;
+	int _y_count;
+};
+
+TestData::TestData(int grid_spacing, double lat_sw_conf, double lon_sw_conf, double lat_ne_conf, double lon_ne_conf)
+	: lat_sw(lat_sw_conf), lon_sw(lon_sw_conf), lat_ne(lat_ne_conf), lon_ne(lon_ne_conf)
+{
+	_lat_sw_data = lat_sw - margin;
+	_lon_sw_data = lon_sw - margin;
+	_lat_ne_data = lat_ne + margin;
+	_lon_ne_data = lon_ne + margin;
+
+	MapProjection ref(_lat_sw_data, _lon_sw_data);
+	double lat, lon;
+	// get approximate delta lat/lon for the grid spacing
+	ref.reproject(grid_spacing, grid_spacing, lat, lon);
+
+	double dlat = lat - _lat_sw_data;
+	double dlon = lon - _lon_sw_data;
+	_x_count = (_lat_ne_data - _lat_sw_data) / dlat * 4; // 4x oversampling
+	_y_count = (_lon_ne_data - _lon_sw_data) / dlon * 4;
+	printf("x count: %i, y count: %i\n", _x_count, _y_count);
+
+	_data = new int16_t[_x_count * _y_count];
+	// setup synthetic dataset, no value must be zero
+	int16_t lowest_value = 500;
+
+	for (int y = 0; y < _y_count; ++y) {
+		for (int x = 0; x < _x_count; ++x) {
+			_data[x + y * _x_count] = lowest_value + x + y;
+		}
+	}
+}
+
+TestData::~TestData()
+{
+	delete[] _data;
+}
+
+int16_t TestData::lookup(double lat, double lon) const
+{
+	while (lat < _lat_sw_data) { lat += 180.; }
+
+	while (lat > _lat_ne_data) { lat -= 180.; }
+
+	while (lon < _lon_sw_data) { lon += 360.; }
+
+	while (lon > _lon_ne_data) { lon -= 360.; }
+
+	int x = (lat - _lat_sw_data) / (_lat_ne_data - _lat_sw_data) * _x_count;
+	int y = (lon - _lon_sw_data) / (_lon_ne_data - _lon_sw_data) * _y_count;
+
+	if (x < 0 || x >= _x_count || y < 0 || y >= _y_count) {
+		printf("x=%i x count=%i y=%i y count=%i\n", x, _x_count, y, _y_count);
+		EXPECT_FALSE(x < 0 || x >= _x_count || y < 0
+			     || y >= _y_count) << "lookup out of range: lat: " << lat << ", lon: " << lon;
+		return 1;
+	}
+
+	return _data[x + y * _x_count];
+}
+
+
+class MavlinkTerrainTest : public ::testing::Test
+{
+public:
+	static void clearStorageDirectory();
+	int validateTerrainDataFiles();
+	void validateFile(const char *filename);
+
+	void SetUp() override
+	{
+		clearStorageDirectory();
+	}
+
+	void uploadData(TerrainUploader &uploader, TestData &dataset, hrt_abstime &time, int grid_spacing,
+			bool enable_timeouts);
+
+	void lookupWaitAndCheck(TerrainProvider &provider, const TestData &dataset, double lat, double lon);
+};
+
+void MavlinkTerrainTest::clearStorageDirectory()
+{
+	DIR *dp = opendir(TERRAIN_STORAGE_DIR);
+
+	if (dp == nullptr) {
+		return; // ignore if we cannot access the directory
+	}
+
+	struct dirent *result = nullptr;
+
+	while ((result = readdir(dp))) {
+
+		if (!strcmp(result->d_name, ".") || !strcmp(result->d_name, "..")) {
+			continue;
+		}
+
+		char filename[64];
+
+		if (snprintf(filename, sizeof(filename), "%s/%s", TERRAIN_STORAGE_DIR, result->d_name) >= (int)sizeof(filename)) {
+			FAIL() << "file name too long: " << result->d_name;
+		}
+
+		EXPECT_EQ(unlink(filename), 0);
+	}
+
+	closedir(dp);
+}
+
+void MavlinkTerrainTest::uploadData(TerrainUploader &uploader, TestData &dataset, hrt_abstime &time, int grid_spacing,
+				    bool enable_timeouts)
+{
+	uploader.updateArea(time, dataset.lat_sw, dataset.lon_sw, dataset.lat_ne, dataset.lon_ne);
+	uploader.update(time);
+
+	// should be downloading now
+	ASSERT_GT(uploader.num4x4BlocksPending(), 0);
+
+	int latest_request_id = 0;
+	int timeout_counter = 0;
+
+	for (int i = 0; i < 10000; ++i) { // limit the amount of loops (arbitrary)
+		if (uploader.num4x4BlocksPending() == 0) {
+			break;
+		}
+
+		time += 10_ms;
+		uploader.update(time);
+		const mavlink_terrain_request_t *request{nullptr};
+		int request_id = uploader.getLatestRequest(&request);
+
+		if (request && latest_request_id != request_id) {
+			latest_request_id = request_id;
+
+			mavlink_terrain_request_t latest_request;
+			memcpy(&latest_request, request, sizeof(latest_request));
+
+			const double request_lat = latest_request.lat * 1e-7;
+			const double request_lon = latest_request.lon * 1e-7;
+			ASSERT_GE(request_lat, -90.);
+			ASSERT_LE(request_lat, 90.);
+			ASSERT_GE(request_lon, -180.);
+			ASSERT_LE(request_lon, 180.);
+
+			// feed in requested data
+			MapProjection ref(request_lat, request_lon);
+			bool had_previous_request = false;
+			bool should_abort_request = false;
+
+			for (int x = 0; x < util::mavlink_num_blocks_x && !should_abort_request; ++x) {
+				for (int y = 0; y < util::mavlink_num_blocks_y && !should_abort_request; ++y) {
+					mavlink_terrain_data_t data{};
+					data.grid_spacing = grid_spacing;
+					data.gridbit = y + x * util::mavlink_num_blocks_y;
+
+					if ((latest_request.mask & ((uint64_t)1 << data.gridbit)) == 0) {
+						continue;
+					}
+
+					data.lat = latest_request.lat;
+					data.lon = latest_request.lon;
+					static constexpr int block_size = util::mavlink_terrain_data_size;
+					float x_offset = x * block_size * grid_spacing;
+					float y_offset = y * block_size * grid_spacing;
+
+					for (int x_block = 0; x_block < block_size; ++x_block) {
+						for (int y_block = 0; y_block < block_size; ++y_block) {
+							double lat_cur, lon_cur;
+							ref.reproject(x_offset + x_block * grid_spacing, y_offset + y_block * grid_spacing, lat_cur, lon_cur);
+							data.data[y_block + block_size * x_block] = dataset.lookup(lat_cur, lon_cur);
+						}
+					}
+
+					// if timeouts are enabled, pass only every 3rd request
+					bool should_timeout = enable_timeouts && (++timeout_counter % 3 != 2);
+
+					if (should_timeout) {
+						time += TerrainUploader::request_timeout + 10_ms;
+						should_abort_request = true;
+
+					} else {
+						time += 10_ms;
+						uploader.handleTerrainData(time, data);
+					}
+
+					uploader.update(time);
+
+
+					// except for the last iteration there should be no new request
+					EXPECT_FALSE(had_previous_request);
+
+					request_id = uploader.getLatestRequest(&request);
+
+					if (request_id != latest_request_id) {
+						had_previous_request = true;
+					}
+				}
+			}
+		}
+	}
+
+	ASSERT_EQ(uploader.num4x4BlocksPending(), 0);
+}
+
+void MavlinkTerrainTest::lookupWaitAndCheck(TerrainProvider &provider, const TestData &dataset, double lat, double lon)
+{
+	int16_t expected = dataset.lookup(lat, lon);
+
+	lat = matrix::wrap(lat, -90., 90.);
+	lon = matrix::wrap(lon, -180., 180.);
+
+	// we expect the lookup to succeed, so we use a relatively large timeout to avoid false CI failures
+	for (int loops = 0; loops < 2000; ++loops) {
+		float alt_msl_m;
+
+		if (provider.lookup(lat, lon, alt_msl_m)) {
+			ASSERT_NEAR((float)expected, alt_msl_m, 2.0f) << "lat: " << lat << " lon: " << lon;
+			return;
+		}
+
+		usleep(1000);
+	}
+
+	FAIL() << "terrain lookup failed for lat=" << lat << ", lon=" << lon;
+}
+
+int MavlinkTerrainTest::validateTerrainDataFiles()
+{
+	DIR *dp = opendir(TERRAIN_STORAGE_DIR);
+
+	EXPECT_TRUE(dp);
+
+	if (!dp) {
+		return 0;
+	}
+
+	struct dirent *result = nullptr;
+
+	int num_files = 0;
+
+	while ((result = readdir(dp))) {
+
+		char filename[64];
+
+		if (snprintf(filename, sizeof(filename), "%s/%s", TERRAIN_STORAGE_DIR, result->d_name) >= (int)sizeof(filename)) {
+			EXPECT_TRUE(false) << "file name too long: " << result->d_name;
+			return 0;
+		}
+
+		unsigned tag_lat, tag_lon;
+
+		if (util::getTagsFromFileName(result->d_name, tag_lat, tag_lon)) {
+
+			validateFile(filename);
+
+			++num_files;
+		}
+	}
+
+	closedir(dp);
+	return num_files;
+}
+
+void MavlinkTerrainTest::validateFile(const char *filename)
+{
+	int fd = open(filename, O_RDONLY, PX4_O_MODE_666);
+
+	if (fd == -1) {
+		FAIL() << "failed to open file: " << filename;
+	}
+
+	printf("checking file %s\n", filename);
+
+	FileHeader header;
+	uint16_t expected_magic = header.magic;
+	uint16_t expected_version = header.version;
+
+	ASSERT_EQ(read(fd, &header, sizeof(header)), sizeof(header));
+	ASSERT_EQ(header.magic, expected_magic);
+	ASSERT_EQ(header.version, expected_version);
+	ASSERT_GT(header.num_points_x, 0);
+	ASSERT_GT(header.num_points_y, 0);
+
+	ASSERT_LE(header.num_points_x * header.num_points_y, util::maxNumItemsPerFile(header.grid_spacing_m));
+
+	// read all data
+	int16_t *buffer = new int16_t[header.num_points_x * header.num_points_y];
+	int read_size = sizeof(int16_t) * header.num_points_x * header.num_points_y;
+	ASSERT_EQ(read(fd, buffer, read_size), read_size);
+
+	// check that all altitude values are non-zero
+	for (int i = 0; i < header.num_points_x * header.num_points_y; ++i) {
+		ASSERT_GT(buffer[i], 0) << "i: " << i;
+	}
+
+	delete[] buffer;
+
+	close(fd);
+}
+
+TEST_F(MavlinkTerrainTest, upload)
+{
+	int grid_spacing = 50;
+	TestData dataset{grid_spacing, 47.357022, 8.468667, 47.371207, 8.493230};
+	TerrainUploader uploader{grid_spacing};
+	hrt_abstime time = 432532423;
+
+	ASSERT_NO_FATAL_FAILURE(uploadData(uploader, dataset, time, grid_spacing, false));
+
+	// validate all files to contain only non-zero altitude data
+	ASSERT_GT(validateTerrainDataFiles(), 0);
+}
+
+TEST_F(MavlinkTerrainTest, reupload_with_timeouts)
+{
+	int grid_spacing = 50;
+	TestData dataset{grid_spacing, 47.357022, 8.468667, 47.371207, 8.493230};
+	TerrainUploader uploader{grid_spacing};
+	hrt_abstime time = 87654321;
+
+	ASSERT_NO_FATAL_FAILURE(uploadData(uploader, dataset, time, grid_spacing, true));
+
+	int num_terrain_files;
+	ASSERT_NO_FATAL_FAILURE(num_terrain_files = validateTerrainDataFiles(););
+	ASSERT_GT(num_terrain_files, 0);
+
+	const mavlink_terrain_request_t *request{nullptr};
+	int request_id = uploader.getLatestRequest(&request);
+
+	// request the same area again
+	uploader.updateArea(time, dataset.lat_sw, dataset.lon_sw, dataset.lat_ne, dataset.lon_ne);
+
+	time += 10_ms;
+
+	// should not be downloading as we specify the same area
+	ASSERT_EQ(uploader.num4x4BlocksPending(), 0);
+	uploader.update(time);
+	ASSERT_EQ(uploader.num4x4BlocksPending(), 0);
+	ASSERT_EQ(request_id, uploader.getLatestRequest(&request));
+
+
+	// request different area, far away: this should prune all data
+	uploader.updateArea(time, dataset.lat_sw, dataset.lon_sw + 1., dataset.lat_ne, dataset.lon_ne + 1.);
+	ASSERT_EQ(validateTerrainDataFiles(), 0);
+}
+
+TEST_F(MavlinkTerrainTest, upload_and_lookup)
+{
+	struct Area {
+		double lat_sw;
+		double lon_sw;
+		double lat_ne;
+		double lon_ne;
+	};
+	std::array<int, 4> grid_spacings{10, 50, 100, 201};
+	std::array<Area, 6> areas{
+		Area{47.357022, 8.468667, 47.371207, 8.493230},
+		Area{0, 0, 0.001, 0.001},
+		Area{-5, -8, -5 + 0.01, -8 + 0.03},
+		Area{-0.0143, -0.02423, 0.014, 0.01},
+		Area{-70.022, -190.0104, -69.992, -189.9633},
+		Area{-79.999, -190.0104, -79.932, -189.9633},
+	};
+
+	for (int grid_spacing : grid_spacings) {
+		for (unsigned area_index = 0; area_index < areas.size(); ++area_index) {
+			printf("Testing with: grid_spacing: %i, area: %i\n", grid_spacing, area_index);
+
+			clearStorageDirectory();
+
+			const Area &area = areas[area_index];
+			TestData dataset{grid_spacing, area.lat_sw, area.lon_sw, area.lat_ne, area.lon_ne};
+			TerrainUploader uploader{grid_spacing};
+			hrt_abstime time = 432532423;
+
+			ASSERT_NO_FATAL_FAILURE(uploadData(uploader, dataset, time, grid_spacing, false));
+
+			// validate all files to contain only non-zero altitude data
+			int num_terrain_files;
+			ASSERT_NO_FATAL_FAILURE(num_terrain_files = validateTerrainDataFiles(););
+			ASSERT_GT(num_terrain_files, 0);
+
+			TerrainProvider provider{grid_spacing};
+
+			// validate lookups in a grid over the configured area
+			const double delta = util::delta_lat_lon_deg / 100.;
+
+			for (double lat = dataset.lat_sw; lat <= dataset.lat_ne; lat += delta) {
+				for (double lon = dataset.lon_sw; lon <= dataset.lon_ne; lon += delta) {
+
+					ASSERT_NO_FATAL_FAILURE(
+						lookupWaitAndCheck(provider, dataset, lat, lon);
+					);
+				}
+			}
+
+			// test at the border of a file
+			double lat_border = floor((dataset.lat_sw + util::delta_lat_lon_deg) / util::delta_lat_lon_deg) *
+					    util::delta_lat_lon_deg;
+			double lon_border = floor((dataset.lon_sw + util::delta_lat_lon_deg) / util::delta_lat_lon_deg) *
+					    util::delta_lat_lon_deg;
+			const double increment = 0.00001;
+			const int count = 30;
+
+			for (double lat = lat_border - increment * count; lat <= lat_border + increment * count; lat += increment) {
+				for (double lon = lon_border - increment * count; lon <= lon_border + increment * count; lon += increment) {
+					if (lat > dataset.lat_sw && lat < dataset.lat_ne && lon > dataset.lon_sw && lon < dataset.lon_ne) {
+						ASSERT_NO_FATAL_FAILURE(
+							lookupWaitAndCheck(provider, dataset, lat, lon);
+						);
+					}
+				}
+			}
+		}
+	}
+}
diff --git a/src/modules/mavlink/mavlink_timesync.cpp b/src/modules/mavlink/mavlink_timesync.cpp
index ee0d2e3272..27666e25d7 100644
--- a/src/modules/mavlink/mavlink_timesync.cpp
+++ b/src/modules/mavlink/mavlink_timesync.cpp
@@ -54,6 +54,12 @@ MavlinkTimesync::handle_message(const mavlink_message_t *msg)
 	switch (msg->msgid) {
 	case MAVLINK_MSG_ID_TIMESYNC: {
 
+			// Herelink ground station keeps requesting to timesync by using MAV_COMP_ID_SYSTEM_CONTROL (Deprecated since 2018-11) as ID.
+			// Given that we can't disable timesyncronization on herelink we need to ignore such requests
+			if (msg->compid == MAV_COMP_ID_SYSTEM_CONTROL) {
+				return;
+			}
+
 			mavlink_timesync_t tsync = {};
 			mavlink_msg_timesync_decode(msg, &tsync);
 
diff --git a/src/modules/mavlink/module.yaml b/src/modules/mavlink/module.yaml
index 10da1b8883..f64f9ad729 100644
--- a/src/modules/mavlink/module.yaml
+++ b/src/modules/mavlink/module.yaml
@@ -68,6 +68,7 @@ parameters:
                 #9: External Vision Minimal # hidden
                 10: Gimbal
                 11: Onboard Low Bandwidth
+                12: uAvionix
             reboot_required: true
             num_instances: *max_num_config_instances
             default: [0, 2, 0]
@@ -118,7 +119,7 @@ parameters:
             type: boolean
             reboot_required: true
             num_instances: *max_num_config_instances
-            default: [true, true, true]
+            default: [false, false, false]
 
         MAV_${i}_UDP_PRT:
             description:
diff --git a/src/modules/mavlink/streams/COMMAND_CANCEL.hpp b/src/modules/mavlink/streams/COMMAND_CANCEL.hpp
new file mode 100644
index 0000000000..71c9c8ba0c
--- /dev/null
+++ b/src/modules/mavlink/streams/COMMAND_CANCEL.hpp
@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+#ifndef COMMAND_CANCEL_HPP
+#define COMMAND_CANCEL_HPP
+
+#include <uORB/topics/vehicle_command_cancel.h>
+
+class MavlinkStreamCommandCancel : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCommandCancel(mavlink); }
+
+	static constexpr const char *get_name_static() { return "COMMAND_CANCEL"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_COMMAND_CANCEL; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return _cmd_cancel_sub.advertised() ? MAVLINK_MSG_ID_COMMAND_CANCEL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+	}
+
+private:
+	explicit MavlinkStreamCommandCancel(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _cmd_cancel_sub{ORB_ID(vehicle_command_cancel)};
+
+	bool send() override
+	{
+		struct vehicle_command_cancel_s cmd_cancel;
+
+		if (_cmd_cancel_sub.update(&cmd_cancel)) {
+
+			mavlink_command_cancel_t msg{};
+
+			msg.command = cmd_cancel.command;
+			msg.target_system = cmd_cancel.target_system;
+			msg.target_component = cmd_cancel.target_component;
+
+			mavlink_msg_command_cancel_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // COMMAND_CANCEL_HPP
diff --git a/src/modules/mavlink/streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp b/src/modules/mavlink/streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp
new file mode 100644
index 0000000000..f0d33b2db9
--- /dev/null
+++ b/src/modules/mavlink/streams/FIGURE_EIGHT_EXECUTION_STATUS.hpp
@@ -0,0 +1,88 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#ifndef FIGURE_EIGHT_EXECUTION_STATUS_HPP
+#define FIGURE_EIGHT_EXECUTION_STATUS_HPP
+
+#include <uORB/topics/figure_eight_status.h>
+#include <mavlink.h>
+#include <mavlink/mavlink_stream.h>
+
+class MavlinkStreamFigureEightStatus : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamFigureEightStatus(mavlink); }
+
+	static constexpr const char *get_name_static() { return "FIGURE_EIGHT_EXECUTION_STATUS"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return _figure_eight_status_subs.advertised() ? MAVLINK_MSG_ID_FIGURE_EIGHT_EXECUTION_STATUS_LEN +
+		       MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
+	}
+
+private:
+	explicit MavlinkStreamFigureEightStatus(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _figure_eight_status_subs{ORB_ID::figure_eight_status};
+
+	bool send() override
+	{
+		figure_eight_status_s figure_eight_status;
+
+		if ((_mavlink->get_free_tx_buf() >= get_size()) && _figure_eight_status_subs.update(&figure_eight_status)) {
+			mavlink_figure_eight_execution_status_t msg_figure_eight_execution_status{};
+
+			msg_figure_eight_execution_status.time_usec = figure_eight_status.timestamp;
+			msg_figure_eight_execution_status.major_radius = figure_eight_status.major_radius;
+			msg_figure_eight_execution_status.minor_radius = figure_eight_status.minor_radius;
+			msg_figure_eight_execution_status.frame = figure_eight_status.frame;
+			msg_figure_eight_execution_status.orientation = figure_eight_status.orientation;
+			msg_figure_eight_execution_status.x = figure_eight_status.x;
+			msg_figure_eight_execution_status.y = figure_eight_status.y;
+			msg_figure_eight_execution_status.z = figure_eight_status.z;
+
+			mavlink_msg_figure_eight_execution_status_send_struct(_mavlink->get_channel(), &msg_figure_eight_execution_status);
+
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // FIGURE_EIGHT_EXECUTION_STATUS_HPP
diff --git a/src/modules/mavlink/streams/GIMBAL_DEVICE_INFORMATION.hpp b/src/modules/mavlink/streams/GIMBAL_DEVICE_INFORMATION.hpp
new file mode 100644
index 0000000000..d0aa42f9f0
--- /dev/null
+++ b/src/modules/mavlink/streams/GIMBAL_DEVICE_INFORMATION.hpp
@@ -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.
+ *
+ ****************************************************************************/
+
+#ifndef GIMBAL_DEVICE_INFORMATION_HPP
+#define GIMBAL_DEVICE_INFORMATION_HPP
+
+#include <uORB/topics/gimbal_device_information.h>
+
+class MavlinkStreamGimbalDeviceInformation : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGimbalDeviceInformation(mavlink); }
+
+	static constexpr const char *get_name_static() { return "GIMBAL_DEVICE_INFORMATION"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		if (_gimbal_device_information_sub.advertised()) {
+			return MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		}
+
+		return 0;
+	}
+
+private:
+	explicit MavlinkStreamGimbalDeviceInformation(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _gimbal_device_information_sub{ORB_ID(gimbal_device_information)};
+
+	bool send() override
+	{
+		gimbal_device_information_s gimbal_device_information;
+
+		if (_gimbal_device_information_sub.advertised() && _gimbal_device_information_sub.copy(&gimbal_device_information)) {
+			// send out gimbal_device_info with info from gimbal_device_information
+			mavlink_gimbal_device_information_t msg{};
+
+			msg.time_boot_ms = gimbal_device_information.timestamp / 1000;
+			memcpy(msg.vendor_name, gimbal_device_information.vendor_name, sizeof(gimbal_device_information.vendor_name));
+			memcpy(msg.model_name, gimbal_device_information.model_name, sizeof(gimbal_device_information.model_name));
+			memcpy(msg.custom_name, gimbal_device_information.custom_name, sizeof(gimbal_device_information.custom_name));
+			msg.firmware_version = gimbal_device_information.firmware_version;
+			msg.hardware_version = gimbal_device_information.hardware_version;
+			msg.uid = gimbal_device_information.uid;
+			msg.cap_flags = gimbal_device_information.cap_flags;
+			msg.roll_min = gimbal_device_information.roll_min;
+			msg.roll_max = gimbal_device_information.roll_max;
+			msg.pitch_min = gimbal_device_information.pitch_min;
+			msg.pitch_max = gimbal_device_information.pitch_max;
+			msg.yaw_min = gimbal_device_information.yaw_min;
+			msg.yaw_max = gimbal_device_information.yaw_max;
+
+			mavlink_msg_gimbal_device_information_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // GIMBAL_DEVICE_INFORMATION_HPP
diff --git a/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp b/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp
index 22802b9082..fb707b3d14 100644
--- a/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp
+++ b/src/modules/mavlink/streams/GPS_GLOBAL_ORIGIN.hpp
@@ -58,12 +58,8 @@ public:
 
 	bool request_message(float param2, float param3, float param4, float param5, float param6, float param7) override
 	{
-		if (_valid) {
-			_force_next_send = true;
-			return true;
-		}
-
-		return false;
+		_force_next_send = true;
+		return send();
 	}
 
 private:
@@ -75,15 +71,14 @@ private:
 	double _ref_lat{static_cast<double>(NAN)};
 	double _ref_lon{static_cast<double>(NAN)};
 	float _ref_alt{NAN};
-
-	bool _valid{false};
 	bool _force_next_send{true};
 
 	bool send() override
 	{
-		vehicle_local_position_s vehicle_local_position;
+		vehicle_local_position_s vehicle_local_position{};
 
-		if (_vehicle_local_position_sub.update(&vehicle_local_position)) {
+		if ((_vehicle_local_position_sub.updated() || _force_next_send)
+		    && _vehicle_local_position_sub.copy(&vehicle_local_position)) {
 			if (vehicle_local_position.xy_global && vehicle_local_position.z_global) {
 
 				static constexpr double LLA_MIN_DIFF = 0.0000001; // ~11.132 mm at the equator
@@ -107,7 +102,6 @@ private:
 					_ref_alt       = vehicle_local_position.ref_alt;
 
 					_force_next_send = false;
-					_valid = true;
 
 					return true;
 				}
diff --git a/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp b/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp
index 8977999457..c06fef2120 100644
--- a/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp
+++ b/src/modules/mavlink/streams/LOCAL_POSITION_NED.hpp
@@ -62,21 +62,20 @@ private:
 		vehicle_local_position_s lpos;
 
 		if (_lpos_sub.update(&lpos)) {
-			if (lpos.xy_valid && lpos.v_xy_valid) {
-				mavlink_local_position_ned_t msg{};
+			mavlink_local_position_ned_t msg{};
 
-				msg.time_boot_ms = lpos.timestamp / 1000;
-				msg.x = lpos.x;
-				msg.y = lpos.y;
-				msg.z = lpos.z;
-				msg.vx = lpos.vx;
-				msg.vy = lpos.vy;
-				msg.vz = lpos.vz;
+			msg.time_boot_ms = lpos.timestamp / 1000;
+			msg.x = lpos.x;
+			msg.y = lpos.y;
+			msg.z = lpos.z;
+			msg.vx = lpos.vx;
+			msg.vy = lpos.vy;
+			msg.vz = lpos.vz;
 
-				mavlink_msg_local_position_ned_send_struct(_mavlink->get_channel(), &msg);
+			mavlink_msg_local_position_ned_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
 
-				return true;
-			}
 		}
 
 		return false;
diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp
new file mode 100644
index 0000000000..a459e58ad5
--- /dev/null
+++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_BASIC_ID.hpp
@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+#ifndef OPEN_DRONE_ID_BASIC_ID_HPP
+#define OPEN_DRONE_ID_BASIC_ID_HPP
+
+#include <uORB/topics/vehicle_status.h>
+
+class MavlinkStreamOpenDroneIdBasicId : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOpenDroneIdBasicId(mavlink); }
+
+	static constexpr const char *get_name_static() { return "OPEN_DRONE_ID_BASIC_ID"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	explicit MavlinkStreamOpenDroneIdBasicId(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	MAV_ODID_UA_TYPE odidTypeForMavType(uint8_t system_type)
+	{
+		switch (system_type) {
+		case MAV_TYPE_GENERIC: return MAV_ODID_UA_TYPE_OTHER;
+
+		case MAV_TYPE_FIXED_WING: return MAV_ODID_UA_TYPE_AEROPLANE;
+
+		case MAV_TYPE_QUADROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_COAXIAL: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_HELICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_ANTENNA_TRACKER: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_GCS: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_AIRSHIP: return MAV_ODID_UA_TYPE_AIRSHIP;
+
+		case MAV_TYPE_FREE_BALLOON: return MAV_ODID_UA_TYPE_FREE_BALLOON;
+
+		case MAV_TYPE_ROCKET: return MAV_ODID_UA_TYPE_ROCKET;
+
+		case MAV_TYPE_GROUND_ROVER: return MAV_ODID_UA_TYPE_GROUND_OBSTACLE;
+
+		case MAV_TYPE_SURFACE_BOAT: return MAV_ODID_UA_TYPE_OTHER;
+
+		case MAV_TYPE_SUBMARINE: return MAV_ODID_UA_TYPE_OTHER;
+
+		case MAV_TYPE_HEXAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_OCTOROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_TRICOPTER: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_FLAPPING_WING: return MAV_ODID_UA_TYPE_ORNITHOPTER;
+
+		case MAV_TYPE_KITE: return MAV_ODID_UA_TYPE_KITE;
+
+		case MAV_TYPE_ONBOARD_CONTROLLER: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_VTOL_TAILSITTER_DUOROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_VTOL_TAILSITTER_QUADROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_VTOL_TILTROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_VTOL_FIXEDROTOR: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_VTOL_TAILSITTER: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_VTOL_RESERVED4: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_VTOL_RESERVED5: return MAV_ODID_UA_TYPE_HYBRID_LIFT;
+
+		case MAV_TYPE_GIMBAL: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_ADSB: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_PARAFOIL: return 	MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE;
+
+		case MAV_TYPE_DODECAROTOR: return MAV_ODID_UA_TYPE_HELICOPTER_OR_MULTIROTOR;
+
+		case MAV_TYPE_CAMERA: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_CHARGING_STATION: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_FLARM: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_SERVO: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_ODID: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_DECAROTOR: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_BATTERY: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_PARACHUTE: return MAV_ODID_UA_TYPE_FREE_FALL_PARACHUTE;
+
+		case MAV_TYPE_LOG: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_OSD: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_IMU: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_GPS: return MAV_ODID_UA_TYPE_NONE;
+
+		case MAV_TYPE_WINCH: return MAV_ODID_UA_TYPE_NONE;
+
+		default: return MAV_ODID_UA_TYPE_OTHER;
+		}
+	}
+
+
+	bool send() override
+	{
+		if (_vehicle_status_sub.updated()) {
+			vehicle_status_s vehicle_status{};
+			_vehicle_status_sub.copy(&vehicle_status);
+
+			mavlink_open_drone_id_basic_id_t msg;
+
+
+			board_get_px4_guid_formated((char *)(msg.uas_id), sizeof(msg.uas_id));
+			msg.id_type = MAV_ODID_ID_TYPE_SERIAL_NUMBER;
+			msg.ua_type = odidTypeForMavType(vehicle_status.system_type);
+			msg.target_component = 0;
+			msg.target_system = 0;
+
+			mavlink_msg_open_drone_id_basic_id_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // OPEN_DRONE_ID_BASIC_ID_HPP
diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp
new file mode 100644
index 0000000000..e0dac3634c
--- /dev/null
+++ b/src/modules/mavlink/streams/OPEN_DRONE_ID_LOCATION.hpp
@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+#ifndef OPEN_DRONE_ID_LOCATION
+#define OPEN_DRONE_ID_LOCATION
+
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+class MavlinkStreamOpenDroneIdLocation : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamOpenDroneIdLocation(mavlink); }
+
+	static constexpr const char *get_name_static() { return "OPEN_DRONE_ID_LOCATION"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
+
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+
+	bool _airborne = false;
+	bool _in_failsafe = false;
+	mavlink_open_drone_id_location_t _message = {0};
+
+	explicit MavlinkStreamOpenDroneIdLocation(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{
+		// Set the message contents to the "UNKNOWN" init values
+		_message.latitude = 0;
+		_message.longitude = 0;
+		_message.altitude_barometric = -1000;
+		_message.altitude_geodetic = -1000;
+		_message.speed_horizontal = 25500;
+		_message.speed_vertical = 6300;
+		_message.direction = 36100;
+		_message.height = -1000;
+	}
+
+	bool send() override
+	{
+		bool updated = false;
+
+		if (_vehicle_status_sub.updated()) {
+			vehicle_status_s vehicle_status{};
+			_vehicle_status_sub.copy(&vehicle_status);
+			_in_failsafe = vehicle_status.failsafe || vehicle_status.failsafe_but_user_took_over;
+			updated = true;
+		}
+
+		if (_vehicle_land_detected_sub.updated()) {
+			vehicle_land_detected_s vehicle_land_detected;
+			_vehicle_land_detected_sub.copy(&vehicle_land_detected);
+			_airborne = !vehicle_land_detected.landed;
+			updated = true;
+		}
+
+		_message.target_component = 0;
+		_message.target_system = 0;
+		_message.height_reference = MAV_ODID_HEIGHT_REF_OVER_TAKEOFF;
+		_message.horizontal_accuracy = MAV_ODID_HOR_ACC_UNKNOWN;
+		_message.vertical_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
+		_message.barometer_accuracy = MAV_ODID_VER_ACC_UNKNOWN;
+		_message.speed_accuracy = MAV_ODID_SPEED_ACC_UNKNOWN;
+
+		_message.status = MAV_ODID_STATUS_GROUND;
+
+		if (_airborne) {
+			_message.status = MAV_ODID_STATUS_AIRBORNE;
+		}
+
+		if (_in_failsafe) {
+			_message.status = MAV_ODID_STATUS_EMERGENCY;
+		}
+
+		if (_vehicle_global_position_sub.updated()) {
+			vehicle_global_position_s vgp;
+			_vehicle_global_position_sub.copy(&vgp);
+			_message.latitude = static_cast<int32_t>(vgp.lat * 1e7);
+			_message.longitude = static_cast<int32_t>(vgp.lon * 1e7);
+			_message.altitude_barometric = vgp.alt;
+			_message.altitude_geodetic = vgp.alt_ellipsoid;
+			updated = true;
+		}
+
+
+		if (_vehicle_local_position_sub.updated()) {
+			vehicle_local_position_s vlp;
+			_vehicle_local_position_sub.copy(&vlp);
+			float speed_xy = sqrtf(vlp.vx * vlp.vx + vlp.vy * vlp.vy);
+			float direction_deg = (atan2f(vlp.vy, vlp.vx) + M_PI_F) * 180.f / M_PI_F;
+			_message.speed_horizontal = vlp.v_xy_valid ?
+						    static_cast<uint16_t>(math::min(25425.f, speed_xy * 100.f)) : 25500;
+			_message.speed_vertical = vlp.v_z_valid ?
+						  static_cast<int16_t>(math::min(6200.f, math::max(-6200.f, -vlp.vz * 100.f))) : 6300;
+			_message.direction = vlp.v_xy_valid ? static_cast<uint16_t>(direction_deg * 100.f) : 36100;
+			_message.height = vlp.z_valid ? -vlp.z : -1000;
+			updated = true;
+		}
+
+		struct timespec ts = {};
+
+		px4_clock_gettime(CLOCK_REALTIME, &ts);
+
+		int64_t utc_time_msec = static_cast<int64_t>(ts.tv_sec) * 1000L + (static_cast<int64_t>(ts.tv_nsec) / 1e6L);
+
+		_message.timestamp = static_cast<float>(utc_time_msec % 3600000L) / 1000.f;
+
+		_message.timestamp_accuracy = MAV_ODID_TIME_ACC_UNKNOWN;
+
+		if (updated) {
+			mavlink_msg_open_drone_id_location_send_struct(_mavlink->get_channel(), &_message);
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // OPEN_DRONE_ID_LOCATION
diff --git a/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp b/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp
index 3476db3a02..36807995c3 100644
--- a/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp
+++ b/src/modules/mavlink/streams/SMART_BATTERY_INFO.hpp
@@ -66,7 +66,7 @@ private:
 			battery_status_s battery_status;
 
 			if (battery_sub.update(&battery_status)) {
-				if (battery_status.serial_number == 0) {
+				if (atoi(battery_status.serial_number) == 0) {
 					// This is not smart battery
 					continue;
 				}
@@ -78,17 +78,8 @@ private:
 				msg.capacity_full = (int32_t)((float)(battery_status.state_of_health * battery_status.capacity) / 100.f);
 				msg.cycle_count = battery_status.cycle_count;
 
-				if (battery_status.manufacture_date) {
-					uint16_t day = battery_status.manufacture_date % 32;
-					uint16_t month = (battery_status.manufacture_date >> 5) % 16;
-					uint16_t year = (80 + (battery_status.manufacture_date >> 9)) % 100;
-
-					//Formatted as 'dd/mm/yy-123456' (maxed 15 + 1 chars)
-					snprintf(msg.serial_number, sizeof(msg.serial_number), "%d/%d/%d-%d", day, month, year, battery_status.serial_number);
-
-				} else {
-					snprintf(msg.serial_number, sizeof(msg.serial_number), "%d", battery_status.serial_number);
-				}
+				static_assert(sizeof(msg.serial_number) == sizeof(battery_status.serial_number), "array size mismatch");
+				memcpy(msg.serial_number, battery_status.serial_number, sizeof(msg.serial_number));
 
 				//msg.device_name = ??
 				msg.weight = -1;
diff --git a/src/modules/mavlink/streams/TIME_ESTIMATE_TO_TARGET.hpp b/src/modules/mavlink/streams/TIME_ESTIMATE_TO_TARGET.hpp
new file mode 100644
index 0000000000..00027802a0
--- /dev/null
+++ b/src/modules/mavlink/streams/TIME_ESTIMATE_TO_TARGET.hpp
@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#ifndef TIME_ESTIMATE_TO_TARGET_HPP
+#define TIME_ESTIMATE_TO_TARGET_HPP
+
+#include <uORB/topics/rtl_time_estimate.h>
+
+class MavlinkStreamTimeEstimateToTarget : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamTimeEstimateToTarget(mavlink); }
+
+	static constexpr const char *get_name_static() { return "TIME_ESTIMATE_TO_TARGET"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	explicit MavlinkStreamTimeEstimateToTarget(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _rtl_estimate_sub{ORB_ID(rtl_time_estimate)};
+
+	bool send() override
+	{
+		if (_rtl_estimate_sub.updated()) {
+			rtl_time_estimate_s rtl_estimate{};
+			_rtl_estimate_sub.copy(&rtl_estimate);
+
+			mavlink_time_estimate_to_target_t msg{};
+			msg.safe_return = rtl_estimate.safe_time_estimate;
+
+			mavlink_msg_time_estimate_to_target_send_struct(_mavlink->get_channel(), &msg);
+
+			return true;
+		}
+
+		return false;
+	}
+};
+
+#endif // TIME_ESTIMATE_TO_TARGET_HPP
diff --git a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_CFG.hpp b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_CFG.hpp
new file mode 100644
index 0000000000..a8767b76d2
--- /dev/null
+++ b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_CFG.hpp
@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#ifndef UAVIONIX_ADSB_OUT_CFG_HPP
+#define UAVIONIX_ADSB_OUT_CFG_HPP
+
+#include <uORB/topics/parameter_update.h>
+
+class MavlinkStreamUavionixADSBOutCfg : public ModuleParams, public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamUavionixADSBOutCfg(mavlink); }
+
+	static constexpr const char *get_name_static() { return "UAVIONIX_ADSB_OUT_CFG"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	bool const_rate() override { return true; }
+
+	unsigned get_size() override
+	{
+		return MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	explicit MavlinkStreamUavionixADSBOutCfg(Mavlink *mavlink) : ModuleParams(nullptr), MavlinkStream(mavlink)
+	{
+		param_t param_fw_airspd_stall = param_find("FW_AIRSPD_STALL");
+
+		if (param_fw_airspd_stall != PARAM_INVALID) {
+			float fw_airspd_stall;
+
+			if (param_get(param_fw_airspd_stall, &fw_airspd_stall) == PX4_OK) {
+				_stall_speed = static_cast<uint16_t>(fw_airspd_stall * 100.0f); // convert [m/s] to [cm/s]
+			}
+		}
+	}
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::ADSB_ICAO_ID>)		_adsb_icao,
+		(ParamInt<px4::params::ADSB_LEN_WIDTH>)		_adsb_len_width,
+		(ParamInt<px4::params::ADSB_EMIT_TYPE>)		_adsb_emit_type,
+		(ParamInt<px4::params::ADSB_GPS_OFF_LAT>)	_adsb_gps_offset_lat,
+		(ParamInt<px4::params::ADSB_GPS_OFF_LON>)	_adsb_gps_offset_lon
+	);
+
+	uint16_t _stall_speed{0}; // [cm/s]
+
+	bool send() override
+	{
+		// Required update for static message is 0.1 [Hz]
+		mavlink_uavionix_adsb_out_cfg_t cfg_msg = {
+			.ICAO = static_cast<uint32_t>(_adsb_icao.get()),
+			.stallSpeed = _stall_speed,
+			.callsign = {'\0'},
+			.emitterType = static_cast<uint8_t>(_adsb_emit_type.get()),
+			.aircraftSize = static_cast<uint8_t>(_adsb_len_width.get()),
+			.gpsOffsetLat = static_cast<uint8_t>(_adsb_gps_offset_lat.get()),
+			.gpsOffsetLon = static_cast<uint8_t>(_adsb_gps_offset_lon.get()),
+			.rfSelect = UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED
+		};
+
+		//memcpy(cfg_msg.callsign, "PX4 UAV ", sizeof(cfg_msg.callsign)); //TODO: add support for callsign
+
+		mavlink_msg_uavionix_adsb_out_cfg_send_struct(_mavlink->get_channel(), &cfg_msg);
+
+		return true;
+	}
+};
+
+#endif // UAVIONIX_ADSB_OUT_CFG_HPP
diff --git a/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp
new file mode 100644
index 0000000000..1edd7cfec3
--- /dev/null
+++ b/src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_DYNAMIC.hpp
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#ifndef UAVIONIX_ADSB_OUT_DYNAMIC_HPP
+#define UAVIONIX_ADSB_OUT_DYNAMIC_HPP
+
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/parameter_update.h>
+
+class MavlinkStreamUavionixADSBOutDynamic : public ModuleParams, public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamUavionixADSBOutDynamic(mavlink); }
+
+	static constexpr const char *get_name_static() { return "UAVIONIX_ADSB_OUT_DYNAMIC"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	bool const_rate() override { return true; }
+
+	unsigned get_size() override
+	{
+		return MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	explicit MavlinkStreamUavionixADSBOutDynamic(Mavlink *mavlink) : ModuleParams(nullptr), MavlinkStream(mavlink) {}
+
+
+	uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
+	uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	DEFINE_PARAMETERS(
+		(ParamInt<px4::params::ADSB_SQUAWK>)		_adsb_squawk,
+		(ParamInt<px4::params::ADSB_EMERGC>)		_adsb_emergc
+	);
+
+
+	bool send() override
+	{
+
+		vehicle_status_s vehicle_status;
+		_vehicle_status_sub.copy(&vehicle_status);
+
+		vehicle_gps_position_s vehicle_gps_position;
+		_vehicle_gps_position_sub.copy(&vehicle_gps_position);
+
+		vehicle_air_data_s vehicle_air_data;
+		_vehicle_air_data_sub.copy(&vehicle_air_data);
+
+		// Required update for dynamic message is 5 [Hz]
+		mavlink_uavionix_adsb_out_dynamic_t dynamic_msg = {
+			.utcTime = static_cast<uint32_t>(vehicle_gps_position.time_utc_usec / 1000000ULL),
+			.gpsLat = vehicle_gps_position.lat,
+			.gpsLon = vehicle_gps_position.lon,
+			.gpsAlt = vehicle_gps_position.alt_ellipsoid,
+			.baroAltMSL = static_cast<int32_t>(vehicle_air_data.baro_pressure_pa / 100.0f), // convert [Pa] to [mBar]
+			.accuracyHor = static_cast<uint32_t>(vehicle_gps_position.eph * 1000.0f), // convert [m] to [mm]
+			.accuracyVert = static_cast<uint16_t>(vehicle_gps_position.epv * 100.0f), // convert [m] to [cm]
+			.accuracyVel = static_cast<uint16_t>(vehicle_gps_position.s_variance_m_s * 1000.f), // convert [m/s] to [mm/s],
+			.velVert = static_cast<int16_t>(-1.0f * vehicle_gps_position.vel_d_m_s * 100.0f), // convert [m/s] to [cm/s]
+			.velNS = static_cast<int16_t>(vehicle_gps_position.vel_n_m_s * 100.0f), // convert [m/s] to [cm/s]
+			.VelEW = static_cast<int16_t>(vehicle_gps_position.vel_e_m_s * 100.0f), // convert [m/s] to [cm/s]
+			.state = UAVIONIX_ADSB_OUT_DYNAMIC_STATE_ON_GROUND,
+			.squawk = static_cast<uint16_t>(_adsb_squawk.get()),
+			.gpsFix = vehicle_gps_position.fix_type,
+			.numSats = vehicle_gps_position.satellites_used,
+			.emergencyStatus = static_cast<uint8_t>(_adsb_emergc.get())
+		};
+
+		if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+			dynamic_msg.state |= ~UAVIONIX_ADSB_OUT_DYNAMIC_STATE_ON_GROUND;
+		}
+
+		mavlink_msg_uavionix_adsb_out_dynamic_send_struct(_mavlink->get_channel(), &dynamic_msg);
+
+		return true;
+	}
+};
+
+#endif // UAVIONIX_ADSB_OUT_DYNAMIC_HPP
diff --git a/src/modules/mavlink/terrain.cpp b/src/modules/mavlink/terrain.cpp
new file mode 100644
index 0000000000..6cf98d2ee4
--- /dev/null
+++ b/src/modules/mavlink/terrain.cpp
@@ -0,0 +1,595 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 "terrain.h"
+
+#include <dirent.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <errno.h>
+
+using namespace terrain;
+
+constexpr char terrain::TerrainUploader::File::tmp_filename[];
+
+
+TerrainUploader::TerrainUploader(int grid_spacing_m)
+{
+	_latest_request.grid_spacing = grid_spacing_m;
+	PX4_DEBUG("terrain: max num items per file: %i (grid spacing=%i m)", util::maxNumItemsPerFile(grid_spacing_m),
+		  grid_spacing_m);
+}
+
+int TerrainUploader::updateArea(const hrt_abstime &now, double lat_sw, double lon_sw, double lat_ne, double lon_ne)
+{
+	// align to file boundary
+	lat_sw = floor(lat_sw / util::delta_lat_lon_deg) * util::delta_lat_lon_deg;
+	lon_sw = floor(lon_sw / util::delta_lat_lon_deg) * util::delta_lat_lon_deg;
+
+	double min_diff = 0.00001;
+
+	if (latestRequestValid() && fabs(lat_sw - _lat_start) < min_diff  &&
+	    fabs(lon_sw - _lon_start) < min_diff  &&
+	    fabs(lat_ne - _lat_end) < min_diff  &&
+	    fabs(lon_ne - _lon_end) < min_diff) {
+		PX4_DEBUG("Request already in progress");
+		return 0;
+	}
+
+	pruneStorage(lat_sw, lon_sw, lat_ne, lon_ne);
+
+	// create storage dir if not exists
+	int mkdir_ret = mkdir(TERRAIN_STORAGE_DIR, S_IRWXU | S_IRWXG | S_IRWXO);
+
+	if (mkdir_ret != OK && errno != EEXIST) {
+		PX4_ERR("failed creating dir: %s (%i)", TERRAIN_STORAGE_DIR, errno);
+		_latest_error = Error::DirectoryCreationFailure;
+		return errno;
+	}
+
+	if (lat_ne < lat_sw || lon_ne < lon_sw) {
+		PX4_ERR("invalid lat/lon area");
+		return -EINVAL;
+	}
+
+	if (lat_sw < -maximum_latitude || lat_ne > maximum_latitude) {
+		PX4_ERR("unsupported lat/lon area");
+		_latest_error = Error::LatitudeExceeded;
+		return -EINVAL;
+	}
+
+	_lat_cur = _lat_start = lat_sw;
+	_lon_cur = _lon_start = lon_sw;
+	_lat_end = lat_ne;
+	_lon_end = lon_ne;
+	_num_4x4_blocks_loaded = 0;
+
+	// calculate the number of pending blocks
+	_num_4x4_blocks_pending = 0;
+
+	for (double lat = _lat_start; lat <= _lat_end; lat += util::delta_lat_lon_deg) {
+		for (double lon = _lon_start; lon <= _lon_end; lon += util::delta_lat_lon_deg) {
+			if (_file.fileExists(lat, lon)) {
+				continue;
+			}
+
+			MapProjection ref(lat, lon);
+			float x, y;
+			ref.project(lat + util::delta_lat_lon_deg, lon + util::delta_lat_lon_deg, x, y);
+			int num_items_x = (x + _latest_request.grid_spacing) / _latest_request.grid_spacing +
+					  1; // see TerrainUploader::File::init()
+			int num_items_y = (y + _latest_request.grid_spacing) / _latest_request.grid_spacing + 1;
+			_num_4x4_blocks_pending += ((num_items_x + util::mavlink_terrain_data_size - 1) / util::mavlink_terrain_data_size) *
+						   ((num_items_y + util::mavlink_terrain_data_size - 1) / util::mavlink_terrain_data_size);
+			PX4_DEBUG("num items x=%i, y=%i, pending blocks=%i", num_items_x, num_items_y, _num_4x4_blocks_pending);
+		}
+	}
+
+	if (_num_4x4_blocks_pending > 0) {
+		PX4_INFO("Terrain: area update: SW: %.3f %.3f, NE: %.3f %.3f",
+			 lat_sw, lon_sw, lat_ne, lon_ne);
+	}
+
+	requestNextFile(now, true);
+
+	return 0;
+}
+
+void TerrainUploader::update(const hrt_abstime &now)
+{
+	if (!latestRequestValid()) {
+		return;
+	}
+
+	if (_next_timeout < now) {
+		// resend current request on a timeout
+		++_current_request_id;
+		_next_timeout = now + request_timeout;
+		PX4_DEBUG("timed out, re-requesting");
+	}
+}
+
+int TerrainUploader::getLatestRequest(const mavlink_terrain_request_t **request) const
+{
+	if (latestRequestValid()) {
+		*request = &_latest_request;
+
+	} else {
+		*request = nullptr;
+	}
+
+	return _current_request_id;
+}
+
+void TerrainUploader::handleTerrainData(const hrt_abstime &now, const mavlink_terrain_data_t &data)
+{
+	if (data.grid_spacing != _latest_request.grid_spacing) {
+		PX4_WARN("unexpected grid_spacing: %i %i", data.grid_spacing, _latest_request.grid_spacing);
+		return;
+	}
+
+	if (data.lat != _latest_request.lat || data.lon != _latest_request.lon) {
+		PX4_DEBUG("unexpected lat/lon: %i == %i, %i == %i", data.lat, _latest_request.lat, data.lon, _latest_request.lon);
+		return;
+	}
+
+	uint64_t bit = (uint64_t)1 << data.gridbit;
+
+	if ((_latest_request.mask & bit) == 0) {
+		// already done / not requested
+		return;
+	}
+
+	static_assert(sizeof(data.data) / sizeof(data.data[0]) == util::mavlink_terrain_data_size *
+		      util::mavlink_terrain_data_size, "mavlink data size mismatch");
+	int block_x = (data.gridbit / util::mavlink_num_blocks_y) * util::mavlink_terrain_data_size;
+	int block_y = (data.gridbit % util::mavlink_num_blocks_y) * util::mavlink_terrain_data_size;
+	PX4_DEBUG("Got terrain data: %.3f %.3f, bit=%i, bx=%i, by=%i, data[0]=%i", data.lat * 1e-7, data.lon * 1e-7,
+		  data.gridbit, block_x, block_y, data.data[0]);
+
+	int ret = _file.writeBlock(data.data, block_x, block_y);
+
+	if (ret != 0) {
+		PX4_ERR("block writing failed (%i)", ret);
+		return;
+	}
+
+	_latest_request.mask &= ~bit; // mark as done
+	--_num_4x4_blocks_pending;
+	++_num_4x4_blocks_loaded;
+	_next_timeout = now + request_timeout;
+
+	if (_latest_request.mask == 0) {
+		if (_file.nextMavlinkRequest(_latest_request)) {
+			++_current_request_id;
+
+		} else {
+			ret = _file.finalize();
+
+			if (ret != 0) {
+				PX4_ERR("finalize failed (%i)", ret);
+				_latest_error = Error::StorageFailure;
+			}
+
+			requestNextFile(now);
+		}
+	}
+}
+
+void TerrainUploader::requestNextFile(const hrt_abstime &now, bool start_with_current)
+{
+	_next_timeout = now + request_timeout;
+	bool need_next_file = false;
+	double delta_lat = start_with_current ? 0. : util::delta_lat_lon_deg;
+
+	do {
+		need_next_file = false;
+		_lat_cur += delta_lat;
+
+		if (_lat_cur > _lat_end) {
+			_lon_cur += util::delta_lat_lon_deg;
+
+			if (_lon_cur > _lon_end) {
+				allRequestsDone();
+				return;
+			}
+
+			_lat_cur = _lat_start;
+		}
+
+		int ret = _file.init(_lat_cur, _lon_cur, _latest_request.grid_spacing);
+
+		if (ret == 1) {
+			need_next_file = true; // already done
+
+		} else if (ret < 0) {
+			PX4_ERR("Failed to init file (%i)", ret);
+			_latest_error = Error::StorageFailure;
+			need_next_file = true; // skip this
+
+		} else {
+			if (_file.nextMavlinkRequest(_latest_request)) {
+				++_current_request_id;
+			}
+		}
+
+		delta_lat = util::delta_lat_lon_deg;
+	} while (need_next_file);
+}
+
+void TerrainUploader::allRequestsDone()
+{
+	if (_num_4x4_blocks_loaded > 0) {
+		PX4_INFO("Terrain: finished all requests");
+	}
+
+	if (_num_4x4_blocks_pending != 0) {
+		PX4_DEBUG("pending blocks not zero! %i", _num_4x4_blocks_pending);
+		_num_4x4_blocks_pending = 0;
+	}
+
+	_next_timeout = 0;
+	_latest_request.mask = 0;
+}
+
+void TerrainUploader::pruneStorage(double lat_sw, double lon_sw, double lat_ne, double lon_ne)
+{
+	DIR *dp = opendir(TERRAIN_STORAGE_DIR);
+
+	if (dp == nullptr) {
+		return; // ignore if we cannot access the directory
+	}
+
+	struct dirent *result = nullptr;
+
+	while ((result = readdir(dp))) {
+
+		if (result->d_type != DIRENT_REGULAR_FILE) {
+			continue;
+		}
+
+		char filename[64];
+
+		if (snprintf(filename, sizeof(filename), "%s/%s", TERRAIN_STORAGE_DIR, result->d_name) >= (int)sizeof(filename)) {
+			PX4_ERR("file name too long: %s", result->d_name);
+			continue;
+		}
+
+		unsigned tag_lat, tag_lon;
+		bool delete_file = false;
+
+		if (util::getTagsFromFileName(result->d_name, tag_lat, tag_lon)) {
+
+			int fd = open(filename, O_RDONLY, PX4_O_MODE_666);
+
+			if (fd == -1) {
+				PX4_ERR("failed to open file %s", filename);
+				continue;
+			}
+
+			// validate the file matches our expectations and is not too far away from the requested area
+			FileHeader header;
+			uint16_t expected_magic = header.magic;
+			uint16_t expected_version = header.version;
+
+			if (read(fd, &header, sizeof(header)) == sizeof(header)) {
+				if (header.magic != expected_magic || header.version != expected_version ||
+				    header.grid_spacing_m != _latest_request.grid_spacing) {
+					delete_file = true;
+
+				} else {
+
+					// check if too far away. This could be extended, e.g. using a maximum used storage size parameter
+					const double max_lat_lon_diff = 0.1;
+					double lat = header.lat_sw;
+					double lon = header.lon_sw;
+
+					// handle wrap-arounds
+					while (lat < lat_sw - max_lat_lon_diff) { lat += 180.; }
+
+					while (lat > lat_ne + max_lat_lon_diff) { lat -= 180.; }
+
+					while (lon < lon_sw - max_lat_lon_diff) { lon += 360.; }
+
+					while (lon > lon_ne + max_lat_lon_diff) { lon -= 360.; }
+
+					delete_file =
+						lat < lat_sw - max_lat_lon_diff ||
+						lat > lat_ne + max_lat_lon_diff ||
+						lon < lon_sw - max_lat_lon_diff ||
+						lon > lon_ne + max_lat_lon_diff;
+				}
+
+			} else {
+				delete_file = true;
+			}
+
+			close(fd);
+
+		} else {
+			delete_file = true;
+		}
+
+		if (delete_file) {
+			PX4_DEBUG("Removing terrain file %s", result->d_name);
+
+			if (unlink(filename) != 0) {
+				PX4_ERR("unlink failed (%i)", errno);
+			}
+		}
+	}
+
+	closedir(dp);
+}
+
+TerrainUploader::File::~File()
+{
+	if (_fd >= 0) {
+		close(_fd);
+	}
+}
+
+int TerrainUploader::File::init(double lat, double lon, int grid_spacing_m)
+{
+	if (_fd >= 0) {
+		close(_fd);
+	}
+
+	_header.grid_spacing_m = grid_spacing_m;
+
+	// check if file already exists, so that we don't need to request it
+	if (fileExists(lat, lon)) {
+		// as the file prunning already removed files with different grid spacing etc., we don't need to check that here
+		PX4_DEBUG("skipping existing file: %.3f, %.3f", _header.lat_sw, _header.lon_sw);
+		return 1;
+	}
+
+	_ref.initReference(_header.lat_sw, _header.lon_sw);
+	float x, y;
+	_ref.project(_header.lat_sw + util::delta_lat_lon_deg, _header.lon_sw + util::delta_lat_lon_deg, x, y);
+	_header.num_points_x = (x + grid_spacing_m) / grid_spacing_m +
+			       1; // add grid_spacing to ensure overlap and one more as x/spacing is the max index
+	_header.num_points_y = (y + grid_spacing_m) / grid_spacing_m + 1;
+
+	_fd = open(tmp_filename, O_CREAT | O_WRONLY, PX4_O_MODE_666);
+
+	if (_fd < 0) {
+		PX4_ERR("Can't open file %s, errno: %d", tmp_filename, errno);
+		return -1;
+	}
+
+	if (write(_fd, &_header, sizeof(_header)) < (int)sizeof(_header)) {
+		return -errno;
+	}
+
+	// fill the file with 0 data
+	uint8_t buf[128];
+	int num_write_bytes = sizeof(int16_t) * _header.num_points_x * _header.num_points_y;
+
+	while (num_write_bytes > 0) {
+		int num_write_bytes_current = math::min((int)sizeof(buf), num_write_bytes);
+
+		if (write(_fd, &buf, num_write_bytes_current) < num_write_bytes_current) {
+			return -errno;
+		}
+
+		num_write_bytes -= num_write_bytes_current;
+	}
+
+	PX4_DEBUG("init file: %.3f, %.3f, num x=%i num y=%i", _header.lat_sw, _header.lon_sw, _header.num_points_x,
+		  _header.num_points_y);
+
+	_x = _y = -1;
+	return 0;
+}
+
+bool TerrainUploader::File::fileExists(double lat, double lon)
+{
+	_header.lat_sw = round(matrix::wrap(lat, -90., 90.) / util::delta_lat_lon_deg) * util::delta_lat_lon_deg;
+	_header.lon_sw = round(matrix::wrap(lon, -180., 180.) / util::delta_lat_lon_deg) * util::delta_lat_lon_deg;
+
+	char filename[64];
+	getCurrentFileName(filename, sizeof(filename));
+	struct stat buffer;
+	return stat(filename, &buffer) == 0;
+}
+
+int TerrainUploader::File::writeBlock(const int16_t data[util::mavlink_terrain_data_size *
+								      util::mavlink_terrain_data_size], int block_x, int block_y)
+{
+	// get x and y indexes
+	const int x0 = _x + block_x;
+	const int y0 = _y + block_y;
+	PX4_DEBUG("writing indexes: x=%i y=%i", x0, y0);
+
+	// bound check
+	if (x0 < 0 || x0 >= _header.num_points_x) {
+		PX4_ERR("x idx out of bounds: %i (%i)", x0, _header.num_points_x);
+		return -EINVAL;
+	}
+
+	if (y0 < 0 || y0 >= _header.num_points_y) {
+		PX4_ERR("y idx out of bounds: %i (%i)", y0, _header.num_points_y);
+		return -EINVAL;
+	}
+
+	// TODO: might be worth storing everything (or 4 rows) in a buffer and write all at once
+	for (int x1 = 0; x1 < util::mavlink_terrain_data_size; ++x1) {
+		if (x0 + x1 >= _header.num_points_x) {
+			break;
+		}
+
+		ssize_t offset = sizeof(_header) + (y0 + (x0 + x1) * _header.num_points_y) * sizeof(int16_t);
+
+		if (lseek(_fd, offset, SEEK_SET) < 0) {
+			return -errno;
+		}
+
+		int num_y = math::min(util::mavlink_terrain_data_size, _header.num_points_y - y0);
+
+		if (write(_fd, &data[x1 * util::mavlink_terrain_data_size], sizeof(int16_t)*num_y) < (int)sizeof(int16_t)*num_y) {
+			return -errno;
+		}
+	}
+
+	return 0;
+}
+
+bool TerrainUploader::File::nextMavlinkRequest(mavlink_terrain_request_t &request)
+{
+	if (_fd < 0) {
+		return false;
+	}
+
+	if (_x == -1) {
+		_x = _y = 0;
+
+	} else {
+		_x += util::mavlink_num_blocks_x * util::mavlink_terrain_data_size;
+
+		if (_x >= _header.num_points_x) {
+			_y += util::mavlink_num_blocks_y * util::mavlink_terrain_data_size;
+
+			if (_y >= _header.num_points_y) {
+				// we're all done
+				return false;
+			}
+
+			_x = 0;
+		}
+	}
+
+	double lat, lon;
+	_ref.reproject(_x * _header.grid_spacing_m, _y * _header.grid_spacing_m, lat, lon);
+	request.lat = lat * 1e7;
+	request.lon = lon * 1e7;
+	// fill in the block bits we want
+	int x_requests = math::min(util::mavlink_num_blocks_x,
+				   (_header.num_points_x - _x + util::mavlink_terrain_data_size - 1) / util::mavlink_terrain_data_size);
+	int y_requests = math::min(util::mavlink_num_blocks_y,
+				   (_header.num_points_y - _y + util::mavlink_terrain_data_size - 1) / util::mavlink_terrain_data_size);
+	request.mask = 0;
+
+	for (int x = 0; x < x_requests; ++x) {
+		for (int y = 0; y < y_requests; ++y) {
+			request.mask |= (uint64_t)1 << (y + x * util::mavlink_num_blocks_y);
+		}
+	}
+
+	PX4_DEBUG("sending new request: %.3f %.3f", lat, lon);
+	return true;
+}
+
+int TerrainUploader::File::finalize()
+{
+	if (_fd < 0) {
+		return -1;
+	}
+
+	close(_fd);
+	_fd = -1;
+	char filename[64];
+	getCurrentFileName(filename, sizeof(filename));
+	PX4_DEBUG("renaming terrain block file: %s -> %s", tmp_filename, filename);
+
+	if (rename(tmp_filename, filename) == -1) {
+		return -errno;
+	}
+
+	return 0;
+}
+
+void TerrainUploader::File::getCurrentFileName(char *filename, int len)
+{
+	util::getFileName(_header.lat_sw + util::delta_lat_lon_deg / 2., _header.lon_sw + util::delta_lat_lon_deg / 2.,
+			  filename, len);
+}
+
+void TerrainUploader::printStatus()
+{
+	PX4_INFO("Num pending 4x4 blocks: %i", _num_4x4_blocks_pending);
+	PX4_INFO("Num downloaded 4x4 blocks: %i", _num_4x4_blocks_loaded);
+	PX4_INFO("Storage dir: %s", TERRAIN_STORAGE_DIR);
+
+	DIR *dp = opendir(TERRAIN_STORAGE_DIR);
+
+	if (dp == nullptr) {
+		return;
+	}
+
+	PX4_INFO("Storage files:");
+
+	struct dirent *result = nullptr;
+
+	while ((result = readdir(dp))) {
+
+		if (result->d_type != DIRENT_REGULAR_FILE) {
+			continue;
+		}
+
+		char filename[64];
+
+		if (snprintf(filename, sizeof(filename), "%s/%s", TERRAIN_STORAGE_DIR, result->d_name) >= (int)sizeof(filename)) {
+			PX4_ERR("file name too long: %s", result->d_name);
+			continue;
+		}
+
+		unsigned tag_lat, tag_lon;
+
+		if (util::getTagsFromFileName(result->d_name, tag_lat, tag_lon)) {
+
+			int fd = open(filename, O_RDONLY, PX4_O_MODE_666);
+
+			if (fd == -1) {
+				PX4_ERR("failed to open file %s", filename);
+				continue;
+			}
+
+			FileHeader header;
+			uint16_t expected_magic = header.magic;
+
+			if (read(fd, &header, sizeof(header)) == sizeof(header)) {
+				if (header.magic == expected_magic) {
+					PX4_INFO("%s: spacing: %im, lat: %.3f, lon: %.3f, #x: %i, #y: %i",
+						 result->d_name, header.grid_spacing_m, header.lat_sw, header.lon_sw,
+						 header.num_points_x, header.num_points_y);
+				}
+			}
+
+			close(fd);
+
+		}
+	}
+
+	closedir(dp);
+}
diff --git a/src/modules/mavlink/terrain.h b/src/modules/mavlink/terrain.h
new file mode 100644
index 0000000000..38e78da251
--- /dev/null
+++ b/src/modules/mavlink/terrain.h
@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 <stdint.h>
+#include <math.h>
+
+#include "mavlink_bridge_header.h"
+
+#include <drivers/drv_hrt.h>
+#include <lib/geo/geo.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/terrain/util.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/log.h>
+#include <matrix/math.hpp>
+
+using namespace time_literals;
+
+namespace terrain
+{
+
+// The following are given by the mavlink messages
+static constexpr int mavlink_num_blocks_x = 7; ///< 7 blocks in north direction
+static constexpr int mavlink_num_blocks_y = 8; ///< 8 blocks in east direction
+static constexpr int mavlink_terrain_data_size = 4; ///< 4x4 data points in TERRAIN_DATA.data
+
+
+/**
+ * @class TerrainUploader
+ * Handles terrain upload and disk storage
+ */
+class TerrainUploader
+{
+public:
+	static constexpr double maximum_latitude = 80.; ///< maximum supported latitude (this also holds for negative values).
+	///< Beyond that the distortion becomes too high (though we could go up to ~85 deg if needed)
+
+	static constexpr hrt_abstime request_timeout = 200_ms;
+
+	/// User-facing errors
+	enum class Error {
+		None = 0,
+		LatitudeExceeded,
+		DirectoryCreationFailure,
+		StorageFailure,
+	};
+
+	TerrainUploader(int grid_spacing_m);
+	~TerrainUploader() = default;
+
+	/**
+	 * Configure an area in which terrain data should be requested and stored to disk.
+	 * If the same area is requested as previously, no new requests are done.
+	 * Note: the latitude must be within [-maximum_latitude, maximum_latitude]. The longitude can be outside of [-180, 180].
+	 * Note: the caller is responsible to add a margin around the area.
+	 * @param now
+	 * @param lat_sw SW latitude corner
+	 * @param lon_sw SW longitude corner
+	 * @param lat_ne NE latitude corner
+	 * @param lon_ne NE longitude corner
+	 * @return 0 on success, <0 error otherwise
+	 */
+	int updateArea(const hrt_abstime &now, double lat_sw, double lon_sw, double lat_ne, double lon_ne);
+
+	void update(const hrt_abstime &now);
+
+	int num4x4BlocksPending() const { return _num_4x4_blocks_pending; }
+	int num4x4BlocksLoaded() const { return _num_4x4_blocks_loaded; }
+
+	/**
+	 * Get the latest request ID & data pointer
+	 * @param request output request (set to nullptr if no active request)
+	 * @return request ID. If this differs from a previous ID, a new request should be sent
+	 */
+	int getLatestRequest(const mavlink_terrain_request_t **request) const;
+
+	void handleTerrainData(const hrt_abstime &now, const mavlink_terrain_data_t &data);
+
+	void printStatus();
+
+	int gridSpacing() const { return _latest_request.grid_spacing; }
+
+	Error getLatestErrorAndClear() { Error e = _latest_error; _latest_error = Error::None; return e; }
+private:
+
+	class File
+	{
+	public:
+		~File();
+
+		/**
+		 * Initialize a new file
+		 * @param lat latitude, expected to be aligned to a multiple of util::delta_lat_lon_deg
+		 * @param lon longitude, expected to be aligned to a multiple of util::delta_lat_lon_deg
+		 * @param grid_spacing_m
+		 * @return 0 on success, 1 if file already exists, <0 error otherwise
+		 */
+		int init(double lat, double lon, int grid_spacing_m);
+
+		int writeBlock(const int16_t data[util::mavlink_terrain_data_size * util::mavlink_terrain_data_size], int block_x,
+			       int block_y);
+
+		bool nextMavlinkRequest(mavlink_terrain_request_t &request);
+
+		int finalize();
+
+		bool fileExists(double lat, double lon);
+	private:
+		static constexpr char tmp_filename[] = TERRAIN_STORAGE_DIR "/next.dat";
+		void getCurrentFileName(char *filename, int len);
+
+		FileHeader _header;
+		MapProjection _ref;
+		int _fd{-1};
+		int _x{-1};
+		int _y{-1};
+	};
+
+	bool latestRequestValid() const { return _latest_request.mask != 0; }
+
+	void requestNextFile(const hrt_abstime &now, bool start_with_current = false);
+	void allRequestsDone();
+
+	void pruneStorage(double lat_sw, double lon_sw, double lat_ne, double lon_ne);
+
+	int _current_request_id{0};
+	mavlink_terrain_request_t _latest_request{};
+	hrt_abstime _next_timeout{0};
+	int _num_4x4_blocks_pending{0};
+	int _num_4x4_blocks_loaded{0};
+
+	File _file;
+
+	double _lat_start;
+	double _lon_start;
+	double _lat_end;
+	double _lon_end;
+	double _lat_cur;
+	double _lon_cur;
+
+	Error _latest_error{Error::None};
+};
+
+}
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 88eb51aa51..8789a3dd52 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -96,17 +96,13 @@ MulticopterAttitudeControl::parameters_updated()
 float
 MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
 {
-	const float throttle_min = _landed ? 0.0f : _param_mpc_manthr_min.get();
-
 	// throttle_stick_input is in range [0, 1]
 	switch (_param_mpc_thr_curve.get()) {
 	case 1: // no rescaling to hover throttle
-		return throttle_min + throttle_stick_input * (_param_mpc_thr_max.get() - throttle_min);
+		return math::interpolate(throttle_stick_input, 0.f, 1.f, _param_mpc_manthr_min.get(), _param_mpc_thr_max.get());
 
 	default: // 0 or other: rescale to hover throttle at 0.5 stick
-		return math::gradual3(throttle_stick_input,
-				      0.f, .5f, 1.f,
-				      throttle_min, _param_mpc_thr_hover.get(), _param_mpc_thr_max.get());
+		return math::interpolateN(throttle_stick_input, {_param_mpc_manthr_min.get(), _param_mpc_thr_hover.get(), _param_mpc_thr_max.get()});
 	}
 }
 
diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp
index 47e1aecf2d..501d3d857e 100644
--- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp
+++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp
@@ -52,7 +52,6 @@ MulticopterPositionControl::MulticopterPositionControl(bool vtol) :
 {
 	parameters_update(true);
 	_tilt_limit_slew_rate.setSlewRate(.2f);
-	reset_setpoint_to_nan(_setpoint);
 	_takeoff_status_pub.advertise();
 }
 
@@ -120,6 +119,8 @@ void MulticopterPositionControl::parameters_update(bool force)
 		if (_param_mpc_xy_vel_all.get() >= 0.f) {
 			float xy_vel = _param_mpc_xy_vel_all.get();
 			num_changed += _param_mpc_vel_manual.commit_no_notification(xy_vel);
+			num_changed += _param_mpc_vel_man_back.commit_no_notification(-1.f);
+			num_changed += _param_mpc_vel_man_side.commit_no_notification(-1.f);
 			num_changed += _param_mpc_xy_cruise.commit_no_notification(xy_vel);
 			num_changed += _param_mpc_xy_vel_max.commit_no_notification(xy_vel);
 		}
@@ -190,6 +191,28 @@ void MulticopterPositionControl::parameters_update(bool force)
 					    "Manual speed has been constrained by maximum speed", _param_mpc_xy_vel_max.get());
 		}
 
+		if (_param_mpc_vel_man_back.get() > _param_mpc_vel_manual.get()) {
+			_param_mpc_vel_man_back.set(_param_mpc_vel_manual.get());
+			_param_mpc_vel_man_back.commit();
+			mavlink_log_critical(&_mavlink_log_pub, "Manual backward speed has been constrained by forward speed\t");
+			/* EVENT
+			 * @description <param>MPC_VEL_MAN_BACK</param> is set to {1:.0}.
+			 */
+			events::send<float>(events::ID("mc_pos_ctrl_man_vel_back_set"), events::Log::Warning,
+					    "Manual backward speed has been constrained by forward speed", _param_mpc_vel_manual.get());
+		}
+
+		if (_param_mpc_vel_man_side.get() > _param_mpc_vel_manual.get()) {
+			_param_mpc_vel_man_side.set(_param_mpc_vel_manual.get());
+			_param_mpc_vel_man_side.commit();
+			mavlink_log_critical(&_mavlink_log_pub, "Manual sideways speed has been constrained by forward speed\t");
+			/* EVENT
+			 * @description <param>MPC_VEL_MAN_SIDE</param> is set to {1:.0}.
+			 */
+			events::send<float>(events::ID("mc_pos_ctrl_man_vel_side_set"), events::Log::Warning,
+					    "Manual sideways speed has been constrained by forward speed", _param_mpc_vel_manual.get());
+		}
+
 		if (_param_mpc_z_v_auto_up.get() > _param_mpc_z_vel_max_up.get()) {
 			_param_mpc_z_v_auto_up.set(_param_mpc_z_vel_max_up.get());
 			_param_mpc_z_v_auto_up.commit();
@@ -234,7 +257,7 @@ void MulticopterPositionControl::parameters_update(bool force)
 		_param_mpc_tko_speed.set(math::min(_param_mpc_tko_speed.get(), _param_mpc_z_vel_max_up.get()));
 		_param_mpc_land_speed.set(math::min(_param_mpc_land_speed.get(), _param_mpc_z_vel_max_dn.get()));
 
-		_takeoff.setSpoolupTime(_param_mpc_spoolup_time.get());
+		_takeoff.setSpoolupTime(_param_com_spoolup_time.get());
 		_takeoff.setTakeoffRampTime(_param_mpc_tko_ramp_t.get());
 		_takeoff.generateInitialRampValue(_param_mpc_z_vel_p_acc.get());
 	}
@@ -331,7 +354,7 @@ void MulticopterPositionControl::Run()
 
 				} else if (previous_position_control_enabled && !_vehicle_control_mode.flag_multicopter_position_control_enabled) {
 					// clear existing setpoint when controller is no longer active
-					reset_setpoint_to_nan(_setpoint);
+					_setpoint = PositionControl::empty_trajectory_setpoint;
 				}
 			}
 		}
@@ -353,21 +376,21 @@ void MulticopterPositionControl::Run()
 		// adjust existing (or older) setpoint with any EKF reset deltas
 		if ((_setpoint.timestamp != 0) && (_setpoint.timestamp < vehicle_local_position.timestamp)) {
 			if (vehicle_local_position.vxy_reset_counter != _vxy_reset_counter) {
-				_setpoint.vx += vehicle_local_position.delta_vxy[0];
-				_setpoint.vy += vehicle_local_position.delta_vxy[1];
+				_setpoint.velocity[0] += vehicle_local_position.delta_vxy[0];
+				_setpoint.velocity[1] += vehicle_local_position.delta_vxy[1];
 			}
 
 			if (vehicle_local_position.vz_reset_counter != _vz_reset_counter) {
-				_setpoint.vz += vehicle_local_position.delta_vz;
+				_setpoint.velocity[2] += vehicle_local_position.delta_vz;
 			}
 
 			if (vehicle_local_position.xy_reset_counter != _xy_reset_counter) {
-				_setpoint.x += vehicle_local_position.delta_xy[0];
-				_setpoint.y += vehicle_local_position.delta_xy[1];
+				_setpoint.position[0] += vehicle_local_position.delta_xy[0];
+				_setpoint.position[1] += vehicle_local_position.delta_xy[1];
 			}
 
 			if (vehicle_local_position.z_reset_counter != _z_reset_counter) {
-				_setpoint.z += vehicle_local_position.delta_z;
+				_setpoint.position[2] += vehicle_local_position.delta_z;
 			}
 
 			if (vehicle_local_position.heading_reset_counter != _heading_reset_counter) {
@@ -401,7 +424,7 @@ void MulticopterPositionControl::Run()
 			if ((_setpoint.timestamp < _time_position_control_enabled)
 			    && (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) {
 
-				_setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states);
+				_setpoint = generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, false);
 			}
 		}
 
@@ -422,13 +445,13 @@ void MulticopterPositionControl::Run()
 				const bool want_takeoff = _vehicle_control_mode.flag_armed
 							  && (vehicle_local_position.timestamp_sample < _setpoint.timestamp + 1_s);
 
-				if (want_takeoff && PX4_ISFINITE(_setpoint.z)
-				    && (_setpoint.z < states.position(2))) {
+				if (want_takeoff && PX4_ISFINITE(_setpoint.position[2])
+				    && (_setpoint.position[2] < states.position(2))) {
 
 					_vehicle_constraints.want_takeoff = true;
 
-				} else if (want_takeoff && PX4_ISFINITE(_setpoint.vz)
-					   && (_setpoint.vz < 0.f)) {
+				} else if (want_takeoff && PX4_ISFINITE(_setpoint.velocity[2])
+					   && (_setpoint.velocity[2] < 0.f)) {
 
 					_vehicle_constraints.want_takeoff = true;
 
@@ -460,13 +483,13 @@ void MulticopterPositionControl::Run()
 			}
 
 			// make sure takeoff ramp is not amended by acceleration feed-forward
-			if (_takeoff.getTakeoffState() == TakeoffState::rampup) {
+			if (_takeoff.getTakeoffState() == TakeoffState::rampup && PX4_ISFINITE(_setpoint.velocity[2])) {
 				_setpoint.acceleration[2] = NAN;
 			}
 
 			if (not_taken_off || flying_but_ground_contact) {
 				// we are not flying yet and need to avoid any corrections
-				reset_setpoint_to_nan(_setpoint);
+				_setpoint = PositionControl::empty_trajectory_setpoint;
 				_setpoint.timestamp = vehicle_local_position.timestamp_sample;
 				Vector3f(0.f, 0.f, 100.f).copyTo(_setpoint.acceleration); // High downwards acceleration to make sure there's no thrust
 
@@ -497,15 +520,15 @@ void MulticopterPositionControl::Run()
 			_control.setInputSetpoint(_setpoint);
 
 			// update states
-			if (!PX4_ISFINITE(_setpoint.z)
-			    && PX4_ISFINITE(_setpoint.vz) && (fabsf(_setpoint.vz) > FLT_EPSILON)
+			if (!PX4_ISFINITE(_setpoint.position[2])
+			    && PX4_ISFINITE(_setpoint.velocity[2]) && (fabsf(_setpoint.velocity[2]) > FLT_EPSILON)
 			    && PX4_ISFINITE(vehicle_local_position.z_deriv) && vehicle_local_position.z_valid && vehicle_local_position.v_z_valid) {
 				// A change in velocity is demanded and the altitude is not controlled.
 				// Set velocity to the derivative of position
 				// because it has less bias but blend it in across the landing speed range
 				//  <  MPC_LAND_SPEED: ramp up using altitude derivative without a step
 				//  >= MPC_LAND_SPEED: use altitude derivative
-				float weighting = fminf(fabsf(_setpoint.vz) / _param_mpc_land_speed.get(), 1.f);
+				float weighting = fminf(fabsf(_setpoint.velocity[2]) / _param_mpc_land_speed.get(), 1.f);
 				states.velocity(2) = vehicle_local_position.z_deriv * weighting + vehicle_local_position.vz * (1.f - weighting);
 			}
 
@@ -516,7 +539,7 @@ void MulticopterPositionControl::Run()
 				// Failsafe
 				_vehicle_constraints = {0, NAN, NAN, false, {}}; // reset constraints
 
-				_control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states));
+				_control.setInputSetpoint(generateFailsafeSetpoint(vehicle_local_position.timestamp_sample, states, true));
 				_control.setVelocityLimits(_param_mpc_xy_vel_max.get(), _param_mpc_z_vel_max_up.get(), _param_mpc_z_vel_max_dn.get());
 				_control.update(dt);
 			}
@@ -556,24 +579,23 @@ void MulticopterPositionControl::Run()
 	perf_end(_cycle_perf);
 }
 
-vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const hrt_abstime &now,
-		const PositionControlStates &states)
+trajectory_setpoint_s MulticopterPositionControl::generateFailsafeSetpoint(const hrt_abstime &now,
+		const PositionControlStates &states, bool warn)
 {
-	// do not warn while we are disarmed, as we might not have valid setpoints yet
-	bool warn = _vehicle_control_mode.flag_armed && ((now - _last_warn) > 2_s);
+	// rate limit the warnings
+	warn = warn && (now - _last_warn) > 2_s;
 
 	if (warn) {
 		PX4_WARN("invalid setpoints");
 		_last_warn = now;
 	}
 
-	vehicle_local_position_setpoint_s failsafe_setpoint{};
-	reset_setpoint_to_nan(failsafe_setpoint);
+	trajectory_setpoint_s failsafe_setpoint = PositionControl::empty_trajectory_setpoint;
 	failsafe_setpoint.timestamp = now;
 
 	if (PX4_ISFINITE(states.velocity(0)) && PX4_ISFINITE(states.velocity(1))) {
 		// don't move along xy
-		failsafe_setpoint.vx = failsafe_setpoint.vy = 0.f;
+		failsafe_setpoint.velocity[0] = failsafe_setpoint.velocity[1] = 0.f;
 
 		if (warn) {
 			PX4_WARN("Failsafe: stop and wait");
@@ -582,7 +604,7 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe
 	} else {
 		// descend with land speed since we can't stop
 		failsafe_setpoint.acceleration[0] = failsafe_setpoint.acceleration[1] = 0.f;
-		failsafe_setpoint.vz = _param_mpc_land_speed.get();
+		failsafe_setpoint.velocity[2] = _param_mpc_land_speed.get();
 
 		if (warn) {
 			PX4_WARN("Failsafe: blind land");
@@ -591,13 +613,13 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe
 
 	if (PX4_ISFINITE(states.velocity(2))) {
 		// don't move along z if we can stop in all dimensions
-		if (!PX4_ISFINITE(failsafe_setpoint.vz)) {
-			failsafe_setpoint.vz = 0.f;
+		if (!PX4_ISFINITE(failsafe_setpoint.velocity[2])) {
+			failsafe_setpoint.velocity[2] = 0.f;
 		}
 
 	} else {
 		// emergency descend with a bit below hover thrust
-		failsafe_setpoint.vz = NAN;
+		failsafe_setpoint.velocity[2] = NAN;
 		failsafe_setpoint.acceleration[2] = .3f;
 
 		if (warn) {
@@ -608,16 +630,6 @@ vehicle_local_position_setpoint_s MulticopterPositionControl::generateFailsafeSe
 	return failsafe_setpoint;
 }
 
-void MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint)
-{
-	setpoint.timestamp = 0;
-	setpoint.x = setpoint.y = setpoint.z = NAN;
-	setpoint.yaw = setpoint.yawspeed = NAN;
-	setpoint.vx = setpoint.vy = setpoint.vz = NAN;
-	setpoint.acceleration[0] = setpoint.acceleration[1] = setpoint.acceleration[2] = NAN;
-	setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
-}
-
 int MulticopterPositionControl::task_spawn(int argc, char *argv[])
 {
 	bool vtol = false;
diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.hpp b/src/modules/mc_pos_control/MulticopterPositionControl.hpp
index 815d08e66f..97c4fd2169 100644
--- a/src/modules/mc_pos_control/MulticopterPositionControl.hpp
+++ b/src/modules/mc_pos_control/MulticopterPositionControl.hpp
@@ -109,8 +109,8 @@ private:
 	hrt_abstime _time_stamp_last_loop{0};		/**< time stamp of last loop iteration */
 	hrt_abstime _time_position_control_enabled{0};
 
-	vehicle_local_position_setpoint_s _setpoint {};
-	vehicle_control_mode_s _vehicle_control_mode {};
+	trajectory_setpoint_s _setpoint{PositionControl::empty_trajectory_setpoint};
+	vehicle_control_mode_s _vehicle_control_mode{};
 
 	vehicle_constraints_s _vehicle_constraints {
 		.timestamp = 0,
@@ -147,12 +147,14 @@ private:
 		(ParamBool<px4::params::MPC_USE_HTE>)       _param_mpc_use_hte,
 
 		// Takeoff / Land
-		(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
+		(ParamFloat<px4::params::COM_SPOOLUP_TIME>) _param_com_spoolup_time, /**< time to let motors spool up after arming */
 		(ParamFloat<px4::params::MPC_TKO_RAMP_T>)   _param_mpc_tko_ramp_t,   /**< time constant for smooth takeoff ramp */
 		(ParamFloat<px4::params::MPC_TKO_SPEED>)    _param_mpc_tko_speed,
 		(ParamFloat<px4::params::MPC_LAND_SPEED>)   _param_mpc_land_speed,
 
 		(ParamFloat<px4::params::MPC_VEL_MANUAL>)   _param_mpc_vel_manual,
+		(ParamFloat<px4::params::MPC_VEL_MAN_BACK>) _param_mpc_vel_man_back,
+		(ParamFloat<px4::params::MPC_VEL_MAN_SIDE>) _param_mpc_vel_man_side,
 		(ParamFloat<px4::params::MPC_XY_CRUISE>)    _param_mpc_xy_cruise,
 		(ParamFloat<px4::params::MPC_LAND_ALT2>)    _param_mpc_land_alt2,    /**< downwards speed limited below this altitude */
 		(ParamInt<px4::params::MPC_POS_MODE>)       _param_mpc_pos_mode,
@@ -221,10 +223,5 @@ private:
 	 * Used to handle transitions where no proper setpoint was generated yet and when the received setpoint is invalid.
 	 * This should only happen briefly when transitioning and never during mode operation or by design.
 	 */
-	vehicle_local_position_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states);
-
-	/**
-	 * Reset setpoints to NAN
-	 */
-	void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint);
+	trajectory_setpoint_s generateFailsafeSetpoint(const hrt_abstime &now, const PositionControlStates &states, bool warn);
 };
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
index 8dd2c521e7..5abbc2fbce 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp
@@ -44,6 +44,8 @@
 
 using namespace matrix;
 
+const trajectory_setpoint_s PositionControl::empty_trajectory_setpoint = {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN};
+
 void PositionControl::setVelocityGains(const Vector3f &P, const Vector3f &I, const Vector3f &D)
 {
 	_gain_vel_p = P;
@@ -93,10 +95,10 @@ void PositionControl::setState(const PositionControlStates &states)
 	_vel_dot = states.acceleration;
 }
 
-void PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint)
+void PositionControl::setInputSetpoint(const trajectory_setpoint_s &setpoint)
 {
-	_pos_sp = Vector3f(setpoint.x, setpoint.y, setpoint.z);
-	_vel_sp = Vector3f(setpoint.vx, setpoint.vy, setpoint.vz);
+	_pos_sp = Vector3f(setpoint.position);
+	_vel_sp = Vector3f(setpoint.velocity);
 	_acc_sp = Vector3f(setpoint.acceleration);
 	_yaw_sp = setpoint.yaw;
 	_yawspeed_sp = setpoint.yawspeed;
@@ -181,9 +183,16 @@ void PositionControl::_velocityControl(const float dt)
 
 	// Use tracking Anti-Windup for horizontal direction: during saturation, the integrator is used to unsaturate the output
 	// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
-	const Vector2f acc_sp_xy_limited = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);
+	const Vector2f acc_sp_xy_produced = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);
 	const float arw_gain = 2.f / _gain_vel_p(0);
-	vel_error.xy() = Vector2f(vel_error) - (arw_gain * (Vector2f(_acc_sp) - acc_sp_xy_limited));
+
+	// The produced acceleration can be greater or smaller than the desired acceleration due to the saturations and the actual vertical thrust (computed independently).
+	// The ARW loop needs to run if the signal is saturated only.
+	const Vector2f acc_sp_xy = _acc_sp.xy();
+	const Vector2f acc_limited_xy = (acc_sp_xy.norm_squared() > acc_sp_xy_produced.norm_squared())
+					? acc_sp_xy_produced
+					: acc_sp_xy;
+	vel_error.xy() = Vector2f(vel_error) - arw_gain * (acc_sp_xy - acc_limited_xy);
 
 	// Make sure integral doesn't get NAN
 	ControlMath::setZeroIfNanVector3f(vel_error);
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
index fa2fd35f80..124fd08b03 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControl.hpp
@@ -41,6 +41,7 @@
 
 #include <lib/mathlib/mathlib.h>
 #include <matrix/matrix/math.hpp>
+#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 
@@ -141,9 +142,9 @@ public:
 	/**
 	 * Pass the desired setpoints
 	 * Note: NAN value means no feed forward/leave state uncontrolled if there's no higher order setpoint.
-	 * @param setpoint a vehicle_local_position_setpoint_s structure
+	 * @param setpoint setpoints including feed-forwards to execute in update()
 	 */
-	void setInputSetpoint(const vehicle_local_position_setpoint_s &setpoint);
+	void setInputSetpoint(const trajectory_setpoint_s &setpoint);
 
 	/**
 	 * Apply P-position and PID-velocity controller that updates the member
@@ -178,6 +179,11 @@ public:
 	 */
 	void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
 
+	/**
+	 * All setpoints are set to NAN (uncontrolled). Timestampt zero.
+	 */
+	static const trajectory_setpoint_s empty_trajectory_setpoint;
+
 private:
 	bool _inputValid();
 
diff --git a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
index 89d22fe39b..215f36bf51 100644
--- a/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
+++ b/src/modules/mc_pos_control/PositionControl/PositionControlTest.cpp
@@ -81,22 +81,6 @@ public:
 		_position_control.setHorizontalThrustMargin(HORIZONTAL_THRUST_MARGIN);
 		_position_control.setTiltLimit(1.f);
 		_position_control.setHoverThrust(.5f);
-
-		resetInputSetpoint();
-	}
-
-	void resetInputSetpoint()
-	{
-		_input_setpoint.x = NAN;
-		_input_setpoint.y = NAN;
-		_input_setpoint.z = NAN;
-		_input_setpoint.yaw = NAN;
-		_input_setpoint.yawspeed = NAN;
-		_input_setpoint.vx = NAN;
-		_input_setpoint.vy = NAN;
-		_input_setpoint.vz = NAN;
-		Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.acceleration);
-		Vector3f(NAN, NAN, NAN).copyTo(_input_setpoint.thrust);
 	}
 
 	bool runController()
@@ -109,7 +93,7 @@ public:
 	}
 
 	PositionControl _position_control;
-	vehicle_local_position_setpoint_s _input_setpoint{};
+	trajectory_setpoint_s _input_setpoint{PositionControl::empty_trajectory_setpoint};
 	vehicle_local_position_setpoint_s _output_setpoint{};
 	vehicle_attitude_setpoint_s _attitude{};
 
@@ -136,27 +120,21 @@ public:
 
 TEST_F(PositionControlBasicDirectionTest, PositionDirection)
 {
-	_input_setpoint.x = .1f;
-	_input_setpoint.y = .1f;
-	_input_setpoint.z = -.1f;
+	Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.position);
 	EXPECT_TRUE(runController());
 	checkDirection();
 }
 
 TEST_F(PositionControlBasicDirectionTest, VelocityDirection)
 {
-	_input_setpoint.vx = .1f;
-	_input_setpoint.vy = .1f;
-	_input_setpoint.vz = -.1f;
+	Vector3f(.1f, .1f, -.1f).copyTo(_input_setpoint.velocity);
 	EXPECT_TRUE(runController());
 	checkDirection();
 }
 
 TEST_F(PositionControlBasicTest, TiltLimit)
 {
-	_input_setpoint.x = 10.f;
-	_input_setpoint.y = 10.f;
-	_input_setpoint.z = -0.f;
+	Vector3f(10.f, 10.f, 0.f).copyTo(_input_setpoint.position);
 
 	EXPECT_TRUE(runController());
 	Vector3f body_z = Quatf(_attitude.q_d).dcm_z();
@@ -176,9 +154,7 @@ TEST_F(PositionControlBasicTest, TiltLimit)
 
 TEST_F(PositionControlBasicTest, VelocityLimit)
 {
-	_input_setpoint.x = 10.f;
-	_input_setpoint.y = 10.f;
-	_input_setpoint.z = -10.f;
+	Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position);
 
 	EXPECT_TRUE(runController());
 	Vector2f velocity_xy(_output_setpoint.vx, _output_setpoint.vy);
@@ -189,9 +165,7 @@ TEST_F(PositionControlBasicTest, VelocityLimit)
 TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
 {
 	// Given a setpoint that drives the controller into vertical and horizontal saturation
-	_input_setpoint.x = 10.f;
-	_input_setpoint.y = 10.f;
-	_input_setpoint.z = -10.f;
+	Vector3f(10.f, 10.f, -10.f).copyTo(_input_setpoint.position);
 
 	// When you run it for one iteration
 	runController();
@@ -220,9 +194,7 @@ TEST_F(PositionControlBasicTest, PositionControlMaxThrustLimit)
 
 TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
 {
-	_input_setpoint.x = 10.f;
-	_input_setpoint.y = 0.f;
-	_input_setpoint.z = 10.f;
+	Vector3f(10.f, 0.f, 10.f).copyTo(_input_setpoint.position);
 
 	runController();
 	Vector3f thrust(_output_setpoint.thrust);
@@ -236,9 +208,8 @@ TEST_F(PositionControlBasicTest, PositionControlMinThrustLimit)
 
 TEST_F(PositionControlBasicTest, FailsafeInput)
 {
-	_input_setpoint.vz = .1f;
-	_input_setpoint.thrust[0] = _input_setpoint.thrust[1] = 0.f;
 	_input_setpoint.acceleration[0] = _input_setpoint.acceleration[1] = 0.f;
+	_input_setpoint.velocity[2] = .1f;
 
 	EXPECT_TRUE(runController());
 	EXPECT_FLOAT_EQ(_attitude.thrust_body[0], 0.f);
@@ -257,14 +228,12 @@ TEST_F(PositionControlBasicTest, IdleThrustInput)
 	EXPECT_TRUE(runController());
 	EXPECT_FLOAT_EQ(_output_setpoint.thrust[0], 0.f);
 	EXPECT_FLOAT_EQ(_output_setpoint.thrust[1], 0.f);
-	EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f);
+	EXPECT_FLOAT_EQ(_output_setpoint.thrust[2], -.1f); // minimum thrust
 }
 
 TEST_F(PositionControlBasicTest, InputCombinationsPosition)
 {
-	_input_setpoint.x = .1f;
-	_input_setpoint.y = .2f;
-	_input_setpoint.z = .3f;
+	Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position);
 
 	EXPECT_TRUE(runController());
 	EXPECT_FLOAT_EQ(_output_setpoint.x, .1f);
@@ -280,13 +249,13 @@ TEST_F(PositionControlBasicTest, InputCombinationsPosition)
 
 TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
 {
-	_input_setpoint.vx = .1f;
-	_input_setpoint.vy = .2f;
-	_input_setpoint.z = .3f; // altitude
+	_input_setpoint.velocity[0] = .1f;
+	_input_setpoint.velocity[1] = .2f;
+	_input_setpoint.position[2] = .3f; // altitude
 
 	EXPECT_TRUE(runController());
-	// EXPECT_TRUE(isnan(_output_setpoint.x));
-	// EXPECT_TRUE(isnan(_output_setpoint.y));
+	EXPECT_TRUE(isnan(_output_setpoint.x));
+	EXPECT_TRUE(isnan(_output_setpoint.y));
 	EXPECT_FLOAT_EQ(_output_setpoint.z, .3f);
 	EXPECT_FLOAT_EQ(_output_setpoint.vx, .1f);
 	EXPECT_FLOAT_EQ(_output_setpoint.vy, .2f);
@@ -299,9 +268,9 @@ TEST_F(PositionControlBasicTest, InputCombinationsPositionVelocity)
 TEST_F(PositionControlBasicTest, SetpointValiditySimple)
 {
 	EXPECT_FALSE(runController());
-	_input_setpoint.x = .1f;
+	_input_setpoint.position[0] = .1f;
 	EXPECT_FALSE(runController());
-	_input_setpoint.y = .2f;
+	_input_setpoint.position[1] = .2f;
 	EXPECT_FALSE(runController());
 	_input_setpoint.acceleration[2] = .3f;
 	EXPECT_TRUE(runController());
@@ -310,13 +279,13 @@ TEST_F(PositionControlBasicTest, SetpointValiditySimple)
 TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
 {
 	// This test runs any combination of set and unset (NAN) setpoints and checks if it gets accepted or rejected correctly
-	float *const setpoint_loop_access_map[] = {&_input_setpoint.x, &_input_setpoint.vx, &_input_setpoint.acceleration[0],
-						   &_input_setpoint.y, &_input_setpoint.vy, &_input_setpoint.acceleration[1],
-						   &_input_setpoint.z, &_input_setpoint.vz, &_input_setpoint.acceleration[2]
+	float *const setpoint_loop_access_map[] = {&_input_setpoint.position[0], &_input_setpoint.velocity[0], &_input_setpoint.acceleration[0],
+						   &_input_setpoint.position[1], &_input_setpoint.velocity[1], &_input_setpoint.acceleration[1],
+						   &_input_setpoint.position[2], &_input_setpoint.velocity[2], &_input_setpoint.acceleration[2]
 						  };
 
 	for (int combination = 0; combination < 512; combination++) {
-		resetInputSetpoint();
+		_input_setpoint = PositionControl::empty_trajectory_setpoint;
 
 		for (int j = 0; j < 9; j++) {
 			if (combination & (1 << j)) {
@@ -335,8 +304,10 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
 
 		EXPECT_EQ(runController(), expected_result) << "combination " << combination << std::endl
 				<< "input" << std::endl
-				<< "position     " << _input_setpoint.x << ", " << _input_setpoint.y << ", " << _input_setpoint.z << std::endl
-				<< "velocity     " << _input_setpoint.vx << ", " << _input_setpoint.vy << ", " << _input_setpoint.vz << std::endl
+				<< "position     " << _input_setpoint.position[0] << ", "
+				<< _input_setpoint.position[1] << ", " << _input_setpoint.position[2] << std::endl
+				<< "velocity     " << _input_setpoint.velocity[0] << ", "
+				<< _input_setpoint.velocity[1] << ", " << _input_setpoint.velocity[2] << std::endl
 				<< "acceleration " << _input_setpoint.acceleration[0] << ", "
 				<< _input_setpoint.acceleration[1] << ", " << _input_setpoint.acceleration[2] << std::endl
 				<< "output" << std::endl
@@ -349,9 +320,7 @@ TEST_F(PositionControlBasicTest, SetpointValidityAllCombinations)
 
 TEST_F(PositionControlBasicTest, InvalidState)
 {
-	_input_setpoint.x = .1f;
-	_input_setpoint.y = .2f;
-	_input_setpoint.z = .3f;
+	Vector3f(.1f, .2f, .3f).copyTo(_input_setpoint.position);
 
 	PositionControlStates states{};
 	states.position(0) = NAN;
@@ -379,9 +348,7 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust)
 	const float hover_thrust = 0.6f;
 	_position_control.setHoverThrust(hover_thrust);
 
-	_input_setpoint.vx = 0.f;
-	_input_setpoint.vy = 0.f;
-	_input_setpoint.vz = -0.f;
+	Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity);
 
 	// WHEN: we run the controller
 	EXPECT_TRUE(runController());
@@ -402,16 +369,14 @@ TEST_F(PositionControlBasicTest, UpdateHoverThrust)
 TEST_F(PositionControlBasicTest, IntegratorWindupWithInvalidSetpoint)
 {
 	// GIVEN: the controller was ran with an invalid setpoint containing some valid values
-	_input_setpoint.x = .1f;
-	_input_setpoint.y = .2f;
+	_input_setpoint.position[0] = .1f;
+	_input_setpoint.position[1] = .2f;
 	// all z-axis setpoints stay NAN
 	EXPECT_FALSE(runController());
 
 	// WHEN: we run the controller with a valid setpoint
-	resetInputSetpoint();
-	_input_setpoint.vx = 0.f;
-	_input_setpoint.vy = 0.f;
-	_input_setpoint.vz = 0.f;
+	_input_setpoint = PositionControl::empty_trajectory_setpoint;
+	Vector3f(0.f, 0.f, 0.f).copyTo(_input_setpoint.velocity);
 	EXPECT_TRUE(runController());
 
 	// THEN: the integral did not wind up and produce unexpected deviation
diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp
index 560b0f6ef8..c56befb3ff 100644
--- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp
+++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp
@@ -73,8 +73,7 @@ public:
 
 	/**
 	 * Update the state for the takeoff.
-	 * @param setpoint a vehicle_local_position_setpoint_s structure
-	 * @return true if setpoint has updated correctly
+	 * Has to be called also when not flying altitude controlled to skip the takeoff and not do it in flight when switching mode.
 	 */
 	void updateTakeoffState(const bool armed, const bool landed, const bool want_takeoff,
 				const float takeoff_desired_vz, const bool skip_takeoff, const hrt_abstime &now_us);
diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c
index ac7f99cd2c..fef1d25bc8 100644
--- a/src/modules/mc_pos_control/mc_pos_control_params.c
+++ b/src/modules/mc_pos_control/mc_pos_control_params.c
@@ -242,7 +242,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_UP, 3.f);
  * @decimal 1
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.f);
+PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.5f);
 
 /**
  * Maximum descent velocity
@@ -257,7 +257,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_V_AUTO_DN, 1.f);
  * @decimal 1
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.f);
+PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX_DN, 1.5f);
 
 /**
  * Proportional gain for horizontal position error
@@ -349,11 +349,14 @@ PARAM_DEFINE_FLOAT(MPC_XY_TRAJ_P, 0.5f);
 PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.0f);
 
 /**
- * Maximum horizontal velocity setpoint for manual controlled mode
+ * Maximum horizontal velocity setpoint in Position mode
  *
  * If velocity setpoint larger than MPC_XY_VEL_MAX is set, then
  * the setpoint will be capped to MPC_XY_VEL_MAX
  *
+ * The maximum sideways and backward speed can be set differently
+ * using MPC_VEL_MAN_SIDE and MPC_VEL_MAN_BACK, respectively.
+ *
  * @unit m/s
  * @min 3.0
  * @max 20.0
@@ -363,6 +366,36 @@ PARAM_DEFINE_FLOAT(MPC_XY_ERR_MAX, 2.0f);
  */
 PARAM_DEFINE_FLOAT(MPC_VEL_MANUAL, 10.0f);
 
+/**
+ * Maximum sideways velocity in Position mode
+ *
+ * If set to a negative value or larger than
+ * MPC_VEL_MANUAL then MPC_VEL_MANUAL is used.
+ *
+ * @unit m/s
+ * @min -1.0
+ * @max 20.0
+ * @increment 0.1
+ * @decimal 2
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_VEL_MAN_SIDE, -1.0f);
+
+/**
+ * Maximum backward velocity in Position mode
+ *
+ * If set to a negative value or larger than
+ * MPC_VEL_MANUAL then MPC_VEL_MANUAL is used.
+ *
+ * @unit m/s
+ * @min -1.0
+ * @max 20.0
+ * @increment 0.1
+ * @decimal 2
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_VEL_MAN_BACK, -1.0f);
+
 /**
  * Maximum horizontal velocity
  *
@@ -415,10 +448,12 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f);
 PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.7f);
 
 /**
- * Land crawl descend rate. Used below
+ * Land crawl descend rate
+ *
+ * Used below MPC_LAND_ALT3 if distance sensor data is availabe.
  *
  * @unit m/s
- * @min 0.3
+ * @min 0.1
  * @decimal 1
  * @group Multicopter Position Control
  */
@@ -436,6 +471,7 @@ PARAM_DEFINE_FLOAT(MPC_LAND_CRWL, 0.3f);
  * @max 1
  * @value 0 Fixed descent speed of MPC_LAND_SPEED
  * @value 1 User assisted descent speed
+ * @group Multicopter Position Control
  */
 PARAM_DEFINE_INT32(MPC_LAND_RC_HELP, 0);
 
@@ -780,22 +816,6 @@ PARAM_DEFINE_FLOAT(MPC_TKO_RAMP_T, 3.0f);
  */
 PARAM_DEFINE_INT32(MPC_POS_MODE, 4);
 
-/**
- * Enforced delay between arming and takeoff
- *
- * For altitude controlled modes the time from arming the motors until
- * a takeoff is possible gets forced to be at least MPC_SPOOLUP_TIME seconds
- * to ensure the motors and propellers can sppol up and reach idle speed before
- * getting commanded to spin faster. This delay is particularly useful for vehicles
- * with slow motor spin-up e.g. because of large propellers.
- *
- * @min 0
- * @max 10
- * @unit s
- * @group Multicopter Position Control
- */
-PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f);
-
 /**
  * Yaw mode.
  *
diff --git a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp
index 1ff41c4682..4f82489403 100644
--- a/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp
+++ b/src/modules/micrortps_bridge/micrortps_client/microRTPS_client_main.cpp
@@ -265,7 +265,7 @@ int micrortps_client_main(int argc, char *argv[])
 
 		_rtps_task = px4_task_spawn_cmd("micrortps_client",
 						SCHED_DEFAULT,
-						SCHED_PRIORITY_DEFAULT,
+						SCHED_PRIORITY_DEFAULT - 4,
 						PX4_STACK_ADJUSTED(2900),
 						(px4_main_t) micrortps_start,
 						(char *const *)argv);
diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt
index 5da49df860..f70a56a4c7 100644
--- a/src/modules/navigator/CMakeLists.txt
+++ b/src/modules/navigator/CMakeLists.txt
@@ -32,6 +32,7 @@
 ############################################################################
 
 add_subdirectory(GeofenceBreachAvoidance)
+add_subdirectory(TerrainFollower)
 
 px4_add_module(
 	MODULE modules__navigator
@@ -48,11 +49,12 @@ px4_add_module(
 		precland.cpp
 		mission_feasibility_checker.cpp
 		geofence.cpp
-		follow_target.cpp
+		terrain_follower_wrapper.cpp
 		vtol_takeoff.cpp
+		vtol_land.cpp
 	DEPENDS
 		geo
-		landing_slope
 		geofence_breach_avoidance
 		motion_planning
+		terrain_follower
 	)
diff --git a/src/modules/navigator/TerrainFollower/CMakeLists.txt b/src/modules/navigator/TerrainFollower/CMakeLists.txt
new file mode 100644
index 0000000000..e6900f76e8
--- /dev/null
+++ b/src/modules/navigator/TerrainFollower/CMakeLists.txt
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2020 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_library(terrain_follower
+	terrain_follower.cpp
+	terrain_follower.hpp
+)
+
+#add_dependencies(terrain_follower terrain)
+target_link_libraries(terrain_follower PRIVATE terrain)
+
+px4_add_functional_gtest(SRC TerrainFollowerTest.cpp LINKLIBS terrain_follower terrain geo modules__navigator)
diff --git a/src/modules/navigator/TerrainFollower/TerrainFollowerTest.cpp b/src/modules/navigator/TerrainFollower/TerrainFollowerTest.cpp
new file mode 100644
index 0000000000..2b3ec6ba3e
--- /dev/null
+++ b/src/modules/navigator/TerrainFollower/TerrainFollowerTest.cpp
@@ -0,0 +1,231 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 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 <navigator/terrain_follower_wrapper.hpp>
+
+using namespace matrix;
+using Vector2d = matrix::Vector2<double>;
+
+class FakeTerrainProvider : public terrain::TerrainProvider
+{
+public:
+	FakeTerrainProvider() :
+		TerrainProvider(200)
+	{
+
+	}
+
+	virtual ~FakeTerrainProvider() {};
+
+	bool lookup(double lat, double lon, float &alt_msl_m) override
+	{
+		alt_msl_m = _terrain_value_to_return;
+		return true;
+	}
+
+	void setTerrainValueToReturn(float val) { _terrain_value_to_return = val; }
+
+private:
+	float _terrain_value_to_return{0.0f};
+
+};
+
+class TerrainFollowerStateSetter
+{
+public:
+	TerrainFollowerStateSetter(TerrainFollowerWrapper &terrain_follower):
+		_terrain_follower(terrain_follower)
+	{
+
+		_terrain_follower.setHomeAltitude(_home_alt_amsl_m);
+		_ref.initReference(_home_global(0), _home_global(1));
+	}
+
+	~TerrainFollowerStateSetter() {};
+
+	matrix::Vector3<double> setCurrentPosNEU(const Vector3f &pos_NEU)
+	{
+		double lat, lon;
+		_ref.reproject(pos_NEU(0), pos_NEU(1), lat, lon);
+		_terrain_follower.setCurrentPosition(Vector2d(lat, lon), pos_NEU(2) + _home_alt_amsl_m);
+		return matrix::Vector3<double> (lat, lon, pos_NEU(2) + _home_alt_amsl_m);
+	}
+
+	mission_item_s setTargetMissionItem(Vector3f pos_NEU)
+	{
+		double lat, lon;
+		_ref.reproject(pos_NEU(0), pos_NEU(1), lat, lon);
+		Vector2d target_pos_lat_lon(lat, lon);
+
+		mission_item_s target_mission_item = {};
+		target_mission_item.lat = target_pos_lat_lon(0);
+		target_mission_item.lon = target_pos_lat_lon(1);
+
+		target_mission_item.altitude = pos_NEU(2); // relative to home
+		target_mission_item.altitude_is_relative = true;
+
+		_terrain_follower.setCurrentMissionItem(target_mission_item);
+
+		return target_mission_item;
+	}
+
+	float getHomeAlt() { return _home_alt_amsl_m; }
+
+	const Vector2d &getHomePosition() { return _home_global; }
+
+private:
+
+	TerrainFollowerWrapper &_terrain_follower;
+
+	Vector2d _home_global = Vector2d(42.1, 8.2);
+	float _home_alt_amsl_m = 200.0;
+
+	MapProjection _ref;
+
+};
+
+TEST(TerrainFollowerTest, FixedWing)
+{
+	FakeTerrainProvider _fake_terrain;
+	TerrainFollowerWrapper terrain_follower;
+	terrain_follower.enable();
+	terrain_follower.setTerrainProvider(&_fake_terrain);
+	TerrainFollowerStateSetter state_setter(terrain_follower);
+
+	const float mission_alt_rel_m = 20.0f;
+	const float ground_speed = 20.0f;
+	bool is_hovercraft = false;
+	const float loiter_radius = 60.0f;
+
+	terrain_follower.setGroundSpeed(ground_speed);
+
+	terrain_follower.setIsHoverCraft(is_hovercraft);
+
+	terrain_follower.setLoiterRadius(loiter_radius);
+
+	Vector3f current_pos_local_NEU(0, 0, mission_alt_rel_m);
+	state_setter.setCurrentPosNEU(current_pos_local_NEU);
+
+	// set target position 2km ahead to the north, at 20m above home
+	Vector3f target_pos_local_NEU(2000, 0, mission_alt_rel_m);
+	state_setter.setTargetMissionItem(target_pos_local_NEU);
+
+	_fake_terrain.setTerrainValueToReturn(state_setter.getHomeAlt() + 100.0f); // this violates terrain
+
+	// there is terrain violation, so should return true
+	bool ret = terrain_follower.updateIntermediateMissionItem(false);
+	ASSERT_TRUE(ret == true);
+
+	mission_item_s intermediate_mission_item = terrain_follower.getIntermediateMissionItem();
+
+	// expect altitude to be 200m above home (100m clearance + 100m terrain + 50m to be within band between min dist and max dist)
+	ASSERT_EQ(intermediate_mission_item.altitude, state_setter.getHomeAlt() + 100.0f + 100.0f + 50.0f);
+
+	// position should be equal to our current position which is the home position
+	ASSERT_EQ(intermediate_mission_item.lat, state_setter.getHomePosition()(0));
+	ASSERT_EQ(intermediate_mission_item.lon, state_setter.getHomePosition()(1));
+}
+
+TEST(TerrainFollowerTest, test)
+{
+	FakeTerrainProvider _fake_terrain;
+	TerrainFollowerWrapper terrain_follower;
+	terrain_follower.enable();
+	terrain_follower.setTerrainProvider(&_fake_terrain);
+	TerrainFollowerStateSetter state_setter(terrain_follower);
+
+
+	Vector3f current_pos_local_NEU(0, 0, 20.0f);
+	state_setter.setCurrentPosNEU(current_pos_local_NEU);
+
+	// not target item set, should return false
+	bool ret = terrain_follower.updateIntermediateMissionItem(false);
+	ASSERT_TRUE(ret == false);
+
+
+	// set target position 2km ahead to the north, at 20m above home
+	Vector3f target_pos_local_NEU(2000, 0, 20.0f);
+	state_setter.setTargetMissionItem(target_pos_local_NEU);
+
+	_fake_terrain.setTerrainValueToReturn(state_setter.getHomeAlt() +
+					      10.0f); // this only gives a 10m alt clearance, min is 100m
+
+	// expect to return true, as we are violating terrain
+	ret = terrain_follower.updateIntermediateMissionItem(false);
+
+	ASSERT_TRUE(ret = true);
+
+	mission_item_s intermediate_mission_item = terrain_follower.getIntermediateMissionItem();
+
+	// expect intermediate mission item with altitude that does not violate (100m min dist, 10m terrain, 50m to be within band between min and max dist)
+	ASSERT_EQ(intermediate_mission_item.altitude, state_setter.getHomeAlt() + 100.0f + 10.0f + 50.0f);
+
+	// expect the terrain follower to tell use that we have an intermediate item
+	ASSERT_TRUE(terrain_follower.hasIntermediateMissionItem());
+
+	// we are now reaching the intermediate waypoint
+	Vector3f new_pos = current_pos_local_NEU + Vector3f(100.0f, 0, 0);
+	state_setter.setCurrentPosNEU(new_pos);
+
+	// we expect this to return true as we need to insert more intermediate waypoint in order not to violate terrain
+	ASSERT_TRUE(terrain_follower.updateIntermediateMissionItem(true));
+
+	intermediate_mission_item = terrain_follower.getIntermediateMissionItem();
+
+	// expect intermediate mission item with altitude that does not violate (100m min dist, 10m terrain, 50m to be within band between min and max dist)
+	ASSERT_EQ(intermediate_mission_item.altitude, state_setter.getHomeAlt() + 100.0f + 10.0f + 50.0f);
+
+	// change the terrain so that no more intermediate waypoint needs to be inserted
+	_fake_terrain.setTerrainValueToReturn(state_setter.getHomeAlt() - 100.0f); // this give plenty of clearance
+
+	// intermediate mission item has been reached and terrain does not violate. this still returns true since we need to reload the original mission item
+	ASSERT_TRUE(terrain_follower.updateIntermediateMissionItem(true));
+
+	// this indicates that there are no more intermediate mission items and thus the mission items from the original mission should be reloaded
+	ASSERT_FALSE(terrain_follower.hasIntermediateMissionItem());
+
+	// we are withing check_distance of the original target mission item and are currently not tracking any intermediate items
+	new_pos(0) = 1950.0f;
+	state_setter.setCurrentPosNEU(new_pos);
+
+	// we are currently not tracking an intermediate waypoint and have come close to the target mission item, don't need to update
+	ASSERT_FALSE(terrain_follower.updateIntermediateMissionItem(false));
+
+	// original mission item still valid
+	ASSERT_TRUE(terrain_follower.isCurrentMissionItemValid());
+
+	// we have reached the target item, now the mission item is invalidated
+	ASSERT_FALSE(terrain_follower.updateIntermediateMissionItem(true));
+	ASSERT_FALSE(terrain_follower.isCurrentMissionItemValid());
+}
diff --git a/src/modules/navigator/TerrainFollower/terrain_follower.cpp b/src/modules/navigator/TerrainFollower/terrain_follower.cpp
new file mode 100644
index 0000000000..01befd6833
--- /dev/null
+++ b/src/modules/navigator/TerrainFollower/terrain_follower.cpp
@@ -0,0 +1,271 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 "terrain_follower.hpp"
+#include <lib/geo/geo.h>
+
+using Vector2d = matrix::Vector2<double>;
+using Vector2f = matrix::Vector2<float>;
+
+TerrainFollower::TerrainFollower() :
+	ModuleParams(nullptr)
+{
+	param_handle_fw_climbrate_max = param_find("FW_T_CLMB_MAX");
+}
+
+TerrainFollower::~TerrainFollower()
+{
+
+}
+
+void TerrainFollower::updateParams()
+{
+	ModuleParams::updateParams();
+	param_get(param_handle_fw_climbrate_max, &param_fw_climbrate_max);
+}
+
+void TerrainFollower::reset()
+{
+	_current_mission_item_valid = false;
+	_has_intermediate_mission_item = false;
+	_dist_to_current_mission_item = 0.0f;
+	_pos_lat_lon.setZero();
+	_alt_amsl_m = 0.0f;
+	_alt_home_amsl_m = 0.0f;
+	_ground_speed_m_s = 0.0f;
+	_had_first_mission_item = false;
+}
+
+void TerrainFollower::setCurrentMissionItem(const mission_item_s &current_mission_item)
+{
+
+	_dist_to_current_mission_item = get_distance_to_next_waypoint(_pos_lat_lon(0), _pos_lat_lon(1),
+					current_mission_item.lat, current_mission_item.lon);
+
+	// if mission item is too close, then don't set it
+	if (_dist_to_current_mission_item > _param_check_dist.get()) {
+		if (_had_first_mission_item) {
+			_previous_mission_item = _current_mission_item;
+			_previous_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+
+		} else {
+			_previous_mission_item = current_mission_item;
+			_previous_mission_item.lat = _pos_lat_lon(0);
+			_previous_mission_item.lon = _pos_lat_lon(1);
+			_previous_mission_item.altitude = _alt_amsl_m;
+			_previous_mission_item.altitude_is_relative = false;
+		}
+
+		_had_first_mission_item = true;
+		_current_mission_item = current_mission_item;
+		_current_mission_item_valid = true;
+		_has_intermediate_mission_item = false;
+	}
+}
+
+Vector2d TerrainFollower::getClosestPointOnMissionPath()
+{
+	float x, y;
+	get_vector_to_next_waypoint(_previous_mission_item.lat, _previous_mission_item.lon, _current_mission_item.lat,
+				    _current_mission_item.lon, &x, &y);
+	Vector2f u_prev_to_target = Vector2f(x, y).unit_or_zero();
+	get_vector_to_next_waypoint(_previous_mission_item.lat, _previous_mission_item.lon, _pos_lat_lon(0), _pos_lat_lon(1),
+				    &x, &y);
+	Vector2f prev_to_pos(x, y);
+
+	const Vector2f prev_to_closest(u_prev_to_target * (prev_to_pos * u_prev_to_target));
+
+	double lat, lon;
+	create_waypoint_from_line_and_dist(_previous_mission_item.lat, _previous_mission_item.lon, _current_mission_item.lat,
+					   _current_mission_item.lon, prev_to_closest.length(), &lat, &lon);
+
+	return Vector2d(lat, lon);
+
+}
+
+bool TerrainFollower::updateIntermediateMissionItem(bool mission_item_reached)
+{
+	if (!_current_mission_item_valid) {
+		return false;
+	}
+
+	bool should_reload_mission_items = false;
+
+	bool target_item_reached = false;
+
+	if (mission_item_reached) {
+
+		if (_has_intermediate_mission_item) {
+			// we reached an intermediate and need to re-load mission items
+			_has_intermediate_mission_item = false;
+			should_reload_mission_items = true;
+
+		} else {
+			// this was the target, don't reload the misson items but advance the mission
+			target_item_reached = true;
+			should_reload_mission_items = false;
+			_current_mission_item_valid = false;
+		}
+	}
+
+	if (!target_item_reached) {
+		_dist_to_current_mission_item = get_distance_to_next_waypoint(_pos_lat_lon(0), _pos_lat_lon(1),
+						_current_mission_item.lat, _current_mission_item.lon);
+
+		if (_dist_to_current_mission_item > math::max(_param_check_dist.get(), 1.5f * _loiter_radius)) {
+			Vector2d check_point_lat_lon = calculateCheckpoint();
+
+			float new_altitude = 0.0f;
+			const bool need_update = needAltitudeUpdate(check_point_lat_lon, new_altitude);
+
+			if (need_update) {
+				if (!_is_hovercraft) {
+					// check if the climb is too steep for a fixed wing
+					const float alt_gradient_max = param_fw_climbrate_max / math::max(_ground_speed_m_s, 0.1f);
+
+					const float alt_grad = getAltitudeGradient(_pos_lat_lon, check_point_lat_lon, _alt_amsl_m,
+							       new_altitude);
+
+					// if the altitude change is too steep, then loiter to that altitude at the closest point of the planned mission path
+					if (alt_grad > alt_gradient_max) {
+						Vector2d closest_point_on_mission_path = getClosestPointOnMissionPath();
+						check_point_lat_lon(0) = closest_point_on_mission_path(0);
+						check_point_lat_lon(1) = closest_point_on_mission_path(1);
+					}
+				}
+
+				// by default we use the current mission item the vehicle is supposed to go to
+				_intermediate_mission_item = _current_mission_item;
+				_intermediate_mission_item.lat = check_point_lat_lon(0);
+				_intermediate_mission_item.lon = check_point_lat_lon(1);
+				_intermediate_mission_item.altitude = new_altitude;
+				_intermediate_mission_item.altitude_is_relative = false;
+				_intermediate_mission_item.autocontinue = true;
+				_intermediate_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+				_intermediate_mission_item.loiter_radius = _loiter_radius;
+
+				should_reload_mission_items = true;
+				_has_intermediate_mission_item = true;
+
+			}
+		}
+	}
+
+	return should_reload_mission_items;
+}
+
+bool TerrainFollower::needAltitudeUpdate(const Vector2d &check_point_lat_lon, float &new_altitude)
+{
+	float dist_to_terrain_at_checkpoint = 0;
+	const bool terrain_valid = getDistanceToTerrainAtLocation(check_point_lat_lon,
+				   dist_to_terrain_at_checkpoint);
+
+	const bool min_alt_violation = _param_terrain_dist_min.get() > 0
+				       && (dist_to_terrain_at_checkpoint < _param_terrain_dist_min.get());
+	const bool max_alt_violation = _param_terrain_dist_max.get() > 0
+				       && (dist_to_terrain_at_checkpoint > _param_terrain_dist_max.get());
+
+	if (terrain_valid && (min_alt_violation || max_alt_violation)) {
+
+		float alt_rel_target;
+
+		if (_param_terrain_dist_min.get() > 0 && _param_terrain_dist_max.get() > 0) {
+			alt_rel_target = 0.5f * (_param_terrain_dist_min.get() + _param_terrain_dist_max.get());
+
+		} else if (min_alt_violation) {
+			alt_rel_target = _param_terrain_dist_min.get();
+
+		} else {
+			alt_rel_target = _param_terrain_dist_max.get();
+		}
+
+		const float alt_at_checkpoint_amsl = getAltitudeAtCheckPoint(check_point_lat_lon);
+		new_altitude = alt_at_checkpoint_amsl + alt_rel_target - dist_to_terrain_at_checkpoint;
+		return true;
+	}
+
+	return false;
+}
+
+Vector2d TerrainFollower::calculateCheckpoint()
+{
+	double check_point_lat, check_point_lon;
+
+	const float dist_prev_current = get_distance_to_next_waypoint(_previous_mission_item.lat, _previous_mission_item.lon,
+					_current_mission_item.lat, _current_mission_item.lon);
+
+	const float look_ahead_dist = math::min(_dist_to_current_mission_item, _param_check_dist.get());
+
+	create_waypoint_from_line_and_dist(_previous_mission_item.lat, _previous_mission_item.lon, _current_mission_item.lat,
+					   _current_mission_item.lon, math::max(dist_prev_current - _dist_to_current_mission_item + look_ahead_dist, 1.0f),
+					   &check_point_lat, &check_point_lon);
+
+	return Vector2d(check_point_lat, check_point_lon);
+}
+
+float TerrainFollower::getAltitudeGradient(const matrix::Vector2<double> &A, const matrix::Vector2<double> &B,
+		const float alt_A,
+		const float altB)
+{
+	const float dist_between_waypoints = get_distance_to_next_waypoint(A(0), A(1), B(0), B(1));
+
+	return (altB - alt_A) / math::max(dist_between_waypoints, 0.1f);
+}
+
+float TerrainFollower::getAltitudeAtCheckPoint(const matrix::Vector2<double> &check_point_lat_lon)
+{
+	const float dist_to_checkpoint_m = get_distance_to_next_waypoint(_pos_lat_lon(0), _pos_lat_lon(1),
+					   check_point_lat_lon(0), check_point_lat_lon(1));
+	const float mission_item_alt_amsl_m = _current_mission_item.altitude_is_relative ? _current_mission_item.altitude +
+					      _alt_home_amsl_m : _current_mission_item.altitude;
+
+	const float alt_gradient = getAltitudeGradient(_pos_lat_lon, Vector2d(_current_mission_item.lat,
+				   _current_mission_item.lon), _alt_amsl_m, mission_item_alt_amsl_m);
+
+	return _alt_amsl_m + alt_gradient * dist_to_checkpoint_m;
+}
+
+bool TerrainFollower::getDistanceToTerrainAtLocation(const matrix::Vector2<double> &check_point_lat_lon,
+		float &dist_to_terrain)
+{
+	bool ret = false;
+	float terrain_alt_amsl_m;
+
+	if (_terrain_provider->lookup(check_point_lat_lon(0), check_point_lat_lon(1), terrain_alt_amsl_m)) {
+		dist_to_terrain = getAltitudeAtCheckPoint(check_point_lat_lon) - terrain_alt_amsl_m;
+		ret = true;
+	}
+
+	return ret;
+}
diff --git a/src/modules/navigator/TerrainFollower/terrain_follower.hpp b/src/modules/navigator/TerrainFollower/terrain_follower.hpp
new file mode 100644
index 0000000000..ffac7cd077
--- /dev/null
+++ b/src/modules/navigator/TerrainFollower/terrain_follower.hpp
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 <lib/mathlib/mathlib.h>
+#include <px4_platform_common/module_params.h>
+#include "../navigation.h"
+#include <px4_platform_common/defines.h>
+#include <lib/terrain/terrain_provider.h>
+
+class TerrainFollower : public ModuleParams
+{
+public:
+	TerrainFollower();
+
+	~TerrainFollower();
+
+	void setCurrentMissionItem(const mission_item_s &current_mission_item);
+
+	void setCurrentPosition(const matrix::Vector2<double> &pos_lat_lon, float alt_amsl) { _pos_lat_lon = pos_lat_lon; _alt_amsl_m = alt_amsl; }
+
+	void setHomeAltitude(float home_alt_m) { _alt_home_amsl_m = home_alt_m; }
+
+	void setTerrainProvider(terrain::TerrainProvider *terrain_provider) { _terrain_provider = terrain_provider; }
+
+	mission_item_s getIntermediateMissionItem() { return _intermediate_mission_item; }
+
+	bool updateIntermediateMissionItem(bool mission_item_reached);
+
+	bool hasIntermediateMissionItem() { return _has_intermediate_mission_item; }
+
+	mission_item_s getCurrentMissionItem() { return _current_mission_item; }
+
+	bool isCurrentMissionItemValid() { return _current_mission_item_valid; }
+
+	void reset();
+
+	void setGroundSpeed(float ground_speed_m_s) { _ground_speed_m_s = ground_speed_m_s; }
+
+	void setIsHoverCraft(bool is_hovercraft) { _is_hovercraft = is_hovercraft; }
+
+	void setLoiterRadius(float radius) { _loiter_radius = radius; }
+
+	void updateParams() override;
+
+
+private:
+
+	matrix::Vector2<double> _pos_lat_lon = {};	// current position of the vehicle
+	float _alt_amsl_m{0.0f};
+	float _alt_home_amsl_m{0.0f};
+	float _ground_speed_m_s{0.0f};
+
+	float _loiter_radius{0.0f};
+
+	mission_item_s _current_mission_item = {}; // the current mission item from the mission plan
+	struct mission_item_s _intermediate_mission_item = {};
+	struct mission_item_s _previous_mission_item = {};
+
+	bool _current_mission_item_valid = false;	// we have a target mission item from the navigator set
+	bool _has_intermediate_mission_item =
+		false;	// we are generating intermediate waypoints in between our location and _current_mission_item
+	bool _had_first_mission_item = false;
+
+	bool _is_hovercraft = true;
+
+	float _dist_to_current_mission_item{0.0f};	// distance from the vehicle to _current_mission_item
+
+	terrain::TerrainProvider *_terrain_provider = nullptr;
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::TF_CHECK_DIST>) _param_check_dist,
+		(ParamFloat<px4::params::TF_TERR_DIST_MIN>) _param_terrain_dist_min,
+		(ParamFloat<px4::params::TF_TERR_DIST_MAX>) _param_terrain_dist_max
+	)
+
+	param_t param_handle_fw_climbrate_max;
+	float param_fw_climbrate_max{0.0f};
+
+	bool getDistanceToTerrainAtLocation(const matrix::Vector2<double> &check_point_lat_lon, float &dist_to_terrain);
+
+	float getAltitudeAtCheckPoint(const matrix::Vector2<double> &check_point_lat_lon);
+
+	float getAltitudeGradient(const matrix::Vector2<double> &A, const matrix::Vector2<double> &B, const float alt_A,
+				  const float altB);
+
+	matrix::Vector2<double> getClosestPointOnMissionPath();
+
+	matrix::Vector2<double> calculateCheckpoint();
+
+	bool needAltitudeUpdate(const matrix::Vector2<double> &check_point_lat_lon, float &new_altitude);
+};
diff --git a/src/modules/navigator/TerrainFollower/terrain_follower_params.c b/src/modules/navigator/TerrainFollower/terrain_follower_params.c
new file mode 100644
index 0000000000..bf8aeb854b
--- /dev/null
+++ b/src/modules/navigator/TerrainFollower/terrain_follower_params.c
@@ -0,0 +1,96 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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.
+ *
+ ****************************************************************************/
+
+/**
+ * Enable Terrain Data upload
+ *
+ * If enabled, configures the terrain data grid spacing.
+ * The recommended value is 100m, or 50m on boards with enough RAM.
+ * Whenever the mission, home position or safe points change,
+ * terrain data is requested within a rectangular area that includes all of
+ * these points.
+ *
+ * This will then be used during RTL.
+ *
+ * Note:
+ * - this feature requres several KB of RAM.
+ * - latitudes beyond +-80 degrees are not supported.
+ *
+ * @value 0 Disabled
+ * @value 50 50m grid spacing
+ * @value 100 100m grid spacing
+ * @value 200 200m grid spacing
+ * @value 400 400m grid spacing
+ * @reboot_required true
+ * @group Terrain Follower
+ */
+PARAM_DEFINE_INT32(TF_TERRAIN_EN, 0);
+
+/**
+ * Terrain follower check distance.
+ *
+ * Look ahead distance the terrain follower will use to detect terrain violation.
+ *
+ * @unit m
+ * @min 10.0
+ * @max 1000.0
+ * @decimal 2
+ * @increment 1.0
+ * @group Terrain Follower
+ */
+PARAM_DEFINE_FLOAT(TF_CHECK_DIST, 100);
+
+
+/**
+ * Terrain follower minimum distance to terrain.
+ *
+ *
+ * @unit m
+ * @min 10.0
+ * @decimal 2
+ * @increment 1.0
+ * @group Terrain Follower
+ */
+PARAM_DEFINE_FLOAT(TF_TERR_DIST_MIN, 100);
+
+/**
+ * Terrain follower maximum distance to terrain.
+ *
+ *
+ * @unit m
+ * @min 10.0
+ * @decimal 2
+ * @increment 1.0
+ * @group Terrain Follower
+ */
+PARAM_DEFINE_FLOAT(TF_TERR_DIST_MAX, 200);
\ No newline at end of file
diff --git a/src/modules/navigator/follow_target.cpp b/src/modules/navigator/follow_target.cpp
deleted file mode 100644
index 56e9a35ec8..0000000000
--- a/src/modules/navigator/follow_target.cpp
+++ /dev/null
@@ -1,407 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013-2016 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 followme.cpp
- *
- * Helper class to track and follow a given position
- *
- * @author Jimmy Johnson <catch22@fastmail.net>
- */
-
-#include "follow_target.h"
-
-#include <string.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <math.h>
-#include <fcntl.h>
-
-#include <systemlib/err.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/follow_target.h>
-#include <lib/geo/geo.h>
-#include <lib/mathlib/math/Limits.hpp>
-
-#include "navigator.h"
-
-using namespace matrix;
-
-constexpr float FollowTarget::_follow_position_matricies[4][9];
-
-FollowTarget::FollowTarget(Navigator *navigator) :
-	MissionBlock(navigator),
-	ModuleParams(navigator)
-{
-	_current_vel.zero();
-	_step_vel.zero();
-	_est_target_vel.zero();
-	_target_distance.zero();
-	_target_position_offset.zero();
-	_target_position_delta.zero();
-}
-
-void FollowTarget::on_inactive()
-{
-	reset_target_validity();
-}
-
-void FollowTarget::on_activation()
-{
-	_follow_offset = _param_nav_ft_dst.get() < 1.0F ? 1.0F : _param_nav_ft_dst.get();
-
-	_responsiveness = math::constrain((float) _param_nav_ft_rs.get(), .1F, 1.0F);
-
-	_follow_target_position = _param_nav_ft_fs.get();
-
-	if ((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) {
-		_follow_target_position = FOLLOW_FROM_BEHIND;
-	}
-
-	_rot_matrix = Dcmf(_follow_position_matricies[_follow_target_position]);
-}
-
-void FollowTarget::on_active()
-{
-	MapProjection target_ref;
-	follow_target_s target_motion_with_offset = {};
-	uint64_t current_time = hrt_absolute_time();
-	bool radius_entered = false;
-	bool radius_exited = false;
-	bool updated = false;
-	float dt_ms = 0;
-
-	if (_follow_target_sub.updated()) {
-		updated = true;
-		follow_target_s target_motion;
-
-		_target_updates++;
-
-		// save last known motion topic for interpolation later
-
-		_previous_target_motion = _current_target_motion;
-
-		_follow_target_sub.copy(&target_motion);
-
-		if (_current_target_motion.timestamp == 0) {
-			_current_target_motion = target_motion;
-		}
-
-		_current_target_motion = target_motion;
-		_current_target_motion.lat = (_current_target_motion.lat * (double)_responsiveness) + target_motion.lat * (double)(
-						     1 - _responsiveness);
-		_current_target_motion.lon = (_current_target_motion.lon * (double)_responsiveness) + target_motion.lon * (double)(
-						     1 - _responsiveness);
-
-	} else if (((current_time - _current_target_motion.timestamp) / 1000) > TARGET_TIMEOUT_MS && target_velocity_valid()) {
-		reset_target_validity();
-	}
-
-	// update distance to target
-
-	if (target_position_valid()) {
-
-		// get distance to target
-
-		target_ref.initReference(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
-		target_ref.project(_current_target_motion.lat, _current_target_motion.lon,
-				   _target_distance(0), _target_distance(1));
-
-	}
-
-	// update target velocity
-
-	if (target_velocity_valid() && updated) {
-
-		dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000);
-
-		// ignore a small dt
-		if (dt_ms > 10.0F) {
-			// get last gps known reference for target
-			target_ref.initReference(_previous_target_motion.lat, _previous_target_motion.lon);
-
-			// calculate distance the target has moved
-			target_ref.project(_current_target_motion.lat, _current_target_motion.lon,
-					   (_target_position_delta(0)), (_target_position_delta(1)));
-
-			// update the average velocity of the target based on the position
-			if (PX4_ISFINITE(_current_target_motion.vx) && PX4_ISFINITE(_current_target_motion.vy)) {
-				// No need to estimate target velocity if we can take it from the target
-				_est_target_vel(0) = _current_target_motion.vx;
-				_est_target_vel(1) = _current_target_motion.vy;
-				_est_target_vel(2) = 0.0f;
-
-			} else {
-				_est_target_vel = _target_position_delta / (dt_ms / 1000.0f);
-			}
-
-			// if the target is moving add an offset and rotation
-			if (_est_target_vel.length() > .5F) {
-				_target_position_offset = _rot_matrix * _est_target_vel.normalized() * _follow_offset;
-			}
-
-			// are we within the target acceptance radius?
-			// give a buffer to exit/enter the radius to give the velocity controller
-			// a chance to catch up
-
-			radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
-			radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M);
-
-			// to keep the velocity increase/decrease smooth
-			// calculate how many velocity increments/decrements
-			// it will take to reach the targets velocity
-			// with the given amount of steps also add a feed forward input that adjusts the
-			// velocity as the position gap increases since
-			// just traveling at the exact velocity of the target will not
-			// get any closer or farther from the target
-
-			_step_vel = (_est_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K;
-			_step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS);
-			_step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS);
-
-			// if we are less than 1 meter from the target don't worry about trying to yaw
-			// lock the yaw until we are at a distance that makes sense
-
-			if ((_target_distance).length() > 1.0F) {
-
-				// yaw rate smoothing
-
-				// this really needs to control the yaw rate directly in the attitude pid controller
-				// but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode
-
-				_yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
-						_navigator->get_global_position()->lon,
-						_current_target_motion.lat,
-						_current_target_motion.lon);
-
-				_yaw_rate = wrap_pi((_yaw_angle - _navigator->get_local_position()->heading) / (dt_ms / 1000.0f));
-
-			} else {
-				_yaw_angle = _yaw_rate = NAN;
-			}
-		}
-
-//		warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d yaw rate = %3.6f",
-//				(double) _step_vel(0),
-//				(double) _step_vel(1),
-//				(double) _current_vel(0),
-//				(double) _current_vel(1),
-//				(double) _est_target_vel(0),
-//				(double) _est_target_vel(1),
-//				(double) (_target_distance).length(),
-//				(double) (_target_position_offset + _target_distance).length(),
-//				_follow_target_state,
-//				(double) _yaw_rate);
-	}
-
-	if (target_position_valid()) {
-
-		// get the target position using the calculated offset
-
-		target_ref.initReference(_current_target_motion.lat, _current_target_motion.lon);
-		target_ref.reproject(_target_position_offset(0), _target_position_offset(1),
-				     target_motion_with_offset.lat, target_motion_with_offset.lon);
-	}
-
-	// clamp yaw rate smoothing if we are with in
-	// 3 degrees of facing target
-
-	if (PX4_ISFINITE(_yaw_rate)) {
-		if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_local_position()->heading)) < math::radians(3.0F)) {
-			_yaw_rate = NAN;
-		}
-	}
-
-	// update state machine
-
-	switch (_follow_target_state) {
-
-	case TRACK_POSITION: {
-
-			if (radius_entered) {
-				_follow_target_state = TRACK_VELOCITY;
-
-			} else if (target_velocity_valid()) {
-				set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target_motion_with_offset, _yaw_angle);
-				// keep the current velocity updated with the target velocity for when it's needed
-				_current_vel = _est_target_vel;
-
-				update_position_sp(true, true, _yaw_rate);
-
-			} else {
-				_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
-			}
-
-			break;
-		}
-
-	case TRACK_VELOCITY: {
-
-			if (radius_exited) {
-				_follow_target_state = TRACK_POSITION;
-
-			} else if (target_velocity_valid()) {
-
-				if ((float)(current_time - _last_update_time) / 1000.0f >= _step_time_in_ms) {
-					_current_vel += _step_vel;
-					_last_update_time = current_time;
-				}
-
-				set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target_motion_with_offset, _yaw_angle);
-
-				update_position_sp(true, false, _yaw_rate);
-
-			} else {
-				_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
-			}
-
-			break;
-		}
-
-	case SET_WAIT_FOR_TARGET_POSITION: {
-
-			// Climb to the minimum altitude
-			// and wait until a position is received
-
-			follow_target_s target = {};
-
-			// for now set the target at the minimum height above the uav
-
-			target.lat = _navigator->get_global_position()->lat;
-			target.lon = _navigator->get_global_position()->lon;
-			target.alt = 0.0F;
-
-			set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target, _yaw_angle);
-
-			update_position_sp(false, false, _yaw_rate);
-
-			_follow_target_state = WAIT_FOR_TARGET_POSITION;
-		}
-
-	/* FALLTHROUGH */
-
-	case WAIT_FOR_TARGET_POSITION: {
-
-			if (is_mission_item_reached() && target_velocity_valid()) {
-				_target_position_offset(0) = _follow_offset;
-				_follow_target_state = TRACK_POSITION;
-			}
-
-			break;
-		}
-	}
-}
-
-void FollowTarget::update_position_sp(bool use_velocity, bool use_position, float yaw_rate)
-{
-	// convert mission item to current setpoint
-
-	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
-	// activate line following in pos control if position is valid
-
-	pos_sp_triplet->previous.valid = use_position;
-	pos_sp_triplet->previous = pos_sp_triplet->current;
-	mission_apply_limitation(_mission_item);
-	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
-	pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
-	pos_sp_triplet->current.velocity_valid = use_velocity;
-	pos_sp_triplet->current.vx = _current_vel(0);
-	pos_sp_triplet->current.vy = _current_vel(1);
-	pos_sp_triplet->next.valid = false;
-	pos_sp_triplet->current.yawspeed_valid = PX4_ISFINITE(yaw_rate);
-	pos_sp_triplet->current.yawspeed = yaw_rate;
-	_navigator->set_position_setpoint_triplet_updated();
-}
-
-void FollowTarget::reset_target_validity()
-{
-	_yaw_rate = NAN;
-	_previous_target_motion = {};
-	_current_target_motion = {};
-	_target_updates = 0;
-	_current_vel.zero();
-	_step_vel.zero();
-	_est_target_vel.zero();
-	_target_distance.zero();
-	_target_position_offset.zero();
-	reset_mission_item_reached();
-	_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
-}
-
-bool FollowTarget::target_velocity_valid()
-{
-	// need at least 2 continuous data points for velocity estimate
-	return (_target_updates >= 2);
-}
-
-bool FollowTarget::target_position_valid()
-{
-	// need at least 1 continuous data points for position estimate
-	return (_target_updates >= 1);
-}
-
-void
-FollowTarget::set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target,
-				     float yaw)
-{
-	if (_navigator->get_land_detected()->landed) {
-		/* landed, don't takeoff, but switch to IDLE mode */
-		item->nav_cmd = NAV_CMD_IDLE;
-
-	} else {
-
-		item->nav_cmd = NAV_CMD_DO_FOLLOW_REPOSITION;
-
-		/* use current target position */
-		item->lat = target.lat;
-		item->lon = target.lon;
-		item->altitude = _navigator->get_home_position()->alt;
-
-		if (min_clearance > 8.0f) {
-			item->altitude += min_clearance;
-
-		} else {
-			item->altitude += 8.0f; // if min clearance is bad set it to 8.0 meters (well above the average height of a person)
-		}
-	}
-
-	item->altitude_is_relative = false;
-	item->yaw = yaw;
-	item->loiter_radius = _navigator->get_loiter_radius();
-	item->acceptance_radius = _navigator->get_acceptance_radius();
-	item->time_inside = 0.0f;
-	item->autocontinue = false;
-	item->origin = ORIGIN_ONBOARD;
-}
diff --git a/src/modules/navigator/follow_target.h b/src/modules/navigator/follow_target.h
deleted file mode 100644
index 25086dca8e..0000000000
--- a/src/modules/navigator/follow_target.h
+++ /dev/null
@@ -1,148 +0,0 @@
-/***************************************************************************
- *
- *   Copyright (c) 2016 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 followme.cpp
- *
- * Helper class to track and follow a given position
- *
- * @author Jimmy Johnson <catch22@fastmail.net>
- */
-
-#pragma once
-
-#include "navigator_mode.h"
-#include "mission_block.h"
-
-#include <mathlib/mathlib.h>
-#include <matrix/math.hpp>
-
-#include <px4_platform_common/module_params.h>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/follow_target.h>
-
-class FollowTarget : public MissionBlock, public ModuleParams
-{
-
-public:
-	FollowTarget(Navigator *navigator);
-	~FollowTarget() = default;
-
-	void on_inactive() override;
-	void on_activation() override;
-	void on_active() override;
-
-private:
-
-	static constexpr int TARGET_TIMEOUT_MS = 2500;
-	static constexpr int TARGET_ACCEPTANCE_RADIUS_M = 5;
-	static constexpr int INTERPOLATION_PNTS = 20;
-	static constexpr float FF_K = .25F;
-	static constexpr float OFFSET_M = 8;
-
-	enum FollowTargetState {
-		TRACK_POSITION,
-		TRACK_VELOCITY,
-		SET_WAIT_FOR_TARGET_POSITION,
-		WAIT_FOR_TARGET_POSITION
-	};
-
-	enum {
-		FOLLOW_FROM_RIGHT,
-		FOLLOW_FROM_BEHIND,
-		FOLLOW_FROM_FRONT,
-		FOLLOW_FROM_LEFT
-	};
-
-	static constexpr float _follow_position_matricies[4][9] = {
-		{ 1.0F, -1.0F, 0.0F,  1.0F,  1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow right
-		{-1.0F,  0.0F, 0.0F,  0.0F, -1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow behind
-		{ 1.0F,  0.0F, 0.0F,  0.0F,  1.0F, 0.0F, 0.0F, 0.0F, 1.0F}, // follow front
-		{ 1.0F,  1.0F, 0.0F, -1.0F,  1.0F, 0.0F, 0.0F, 0.0F, 1.0F}  // follow left side
-	};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::NAV_MIN_FT_HT>) _param_nav_min_ft_ht,
-		(ParamFloat<px4::params::NAV_FT_DST>) _param_nav_ft_dst,
-		(ParamInt<px4::params::NAV_FT_FS>) _param_nav_ft_fs,
-		(ParamFloat<px4::params::NAV_FT_RS>) _param_nav_ft_rs
-	)
-
-	FollowTargetState _follow_target_state{SET_WAIT_FOR_TARGET_POSITION};
-	int _follow_target_position{FOLLOW_FROM_BEHIND};
-
-	uORB::Subscription _follow_target_sub{ORB_ID(follow_target)};
-	float _step_time_in_ms{0.0f};
-	float _follow_offset{OFFSET_M};
-
-	uint64_t _target_updates{0};
-	uint64_t _last_update_time{0};
-
-	matrix::Vector3f _current_vel;
-	matrix::Vector3f _step_vel;
-	matrix::Vector3f _est_target_vel;
-	matrix::Vector3f _target_distance;
-	matrix::Vector3f _target_position_offset;
-	matrix::Vector3f _target_position_delta;
-	matrix::Vector3f _filtered_target_position_delta;
-
-	follow_target_s _current_target_motion{};
-	follow_target_s _previous_target_motion{};
-
-	float _yaw_rate{0.0f};
-	float _responsiveness{0.0f};
-	float _yaw_angle{0.0f};
-
-	// Mavlink defined motion reporting capabilities
-	enum {
-		POS = 0,
-		VEL = 1,
-		ACCEL = 2,
-		ATT_RATES = 3
-	};
-
-	matrix::Dcmf _rot_matrix;
-
-	void track_target_position();
-	void track_target_velocity();
-	bool target_velocity_valid();
-	bool target_position_valid();
-	void reset_target_validity();
-	void update_position_sp(bool velocity_valid, bool position_valid, float yaw_rate);
-	void update_target_motion();
-	void update_target_velocity();
-
-	/**
-	 * Set follow_target item
-	 */
-	void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
-};
diff --git a/src/modules/navigator/land.cpp b/src/modules/navigator/land.cpp
index 38fba5136a..92f9cee2fe 100644
--- a/src/modules/navigator/land.cpp
+++ b/src/modules/navigator/land.cpp
@@ -65,6 +65,10 @@ Land::on_activation()
 	_navigator->set_can_loiter_at_sp(false);
 
 	_navigator->set_position_setpoint_triplet_updated();
+
+	_navigator->acquire_gimbal_control();
+	_navigator->set_gimbal_neutral();
+	_navigator->release_gimbal_control();
 }
 
 void
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 3f8951e7c9..0ef4288089 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -63,9 +63,10 @@
 
 using namespace time_literals;
 
-Mission::Mission(Navigator *navigator) :
+Mission::Mission(Navigator *navigator, TerrainFollowerWrapper &terrain_follower) :
 	MissionBlock(navigator),
-	ModuleParams(navigator)
+	ModuleParams(navigator),
+	_terrain_follower(terrain_follower)
 {
 	mission_init();
 }
@@ -165,14 +166,10 @@ Mission::on_inactive()
 void
 Mission::on_inactivation()
 {
-	// Disable camera trigger
-	vehicle_command_s cmd {};
-	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
-	// Pause trigger
-	cmd.param1 = -1.0f;
-	cmd.param3 = 1.0f;
-	_navigator->publish_vehicle_cmd(&cmd);
+	_navigator->disable_camera_trigger();
+	_navigator->stop_capturing_images();
 
+	_navigator->set_gimbal_neutral();
 	_navigator->release_gimbal_control();
 
 	if (_navigator->get_precland()->is_activated()) {
@@ -183,6 +180,8 @@ Mission::on_inactivation()
 
 	/* reset so current mission item gets restarted if mission was paused */
 	_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+
+	_inactivation_index = _current_mission_index;
 }
 
 void
@@ -197,18 +196,39 @@ Mission::on_activation()
 		_mission_waypoints_changed = false;
 	}
 
+	if (_mission_changed) {
+		_navigator->reset_stored_cruising_speed();
+
+	} else {
+		_navigator->restore_cruising_speed();
+	}
+
 	// we already reset the mission items
 	_execution_mode_changed = false;
 
-	set_mission_items();
+	_terrain_follower.reset();
+	_navigator->setTerrainFollowerState();
 
-	// unpause triggering if it was paused
-	vehicle_command_s cmd = {};
-	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
-	// unpause trigger
-	cmd.param1 = -1.0f;
-	cmd.param3 = 0.0f;
-	_navigator->publish_vehicle_cmd(&cmd);
+	if (_current_mission_index > 0 && _current_mission_index != _inactivation_index) {
+		reset_command_cache();
+		updateChachedCommandsUpToIndex(_current_mission_index - 1);
+	}
+
+	unsigned resume_index;
+
+	if (cameraWasTriggering()
+	    && getPreviousPositionItemIndex(_mission, _current_mission_index - 1, resume_index)) {
+		// the mission we are resuming had camera triggering enabled. In order to not lose any images
+		// we restart the mission at the previous position item.
+		// we will replay the cached commands once we reach the previous position item
+		set_current_mission_index(resume_index);
+
+	} else {
+		set_mission_items();
+	}
+
+	_replay_cached_gimbal_commands_at_next_waypoint = haveCachedCommands();
+	_inactivation_index = -1;
 }
 
 void
@@ -216,8 +236,8 @@ Mission::on_active()
 {
 	check_mission_valid(false);
 
-	/* check if anything has changed */
-	bool mission_sub_updated = _mission_sub.updated();
+	/* Check if stored mission plan has changed */
+	const bool mission_sub_updated = _mission_sub.updated();
 
 	if (mission_sub_updated) {
 		_navigator->reset_triplets();
@@ -241,11 +261,60 @@ Mission::on_active()
 		}
 
 		_execution_mode_changed = false;
+		_terrain_follower.reset();
+		_navigator->setTerrainFollowerState();
 		set_mission_items();
 	}
 
-	/* lets check if we reached the current mission item */
-	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
+	bool reload_mission_items = false;
+	const bool mission_item_reached = is_mission_item_reached_or_completed();
+
+	if (mission_item_reached && _replay_cached_gimbal_commands_at_next_waypoint
+	    && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
+
+		replay_cached_gimbal_commands();
+		_replay_cached_gimbal_commands_at_next_waypoint = false;
+
+		mission_item_s next_mission_item = {};
+
+		if (getNextPositionMissionItem(_mission, _current_mission_index + 1, next_mission_item)
+		    && !PX4_ISFINITE(_mission_item.yaw)) {
+			// we are at the waypoint from which we want to resume the mission, first make sure that we are facing the next waypoint if there is one
+			_mission_item.yaw = matrix::wrap_pi(get_bearing_to_next_waypoint(_mission_item.lat, _mission_item.lon,
+							    next_mission_item.lat, next_mission_item.lon));
+			_mission_item.force_heading = true;
+
+			// replay camera commands at the next waypoint to ensure the gimbal and vehicle are aligned for shooting
+			_replay_cached_camera_commands_at_next_waypoint = cameraWasTriggering();
+
+		}
+
+		mission_apply_limitation(_mission_item);
+		mission_item_to_position_setpoint(_mission_item, &_navigator->get_position_setpoint_triplet()->current);
+
+		reset_mission_item_reached();
+
+		_navigator->set_position_setpoint_triplet_updated();
+
+		return;
+	}
+
+	if (_replay_cached_camera_commands_at_next_waypoint && mission_item_reached) {
+		replay_cached_camera_commands();
+		_replay_cached_camera_commands_at_next_waypoint = false;
+	}
+
+	_navigator->setTerrainFollowerState();
+
+	if (mission_item_reached || (hrt_absolute_time() - _time_last_terrain_checked > 1000000
+				     && !_terrain_follower.hasIntermediateMissionItem())) {
+
+		reload_mission_items = _terrain_follower.updateIntermediateMissionItem(mission_item_reached);
+		_time_last_terrain_checked = hrt_absolute_time();
+	}
+
+	/* lets check if we reached the current mission item or if the terrain follower wants to insert an intermediate item*/
+	if (_mission_type != MISSION_TYPE_NONE && (mission_item_reached || reload_mission_items))  {
 		/* If we just completed a takeoff which was inserted before the right waypoint,
 		   there is no need to report that we reached it because we didn't. */
 		if (_work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
@@ -254,7 +323,11 @@ Mission::on_active()
 
 		if (_mission_item.autocontinue) {
 			/* switch to next waypoint if 'autocontinue' flag set */
-			advance_mission();
+
+			if (!reload_mission_items) {
+				advance_mission();
+			}
+
 			set_mission_items();
 		}
 
@@ -314,6 +387,10 @@ Mission::set_current_mission_index(uint16_t index)
 
 		_current_mission_index = index;
 
+		if (_current_mission_index == 0) {
+			reset_command_cache();
+		}
+
 		// a mission index is set manually which has the higher priority than the closest mission item
 		// as it is set by the user
 		_mission_waypoints_changed = false;
@@ -324,6 +401,9 @@ Mission::set_current_mission_index(uint16_t index)
 			_navigator->get_position_setpoint_triplet()->previous.valid = false;
 			_navigator->get_position_setpoint_triplet()->current.valid = false;
 			_navigator->get_position_setpoint_triplet()->next.valid = false;
+
+			_terrain_follower.reset();
+			_navigator->setTerrainFollowerState();
 			set_mission_items();
 		}
 
@@ -364,6 +444,7 @@ Mission::set_execution_mode(const uint8_t mode)
 					pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 					publish_navigator_mission_item(); // for logging
 					_navigator->set_position_setpoint_triplet_updated();
+					cache_command(_mission_item);
 					issue_command(_mission_item);
 				}
 
@@ -389,6 +470,7 @@ Mission::set_execution_mode(const uint8_t mode)
 				// handle switch from reverse to forward mission
 				if (_current_mission_index < 0) {
 					_current_mission_index = 0;
+					reset_command_cache();
 
 				} else if (_current_mission_index < _mission.count - 1) {
 					++_current_mission_index;
@@ -442,6 +524,9 @@ Mission::find_mission_land_start()
 			_landing_start_lon = missionitem.lon;
 			_landing_start_alt = missionitem.altitude_is_relative ?	missionitem.altitude +
 					     _navigator->get_home_position()->alt : missionitem.altitude;
+			_landing_loiter_radius = (PX4_ISFINITE(missionitem.loiter_radius)
+						  && fabsf(missionitem.loiter_radius) > FLT_EPSILON) ? fabsf(missionitem.loiter_radius) :
+						 _navigator->get_loiter_radius();
 			_land_start_available = true;
 		}
 
@@ -504,72 +589,101 @@ void
 Mission::update_mission()
 {
 
-	bool failed = true;
+	bool failed = false;
 
 	/* Reset vehicle_roi
 	 * Missions that do not explicitly configure ROI would not override
 	 * an existing ROI setting from previous missions */
 	_navigator->reset_vroi();
 
-	const mission_s old_mission = _mission;
+	_old_mission = _mission;
+	_old_mission.current_seq = _current_mission_index;
 
 	if (_mission_sub.copy(&_mission)) {
-		/* determine current index */
-		if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) {
-			_current_mission_index = _mission.current_seq;
+
+		/* If _mission.clear_mission flag is set to true, mission is deleted from dataman.
+		 * No mission validation needed. */
+		if (_mission.clear_mission) {
+			_mission.count = 0;
+			_mission.current_seq = 0;
+			_current_mission_index = 0;
+			_mission.valid = false;
 
 		} else {
-			/* if less items available, reset to first item */
-			if (_current_mission_index >= (int)_mission.count) {
-				_current_mission_index = 0;
 
-			} else if (_current_mission_index < 0) {
-				/* if not initialized, set it to 0 */
-				_current_mission_index = 0;
+			/* determine current index */
+			if (_mission.current_seq >= 0 && _mission.current_seq < (int)_mission.count) {
+				_current_mission_index = _mission.current_seq;
+
+			} else {
+				/* if less items available, reset to first item */
+				if (_current_mission_index >= (int)_mission.count) {
+					_current_mission_index = 0;
+
+				} else if (_current_mission_index < 0) {
+					/* if not initialized, set it to 0 */
+					_current_mission_index = 0;
+				}
+
+				/* otherwise, just leave it */
 			}
 
-			/* otherwise, just leave it */
-		}
+			check_mission_valid(true);
 
-		check_mission_valid(true);
+			failed = !_navigator->get_mission_result()->valid;
 
-		failed = !_navigator->get_mission_result()->valid;
+			if (!failed) {
+				/* reset mission failure if we have an updated valid mission */
+				_navigator->get_mission_result()->failure = false;
 
-		if (!failed) {
-			/* reset mission failure if we have an updated valid mission */
-			_navigator->get_mission_result()->failure = false;
+				/* reset sequence info as well */
+				_navigator->get_mission_result()->seq_reached = -1;
+				_navigator->get_mission_result()->seq_total = _mission.count;
 
-			/* reset sequence info as well */
-			_navigator->get_mission_result()->seq_reached = -1;
-			_navigator->get_mission_result()->seq_total = _mission.count;
+				/* reset work item if new mission has been accepted */
+				_work_item_type = WORK_ITEM_TYPE_DEFAULT;
+				_mission_changed = true;
+			}
 
-			/* reset work item if new mission has been accepted */
-			_work_item_type = WORK_ITEM_TYPE_DEFAULT;
-			_mission_changed = true;
-		}
+			/* check if the mission waypoints changed while the vehicle is in air
+			 * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */
+			if (((_mission.count != _old_mission.count) ||
+			     (_mission.dataman_id != _old_mission.dataman_id)) &&
+			    !_navigator->get_land_detected()->landed) {
+				_mission_waypoints_changed = true;
+			}
 
-		/* check if the mission waypoints changed while the vehicle is in air
-		 * TODO add a flag to mission_s which actually tracks if the position of the waypoint changed */
-		if (((_mission.count != old_mission.count) ||
-		     (_mission.dataman_id != old_mission.dataman_id)) &&
-		    !_navigator->get_land_detected()->landed) {
-			_mission_waypoints_changed = true;
 		}
 
 	} else {
 		PX4_ERR("mission update failed");
 	}
 
-	if (failed) {
+	if (failed && (_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+		// if in-air and the mission upload fails, restore the old mission
+
 		// only warn if the check failed on merit
 		if ((int)_mission.count > 0) {
 			PX4_WARN("mission check failed");
 		}
 
-		// reset the mission
-		_mission.count = 0;
-		_mission.current_seq = 0;
-		_current_mission_index = 0;
+		bool restored = restore_old_mission();
+
+		if (!restored) {
+
+			// Mission can't be restored, reset the mission
+			_mission.count = 0;
+			_mission.current_seq = 0;
+			_current_mission_index = 0;
+
+		} else {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: previous mission is uploaded.");
+			events::send(events::ID("mission_restore_old"), events::Log::Error, "Mission rejected: previous mission is uploaded");
+		}
+	}
+
+	if (_current_mission_index == 0) {
+		reset_command_cache();
 	}
 
 	// find and store landing start marker (if available)
@@ -578,6 +692,68 @@ Mission::update_mission()
 	set_current_mission_item();
 }
 
+bool
+Mission::restore_old_mission()
+{
+	bool success = false;
+
+	_mission = _old_mission;
+
+	if ((_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED) &&
+	    _mission.valid) {
+
+		/* if armed and valid don't check old mission validity again but update navigator */
+		_navigator->get_mission_result()->valid = true;
+		_navigator->get_mission_result()->seq_total = _mission.count;
+		_navigator->increment_mission_instance_count();
+		_navigator->set_mission_result_updated();
+		_home_inited = _navigator->home_global_position_valid();
+		find_mission_land_start();
+
+	} else {
+		/* if not armed check validity again in case we changed feasibility parameters */
+		check_mission_valid(true);
+	}
+
+	if (_mission.valid) {
+		_mission_changed = true;
+		_navigator->get_mission_result()->failure = false;
+
+		/* For completed old mission reset sequence to the first item. */
+		if (_old_mission.current_seq >= _old_mission.count) {
+			_current_mission_index = 0;
+
+		} else {
+			_current_mission_index = _old_mission.current_seq;
+		}
+
+		/* lock MISSION_STATE item */
+		int dm_lock_ret = dm_lock(DM_KEY_MISSION_STATE);
+
+		if (dm_lock_ret != 0) {
+			PX4_ERR("DM_KEY_MISSION_STATE lock failed");
+		}
+
+		/* Since mission is invalid restore DM_KEY_MISSION_STATE with old dataman_id.
+		 * This helps mavlink_mission module to not overwrite the valid mission if two or more invalid missions are sent.
+		 */
+		int res =  dm_write(DM_KEY_MISSION_STATE, 0, &_mission, sizeof(mission_s));
+
+		/* unlock MISSION_STATE item */
+		if (dm_lock_ret == 0) {
+			dm_unlock(DM_KEY_MISSION_STATE);
+		}
+
+		if (res == sizeof(mission_s)) {
+			success = true;
+
+		} else {
+			PX4_ERR("Can't write DM_KEY_MISSION_STATE. The next invalid mission will not be restored!");
+		}
+	}
+
+	return success;
+}
 
 void
 Mission::advance_mission()
@@ -647,8 +823,16 @@ Mission::set_mission_items()
 
 	work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
 
-	if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item,
-				  &mission_item_after_next_position, &has_after_next_position_item)) {
+	// get an intermediate mission item from the terrain follower if required
+	if (_terrain_follower.hasIntermediateMissionItem()) {
+		_mission_item = _terrain_follower.getIntermediateMissionItem();
+		mission_item_next_position = _terrain_follower.getCurrentMissionItem();
+		has_next_position_item = true;
+
+		// otherwise read the next item from the mission storage
+
+	} else if (prepare_mission_items(&_mission_item, &mission_item_next_position, &has_next_position_item,
+					 &mission_item_after_next_position, &has_after_next_position_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_MISSION) {
 			mavlink_log_info(_navigator->get_mavlink_log_pub(),
@@ -667,8 +851,12 @@ Mission::set_mission_items()
 
 		_mission_type = MISSION_TYPE_MISSION;
 
+		// give the terrain follower the current mission item if it contains a position
+		if (item_contains_position(_mission_item) && !_terrain_follower.isCurrentMissionItemValid()) {
+			_terrain_follower.setCurrentMissionItem(_mission_item);
+		}
+
 	} else {
-		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
 
 			if (_navigator->get_land_detected()->landed) {
@@ -820,15 +1008,10 @@ Mission::set_mission_items()
 				} else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
 					   && _work_item_type == WORK_ITEM_TYPE_DEFAULT
 					   && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
+					// if the vehicle is already in fixed wing mode then the current mission item
+					// will be accepted immediately and the work items will be skipped
+					_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
 
-					if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-						/* haven't transitioned yet, trigger vtol takeoff logic below */
-						_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
-
-					} else {
-						/* already in fixed-wing, go to waypoint */
-						_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-					}
 
 					/* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */
 					_mission_item.yaw = NAN;
@@ -889,17 +1072,6 @@ Mission::set_mission_items()
 					pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
 				}
 
-				/* takeoff completed and transitioned, move to takeoff wp as fixed wing */
-				if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF
-				    && _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF
-				    && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) {
-
-					new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
-					_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-					_mission_item.autocontinue = true;
-					_mission_item.time_inside = 0.0f;
-				}
-
 				/* move to land wp as fixed wing */
 				if (_mission_item.nav_cmd == NAV_CMD_VTOL_LAND
 				    && (_work_item_type == WORK_ITEM_TYPE_DEFAULT || _work_item_type == WORK_ITEM_TYPE_TRANSITON_AFTER_TAKEOFF)
@@ -1062,6 +1234,21 @@ Mission::set_mission_items()
 		case mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD: {
 				position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
+				/* If a custom action exists, set it and make it available to the navigator
+				   main routine */
+				if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT_USER_1
+				    && !_navigator->get_in_custom_action()
+				    && _current_mission_index != _previous_custom_action_mission_index) {
+					CustomAction custom_action{};
+					custom_action.id = _mission_item.params[0];
+					custom_action.timeout = _mission_item.params[2] * 1000000;
+					custom_action.failure_action = static_cast<CustomAction::FailureAction>(_mission_item.params[3]);
+
+					_navigator->set_custom_action(custom_action);
+
+					PX4_INFO("Processing custom action #%d", custom_action.id);
+				}
+
 				/* turn towards next waypoint before MC to FW transition */
 				if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION
 				    && _work_item_type == WORK_ITEM_TYPE_DEFAULT
@@ -1119,9 +1306,6 @@ Mission::set_mission_items()
 	}
 
 	/*********************************** set setpoints and check next *********************************************/
-
-	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
-
 	// The logic in this section establishes the tracking between the current waypoint
 	// which we are approaching and the next waypoint, which will tell us in which direction
 	// we will change our trajectory right after reaching it.
@@ -1130,8 +1314,9 @@ Mission::set_mission_items()
 	// we are searching around the current mission item in the list to find the closest
 	// gate and the closest waypoint. We then store them separately.
 
-	// Check if the mission item is a gate
-	// along the current trajectory
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	// Check if the mission item is a gate along the current trajectory
 	if (item_contains_gate(_mission_item)) {
 
 		// The mission item is a gate, let's check if the next item in the list provides
@@ -1151,6 +1336,7 @@ Mission::set_mission_items()
 		// TODO Precision land needs to be refactored: https://github.com/PX4/Firmware/issues/14320
 		if (new_work_item_type != WORK_ITEM_TYPE_PRECISION_LAND) {
 			mission_apply_limitation(_mission_item);
+
 			mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
 		}
 
@@ -1165,7 +1351,27 @@ Mission::set_mission_items()
 	}
 
 	/* issue command if ready (will do nothing for position mission items) */
-	issue_command(_mission_item);
+	if (_mission_item.nav_cmd != NAV_CMD_WAYPOINT_USER_1) {
+		cache_command(_mission_item);
+		issue_command(_mission_item);
+	}
+
+	/* In the case of NAV_CMD_WAYPOINT_USER_1 mission items (for custom actions)
+	 * it will not send the same MAV_CMD_WAYPOINT_USER_1 command if for some
+	 * reason we left Mission mode and returned back to it. This avoids that it
+	 * gets sent at least twice after getting back to Mission mode and before
+	 * advancing to the next waypoint.
+	 * (e.g. if a custom action command changes the FMU to another mode, this
+	 * will avoid that the same  gets sent again when the FMU changes back to
+	 * Mission mode).
+	 */
+	if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT_USER_1
+	    && _current_mission_index != _previous_custom_action_mission_index) {
+		cache_command(_mission_item);
+		issue_command(_mission_item);
+	}
+
+	_previous_custom_action_mission_index = _current_mission_index;
 
 	/* set current work item type */
 	_work_item_type = new_work_item_type;
@@ -1184,10 +1390,8 @@ Mission::set_mission_items()
 		set_current_mission_item();
 	}
 
-	// If the mission item under evaluation contains a gate,
-	// then we need to check if we have a next position item so
-	// the controller can fly the correct line between the
-	// current and next setpoint
+	// If the mission item under evaluation contains a gate, we need to check if we have a next position item so
+	// the controller can fly the correct line between the current and next setpoint
 	if (item_contains_gate(_mission_item)) {
 		if (has_after_next_position_item) {
 			/* got next mission item, update setpoint triplet */
@@ -1199,9 +1403,13 @@ Mission::set_mission_items()
 		}
 
 	} else {
-		/* allow the vehicle to decelerate before reaching a wp with a hold time */
+		// Allow a rotary wing vehicle to decelerate before reaching a wp with a hold time or a timeout
+		// This is done by setting the position triplet's next position's valid flag to false,
+		// which makes the FlightTask disregard the next position
+		// TODO: Setting the next waypoint's validity flag to handle braking / correct waypoint behavior
+		// seems hacky, handle this more properly.
 		const bool brake_for_hold = _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
-					    && get_time_inside(_mission_item) > FLT_EPSILON;
+					    && (get_time_inside(_mission_item) > FLT_EPSILON || item_has_timeout(_mission_item));
 
 		if (_mission_item.autocontinue && !brake_for_hold) {
 			/* try to process next mission item */
@@ -1220,7 +1428,6 @@ Mission::set_mission_items()
 		}
 	}
 
-	publish_navigator_mission_item(); // for logging
 	_navigator->set_position_setpoint_triplet_updated();
 }
 
@@ -1745,9 +1952,9 @@ Mission::check_mission_valid(bool force)
 		_navigator->get_mission_result()->valid =
 			_missionFeasibilityChecker.checkMissionFeasible(_mission,
 					_param_mis_dist_1wp.get(),
-					_param_mis_dist_wps.get(),
-					_navigator->mission_landing_required());
+					_param_mis_dist_wps.get());
 
+		_mission.valid = _navigator->get_mission_result()->valid;
 		_navigator->get_mission_result()->seq_total = _mission.count;
 		_navigator->increment_mission_instance_count();
 		_navigator->set_mission_result_updated();
@@ -1812,8 +2019,9 @@ Mission::reset_mission(struct mission_s &mission)
 bool
 Mission::need_to_reset_mission()
 {
-	/* reset mission state when disarmed */
-	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset) {
+	/* reset mission state when disarmed and the mission was finished */
+	if (_navigator->get_vstatus()->arming_state != vehicle_status_s::ARMING_STATE_ARMED && _need_mission_reset
+	    && (_current_mission_index == _mission.count - 1 || _navigator->get_vstatus()->is_vtol)) {
 		_need_mission_reset = false;
 		return true;
 	}
@@ -1897,7 +2105,7 @@ bool Mission::position_setpoint_equal(const position_setpoint_s *p1, const posit
 		(fabsf(p1->yawspeed - p2->yawspeed) < FLT_EPSILON) &&
 		(p1->yawspeed_valid == p2->yawspeed_valid) &&
 		(fabsf(p1->loiter_radius - p2->loiter_radius) < FLT_EPSILON) &&
-		(p1->loiter_direction == p2->loiter_direction) &&
+		(p1->loiter_direction_counter_clockwise == p2->loiter_direction_counter_clockwise) &&
 		(fabsf(p1->acceptance_radius - p2->acceptance_radius) < FLT_EPSILON) &&
 		(fabsf(p1->cruising_speed - p2->cruising_speed) < FLT_EPSILON) &&
 		((fabsf(p1->cruising_throttle - p2->cruising_throttle) < FLT_EPSILON) || (!PX4_ISFINITE(p1->cruising_throttle)
@@ -1934,3 +2142,136 @@ void Mission::publish_navigator_mission_item()
 
 	_navigator_mission_item_pub.publish(navigator_mission_item);
 }
+
+bool Mission::getPreviousPositionItemIndex(const mission_s &mission, int start_index, unsigned &prev_pos_index)
+{
+	struct mission_item_s missionitem = {};
+
+	for (int index = start_index; index >= 0; index--) {
+
+		if (!readMissionItemAtIndex(mission, index, missionitem)) {
+			break;
+		}
+
+		if (MissionBlock::item_contains_position(missionitem)) {
+			prev_pos_index = index;
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool Mission::getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item)
+{
+	while (start_index < mission.count) {
+		if (readMissionItemAtIndex(mission, start_index, mission_item) && MissionBlock::item_contains_position(mission_item)) {
+			return true;
+		}
+
+		start_index++;
+	}
+
+	return false;
+}
+
+bool Mission::readMissionItemAtIndex(const mission_s &mission, const int index, mission_item_s &missionitem)
+{
+	bool success = false;
+
+	if (index >= 0 && index < mission.count) {
+		const dm_item_t dm_current = (dm_item_t)mission.dataman_id;
+		const ssize_t len = sizeof(missionitem);
+
+		success = (dm_read(dm_current, index, &missionitem, len) == len);
+	}
+
+	return success;
+}
+
+void Mission::cache_command(const mission_item_s &mission_item)
+{
+	switch (mission_item.nav_cmd) {
+	case NAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE:
+		_last_gimbal_configure_command = mission_item;
+		break;
+
+	case NAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
+		_last_gimbal_control_command = mission_item;
+		break;
+
+	case NAV_CMD_SET_CAMERA_MODE:
+		_last_camera_mode_command = mission_item;
+		break;
+
+	case NAV_CMD_DO_SET_CAM_TRIGG_DIST:
+	case NAV_CMD_DO_TRIGGER_CONTROL:
+	case NAV_CMD_IMAGE_START_CAPTURE:
+	case NAV_CMD_IMAGE_STOP_CAPTURE:
+		_last_camera_trigger_command = mission_item;
+		break;
+
+
+	default:
+		break;
+	}
+}
+
+void Mission::replay_cached_gimbal_commands()
+{
+	if (_last_gimbal_configure_command.nav_cmd > 0) {
+		issue_command(_last_gimbal_configure_command);
+	}
+
+	if (_last_gimbal_control_command.nav_cmd > 0) {
+		issue_command(_last_gimbal_control_command);
+	}
+}
+
+void Mission::replay_cached_camera_commands()
+{
+	if (_last_camera_mode_command.nav_cmd > 0) {
+		issue_command(_last_camera_mode_command);
+	}
+
+	if (_last_camera_trigger_command.nav_cmd > 0) {
+		issue_command(_last_camera_trigger_command);
+	}
+}
+
+void Mission::reset_command_cache()
+{
+	_last_gimbal_configure_command = {};
+	_last_gimbal_control_command = {};
+	_last_camera_mode_command = {};
+	_last_camera_trigger_command = {};
+}
+
+bool Mission::haveCachedCommands()
+{
+	return _last_gimbal_configure_command.nav_cmd > 0 ||
+	       _last_gimbal_control_command.nav_cmd > 0 ||
+	       _last_camera_mode_command.nav_cmd > 0 ||
+	       _last_camera_trigger_command.nav_cmd > 0;
+
+}
+
+bool Mission::cameraWasTriggering()
+{
+	return (_last_camera_trigger_command.nav_cmd == NAV_CMD_DO_TRIGGER_CONTROL
+		&& (int)(_last_camera_trigger_command.params[0] + 0.5f) == 1) ||
+	       (_last_camera_trigger_command.nav_cmd == NAV_CMD_IMAGE_START_CAPTURE) ||
+	       (_last_camera_trigger_command.nav_cmd == NAV_CMD_DO_SET_CAM_TRIGG_DIST
+		&& _last_camera_trigger_command.params[0] > 0.f);
+}
+
+void Mission::updateChachedCommandsUpToIndex(const int end_index)
+{
+	for (int i = 0; i <= end_index; i++) {
+		mission_item_s mission_item = {};
+
+		if (readMissionItemAtIndex(_mission, i, mission_item)) {
+			cache_command(mission_item);
+		}
+	}
+}
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 24dd8bf1c9..625e2bfb34 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -33,7 +33,9 @@
 /**
  * @file mission.h
  *
- * Navigator mode to access missions
+ * Mission mode class that handles everything related to executing a mission.
+ * This class gets included as one of the 'modes' in the Navigator, along with other
+ * modes like RTL, Loiter, etc.
  *
  * @author Julian Oes <julian@oes.ch>
  * @author Thomas Gubler <thomasgubler@gmail.com>
@@ -47,6 +49,7 @@
 #include "mission_block.h"
 #include "mission_feasibility_checker.h"
 #include "navigator_mode.h"
+#include "terrain_follower_wrapper.hpp"
 
 #include <float.h>
 
@@ -69,7 +72,7 @@ class Navigator;
 class Mission : public MissionBlock, public ModuleParams
 {
 public:
-	Mission(Navigator *navigator);
+	Mission(Navigator *navigator, TerrainFollowerWrapper &terrain_follower);
 	~Mission() override = default;
 
 	void on_inactive() override;
@@ -94,6 +97,7 @@ public:
 	double get_landing_lat() { return _landing_lat; }
 	double get_landing_lon() { return _landing_lon; }
 	float get_landing_alt() { return _landing_alt; }
+	float get_landing_loiter_rad() { return _landing_loiter_radius; }
 
 	void set_closest_item_as_current();
 
@@ -112,13 +116,21 @@ private:
 	 */
 	void update_mission();
 
+	/**
+	 * Restore old mission if update mission was not successful
+	 */
+	bool restore_old_mission();
+
 	/**
 	 * Move on to next mission item or switch to loiter
 	 */
 	void advance_mission();
 
 	/**
-	 * Set new mission items
+	 * @brief Configures mission items in current setting
+	 *
+	 * Configure the mission items depending on current mission item index and settings such
+	 * as terrain following, etc.
 	 */
 	void set_mission_items();
 
@@ -234,6 +246,24 @@ private:
 
 	void publish_navigator_mission_item();
 
+	bool getPreviousPositionItemIndex(const mission_s &mission, int start_index, unsigned &prev_pos_index);
+
+	bool getNextPositionMissionItem(const mission_s &mission, int start_index, mission_item_s &mission_item);
+
+	bool readMissionItemAtIndex(const mission_s &mission, const int index, mission_item_s &missionitem);
+	void cache_command(const mission_item_s &mission_item);
+
+	void updateChachedCommandsUpToIndex(int end_index);
+
+
+	void replay_cached_gimbal_commands();
+	void replay_cached_camera_commands();
+	void reset_command_cache();
+
+	bool haveCachedCommands();
+
+	bool cameraWasTriggering();
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MIS_DIST_1WP>) _param_mis_dist_1wp,
 		(ParamFloat<px4::params::MIS_DIST_WPS>) _param_mis_dist_wps,
@@ -244,8 +274,13 @@ private:
 
 	uORB::Subscription	_mission_sub{ORB_ID(mission)};		/**< mission subscription */
 	mission_s		_mission {};
+	mission_s		_old_mission {};			/**< old mission is used as a backup and for comparison with a new mission */
 
 	int32_t _current_mission_index{-1};
+	int32_t _previous_custom_action_mission_index{-1};
+
+	TerrainFollowerWrapper &_terrain_follower;
+	hrt_abstime _time_last_terrain_checked{0};
 
 	// track location of planned mission landing
 	bool	_land_start_available{false};
@@ -258,6 +293,8 @@ private:
 	double _landing_lon{0.0};
 	float _landing_alt{0.0f};
 
+	float _landing_loiter_radius{0.f};
+
 	bool _need_takeoff{true};					/**< if true, then takeoff must be performed before going to the first waypoint (if needed) */
 
 	hrt_abstime _time_mission_deactivated{0};
@@ -271,8 +308,9 @@ private:
 	bool _home_inited{false};
 	bool _need_mission_reset{false};
 	bool _mission_waypoints_changed{false};
-	bool _mission_changed{false}; /** < true if the mission changed since the mission mode was active */
+	bool _mission_changed{false};	/** < true if the mission changed since the mission mode was active */
 
+	// Work Item corresponds to the sub-mode set on the "MAV_CMD_DO_SET_MODE" MAVLink message
 	enum work_item_type {
 		WORK_ITEM_TYPE_DEFAULT,		/**< default mission item */
 		WORK_ITEM_TYPE_TAKEOFF,		/**< takeoff before moving to waypoint */
@@ -285,4 +323,14 @@ private:
 
 	uint8_t _mission_execution_mode{mission_result_s::MISSION_EXECUTION_MODE_NORMAL};	/**< the current mode of how the mission is executed,look at mission_result.msg for the definition */
 	bool _execution_mode_changed{false};
+
+
+	bool _replay_cached_gimbal_commands_at_next_waypoint = false;
+	bool _replay_cached_camera_commands_at_next_waypoint = false;
+	int _inactivation_index = -1;
+
+	mission_item_s _last_gimbal_configure_command {};
+	mission_item_s _last_gimbal_control_command {};
+	mission_item_s _last_camera_mode_command {};
+	mission_item_s _last_camera_trigger_command {};
 };
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index ddaa4501a6..21d7ccd875 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -59,35 +59,19 @@ using matrix::wrap_pi;
 MissionBlock::MissionBlock(Navigator *navigator) :
 	NavigatorMode(navigator)
 {
-	_mission_item.lat = (double)NAN;
-	_mission_item.lon = (double)NAN;
-	_mission_item.yaw = NAN;
-	_mission_item.loiter_radius = _navigator->get_loiter_radius();
-	_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
-	_mission_item.time_inside = 0.0f;
-	_mission_item.autocontinue = true;
-	_mission_item.origin = ORIGIN_ONBOARD;
+
 }
 
 bool
-MissionBlock::is_mission_item_reached()
+MissionBlock::is_mission_item_reached_or_completed()
 {
-	/* handle non-navigation or indefinite waypoints */
-
-	hrt_abstime now = hrt_absolute_time();
+	const hrt_abstime now = hrt_absolute_time();
 
+	// Handle indefinite waypoints and action commands
 	switch (_mission_item.nav_cmd) {
+
+	// Action Commands that doesn't have timeout completes instantaneously
 	case NAV_CMD_DO_SET_SERVO:
-		return true;
-
-	case NAV_CMD_LAND: /* fall through */
-	case NAV_CMD_VTOL_LAND:
-		return _navigator->get_land_detected()->landed;
-
-	case NAV_CMD_IDLE: /* fall through */
-	case NAV_CMD_LOITER_UNLIMITED:
-		return false;
-
 	case NAV_CMD_DO_LAND_START:
 	case NAV_CMD_DO_TRIGGER_CONTROL:
 	case NAV_CMD_DO_DIGICAM_CONTROL:
@@ -112,8 +96,18 @@ MissionBlock::is_mission_item_reached()
 	case NAV_CMD_SET_CAMERA_FOCUS:
 	case NAV_CMD_DO_CHANGE_SPEED:
 	case NAV_CMD_DO_SET_HOME:
+	case NAV_CMD_WAYPOINT_USER_1:
 		return true;
 
+	// Indefinite Waypoints
+	case NAV_CMD_LAND: /* fall through */
+	case NAV_CMD_VTOL_LAND:
+		return _navigator->get_land_detected()->landed;
+
+	case NAV_CMD_IDLE: /* fall through */
+	case NAV_CMD_LOITER_UNLIMITED:
+		return false;
+
 	case NAV_CMD_DO_VTOL_TRANSITION:
 
 		if (int(_mission_item.params[0]) == 3) {
@@ -129,6 +123,13 @@ MissionBlock::is_mission_item_reached()
 			return false;
 		}
 
+	case NAV_CMD_VTOL_TAKEOFF:
+		if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			return true;
+		}
+
+		break;
+
 	case NAV_CMD_DELAY:
 		// Set reached flags directly such that only the delay time is considered
 		_waypoint_position_reached = true;
@@ -139,11 +140,49 @@ MissionBlock::is_mission_item_reached()
 			_time_wp_reached = now;
 		}
 
+		break;
+
+	case NAV_CMD_DO_WINCH: {
+			const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) *
+					1E-6f; // TODO: Add proper microseconds_to_seconds function
+
+			if (_payload_deploy_ack_successful) {
+				PX4_DEBUG("Winch Deploy Ack received! Resuming mission");
+				return true;
+
+			} else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) {
+				PX4_DEBUG("Winch Deploy Timed out, resuming mission!");
+				return true;
+
+			}
+
+			// We are still waiting for the acknowledgement / execution of deploy
+			return false;
+		}
+
+	case NAV_CMD_DO_GRIPPER: {
+			const float payload_deploy_elasped_time_s = (now - _payload_deployed_time) * 1E-6f;
+
+			if (_payload_deploy_ack_successful) {
+				PX4_DEBUG("Gripper Deploy Ack received! Resuming mission");
+				return true;
+
+			} else if (payload_deploy_elasped_time_s > _payload_deploy_timeout_s) {
+				PX4_DEBUG("Gripper Deploy Timed out, resuming mission!");
+				return true;
+
+			}
+
+			// We are still waiting for the acknowledgement / execution of deploy
+			return false;
+		}
+
 	default:
 		/* do nothing, this is a 3D waypoint */
 		break;
 	}
 
+	// Update the 'waypoint position reached' status
 	if (!_navigator->get_land_detected()->landed && !_waypoint_position_reached) {
 
 		float dist = -1.0f;
@@ -152,6 +191,11 @@ MissionBlock::is_mission_item_reached()
 
 		const float mission_item_altitude_amsl = get_absolute_altitude_for_item(_mission_item);
 
+		// consider mission_item.loiter_radius invalid if NAN or 0, use default value in this case.
+		const float mission_item_loiter_radius_abs = (PX4_ISFINITE(_mission_item.loiter_radius)
+				&& fabsf(_mission_item.loiter_radius) > FLT_EPSILON) ? fabsf(_mission_item.loiter_radius) :
+				_navigator->get_loiter_radius();
+
 		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, mission_item_altitude_amsl,
 				_navigator->get_global_position()->lat,
 				_navigator->get_global_position()->lon,
@@ -171,7 +215,7 @@ MissionBlock::is_mission_item_reached()
 
 			float altitude_acceptance_radius = _navigator->get_altitude_acceptance_radius();
 
-			/* It should be safe to just use half of the takoeff_alt as an acceptance radius. */
+			/* It should be safe to just use half of the takeoff_alt as an acceptance radius. */
 			if (takeoff_alt > 0 && takeoff_alt < altitude_acceptance_radius) {
 				altitude_acceptance_radius = takeoff_alt / 2.0f;
 			}
@@ -182,15 +226,22 @@ MissionBlock::is_mission_item_reached()
 				_waypoint_position_reached = true;
 			}
 
+		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
+			   && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+			/* fixed-wing takeoff is reached once the vehicle has exceeded the takeoff altitude */
+			if (_navigator->get_global_position()->alt > mission_item_altitude_amsl) {
+				_waypoint_position_reached = true;
+			}
+
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
 			   && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
-			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
+			// Accept takeoff waypoint to be reached if the distance in 2D plane is within acceptance radius
 			if (dist_xy >= 0.0f && dist_xy <= _navigator->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
+			// For takeoff mission items use the parameter for the takeoff acceptance radius
 			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()
 			    && dist_z <= _navigator->get_altitude_acceptance_radius()) {
 				_waypoint_position_reached = true;
@@ -208,7 +259,7 @@ MissionBlock::is_mission_item_reached()
 			 */
 
 			// check if within loiter radius around wp, if yes then set altitude sp to mission item
-			if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
+			if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs)
 			    && dist_z <= _navigator->get_altitude_acceptance_radius()) {
 
 				_waypoint_position_reached = true;
@@ -230,15 +281,15 @@ MissionBlock::is_mission_item_reached()
 						&dist_xy, &dist_z);
 
 				// check if within loiter radius around wp, if yes then set altitude sp to mission item
-				if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
-				    && dist_z <= _navigator->get_default_altitude_acceptance_radius()) {
+				if (dist >= 0.0f && dist_xy <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs)
+				    && dist_z <= _navigator->get_altitude_acceptance_radius()) {
 
 					curr_sp->alt = mission_item_altitude_amsl;
 					curr_sp->type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 					_navigator->set_position_setpoint_triplet_updated();
 				}
 
-			} else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + fabsf(_mission_item.loiter_radius))
+			} else if (dist >= 0.f && dist_xy <= (_navigator->get_acceptance_radius() + mission_item_loiter_radius_abs)
 				   && dist_z <= _navigator->get_altitude_acceptance_radius()) {
 				// loitering, check if new altitude is reached, while still also having check on position
 
@@ -359,8 +410,7 @@ MissionBlock::is_mission_item_reached()
 		}
 	}
 
-	/* Check if the requested yaw setpoint is reached (only for rotary wing flight). */
-
+	// Update the 'waypoint position reached' status (only for rotary wing flight)
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
 		if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
@@ -396,7 +446,7 @@ MissionBlock::is_mission_item_reached()
 		}
 	}
 
-	/* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */
+	// Handle Loiter/Delay Timeout if the waypoint position and yaw setpoint got reached
 	if (_waypoint_position_reached && _waypoint_yaw_reached) {
 
 		bool time_inside_reached = false;
@@ -407,8 +457,8 @@ MissionBlock::is_mission_item_reached()
 			time_inside_reached = true;
 		}
 
-		// check if heading for exit is reached (only applies for fixed-wing flight)
-		bool exit_heading_reached = false;
+		// check if course for exit is reached (only applies for fixed-wing flight)
+		bool exit_course_reached = false;
 
 		if (time_inside_reached) {
 
@@ -418,37 +468,38 @@ MissionBlock::is_mission_item_reached()
 			const float dist_current_next = get_distance_to_next_waypoint(curr_sp_new->lat, curr_sp_new->lon, next_sp.lat,
 							next_sp.lon);
 
-			/* enforce exit heading if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set,
+			/* enforce exit course if in FW, the next wp is valid, the vehicle is currently loitering and either having force_heading set,
 			   or if loitering to achieve altitdue at a NAV_CMD_WAYPOINT */
-			const bool enforce_exit_heading = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
-							  &&
-							  next_sp.valid &&
-							  curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER &&
-							  (_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT);
+			const bool enforce_exit_course = _navigator->get_vstatus()->vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+							 && next_sp.valid
+							 && curr_sp_new->type == position_setpoint_s::SETPOINT_TYPE_LOITER
+							 && (_mission_item.force_heading || _mission_item.nav_cmd == NAV_CMD_WAYPOINT);
 
-			// can only enforce exit heading if next waypoint is not within loiter radius of current waypoint
-			const bool exit_heading_is_reachable = dist_current_next > 1.2f * curr_sp_new->loiter_radius;
+			// can only enforce exit course if next waypoint is not within loiter radius of current waypoint
+			const bool exit_course_is_reachable = dist_current_next > 1.2f * curr_sp_new->loiter_radius;
 
-			if (enforce_exit_heading && exit_heading_is_reachable) {
+			if (enforce_exit_course && exit_course_is_reachable) {
 
-				float yaw_err = 0.0f;
+				float vehicle_position_to_next_waypoint_north;
+				float vehicle_position_to_next_waypoint_east;
+				get_vector_to_next_waypoint(_navigator->get_global_position()->lat, _navigator->get_global_position()->lon, next_sp.lat,
+							    next_sp.lon, &vehicle_position_to_next_waypoint_north,  &vehicle_position_to_next_waypoint_east);
 
-				// set required yaw from bearing to the next mission item
-				_mission_item.yaw = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
-						    _navigator->get_global_position()->lon,
-						    next_sp.lat, next_sp.lon);
-				const float cog = atan2f(_navigator->get_local_position()->vy, _navigator->get_local_position()->vx);
-				yaw_err = wrap_pi(_mission_item.yaw - cog);
+				// this vector defines the exit bearing
+				const matrix::Vector2f vector_vehicle_position_to_next_waypoint = {vehicle_position_to_next_waypoint_north, vehicle_position_to_next_waypoint_east};
 
-				exit_heading_reached = fabsf(yaw_err) < _navigator->get_yaw_threshold();
+				const matrix::Vector2f vehicle_ground_velocity = {_navigator->get_local_position()->vx, _navigator->get_local_position()->vy};
+
+				exit_course_reached = vector_vehicle_position_to_next_waypoint.dot(vehicle_ground_velocity) >
+						      vector_vehicle_position_to_next_waypoint.norm() * vehicle_ground_velocity.norm() * kCosineExitCourseThreshold;
 
 			} else {
-				exit_heading_reached = true;
+				exit_course_reached = true;
 			}
 		}
 
 		// set exit flight course to next waypoint
-		if (exit_heading_reached) {
+		if (exit_course_reached) {
 			position_setpoint_s &curr_sp = _navigator->get_position_setpoint_triplet()->current;
 			const position_setpoint_s &next_sp = _navigator->get_position_setpoint_triplet()->next;
 
@@ -463,15 +514,15 @@ MissionBlock::is_mission_item_reached()
 				float bearing = get_bearing_to_next_waypoint(curr_sp.lat, curr_sp.lon, next_sp.lat, next_sp.lon);
 
 				// calculate (positive) angle between current bearing vector (orbit center to next waypoint) and vector pointing to tangent exit location
-				const float ratio = math::min(fabsf(_mission_item.loiter_radius / range), 1.0f);
+				const float ratio = math::min(fabsf(curr_sp.loiter_radius / range), 1.0f);
 				float inner_angle = acosf(ratio);
 
 				// Compute "ideal" tangent origin
-				if (curr_sp.loiter_direction > 0) {
-					bearing -= inner_angle;
+				if (curr_sp.loiter_direction_counter_clockwise) {
+					bearing += inner_angle;
 
 				} else {
-					bearing += inner_angle;
+					bearing -= inner_angle;
 				}
 
 				// set typ to position, will get set to loiter in the fw position controller once close
@@ -519,6 +570,23 @@ MissionBlock::issue_command(const mission_item_s &item)
 
 		_actuator_pub.publish(actuators);
 
+	} else if (item.nav_cmd == NAV_CMD_DO_WINCH ||
+		   item.nav_cmd == NAV_CMD_DO_GRIPPER) {
+		// Initiate Payload Deployment
+		vehicle_command_s vcmd = {};
+		vcmd.command = item.nav_cmd;
+		vcmd.param1 = item.params[0];
+		vcmd.param2 = item.params[1];
+		vcmd.param3 = item.params[2];
+		vcmd.param4 = item.params[3];
+		vcmd.param5 = static_cast<double>(item.params[4]);
+		vcmd.param6 = static_cast<double>(item.params[5]);
+		_navigator->publish_vehicle_cmd(&vcmd);
+
+		// Reset payload deploy flag & data to get ready to receive deployment ack result
+		_payload_deploy_ack_successful = false;
+		_payload_deployed_time = hrt_absolute_time();
+
 	} else {
 
 		// This is to support legacy DO_MOUNT_CONTROL as part of a mission.
@@ -526,8 +594,8 @@ MissionBlock::issue_command(const mission_item_s &item)
 			_navigator->acquire_gimbal_control();
 		}
 
-		// we're expecting a mission command item here so assign the "raw" inputs to the command
-		// (MAV_FRAME_MISSION mission item)
+		// Mission item's NAV_CMD enums directly map to the according vehicle command
+		// So set the raw value directly (MAV_FRAME_MISSION mission item)
 		vehicle_command_s vcmd = {};
 		vcmd.command = item.nav_cmd;
 		vcmd.param1 = item.params[0];
@@ -567,6 +635,17 @@ MissionBlock::get_time_inside(const mission_item_s &item) const
 	return 0.0f;
 }
 
+// TODO: get_time_inside and item_has_timeout is quite redundant. Separate them out
+// Problem arises from the fact that DO_WINCH and DO_GRIPPER *should be an instantaneous command,
+// and shouldn't have a timeout defined as it is a DO_* command. It should rather be defined as CONDITION_GRIPPER
+// or so, and have a function named 'item_is_conditional'
+// Reference: https://mavlink.io/en/services/mission.html#mavlink_commands
+bool
+MissionBlock::item_has_timeout(const mission_item_s &item)
+{
+	return item.nav_cmd == NAV_CMD_DO_WINCH || item.nav_cmd == NAV_CMD_DO_GRIPPER;
+}
+
 bool
 MissionBlock::item_contains_position(const mission_item_s &item)
 {
@@ -577,8 +656,7 @@ MissionBlock::item_contains_position(const mission_item_s &item)
 	       item.nav_cmd == NAV_CMD_TAKEOFF ||
 	       item.nav_cmd == NAV_CMD_LOITER_TO_ALT ||
 	       item.nav_cmd == NAV_CMD_VTOL_TAKEOFF ||
-	       item.nav_cmd == NAV_CMD_VTOL_LAND ||
-	       item.nav_cmd == NAV_CMD_DO_FOLLOW_REPOSITION;
+	       item.nav_cmd == NAV_CMD_VTOL_LAND;
 }
 
 bool
@@ -596,7 +674,7 @@ MissionBlock::item_contains_marker(const mission_item_s &item)
 bool
 MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, position_setpoint_s *sp)
 {
-	/* don't change the setpoint for non-position items */
+	// Don't change the setpoint for non-position items
 	if (!item_contains_position(item)) {
 		return false;
 	}
@@ -608,9 +686,9 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	sp->yaw_valid = PX4_ISFINITE(item.yaw);
 	sp->loiter_radius = (fabsf(item.loiter_radius) > NAV_EPSILON_POSITION) ? fabsf(item.loiter_radius) :
 			    _navigator->get_loiter_radius();
-	sp->loiter_direction = math::signNoZero(item.loiter_radius);
+	sp->loiter_direction_counter_clockwise = item.loiter_radius < 0;
 
-	if (item.acceptance_radius > 0.0f && PX4_ISFINITE(item.acceptance_radius)) {
+	if (item.acceptance_radius > 0.001f && PX4_ISFINITE(item.acceptance_radius)) {
 		// if the mission item has a specified acceptance radius, overwrite the default one from parameters
 		sp->acceptance_radius = item.acceptance_radius;
 
@@ -621,6 +699,10 @@ MissionBlock::mission_item_to_position_setpoint(const mission_item_s &item, posi
 	sp->cruising_speed = _navigator->get_cruising_speed();
 	sp->cruising_throttle = _navigator->get_cruising_throttle();
 
+	// for fixed wing we don't use cruising_throttle directly anymore, instead we command airspeed setpoints via cruising_speed
+	// we still use cruising throttle here to determine if gliding is enabled
+	sp->gliding_enabled = (_navigator->get_cruising_throttle() < FLT_EPSILON);
+
 	switch (item.nav_cmd) {
 	case NAV_CMD_IDLE:
 		sp->type = position_setpoint_s::SETPOINT_TYPE_IDLE;
@@ -739,6 +821,7 @@ MissionBlock::set_takeoff_item(struct mission_item_s *item, float abs_altitude)
 	item->altitude = abs_altitude;
 	item->altitude_is_relative = false;
 
+	item->acceptance_radius = _navigator->get_acceptance_radius();
 	item->loiter_radius = _navigator->get_loiter_radius();
 	item->autocontinue = false;
 	item->origin = ORIGIN_ONBOARD;
@@ -804,7 +887,13 @@ MissionBlock::set_vtol_transition_item(struct mission_item_s *item, const uint8_
 	item->nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
 	item->params[0] = (float) new_mode;
 	item->params[1] = 0.0f;
-	item->yaw = _navigator->get_local_position()->heading; // ideally that would be course and not heading
+
+	// Keep yaw from previous mission item if valid, as that is containing the transition heading.
+	// If not valid use current yaw as yaw setpoint
+	if (!PX4_ISFINITE(item->yaw)) {
+		item->yaw = _navigator->get_local_position()->heading; // ideally that would be course and not heading
+	}
+
 	item->autocontinue = true;
 }
 
@@ -841,3 +930,16 @@ MissionBlock::get_absolute_altitude_for_item(const mission_item_s &mission_item)
 		return mission_item.altitude;
 	}
 }
+
+void
+MissionBlock::initialize()
+{
+	_mission_item.lat = (double)NAN;
+	_mission_item.lon = (double)NAN;
+	_mission_item.yaw = NAN;
+	_mission_item.loiter_radius = _navigator->get_loiter_radius();
+	_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+	_mission_item.time_inside = 0.0f;
+	_mission_item.autocontinue = true;
+	_mission_item.origin = ORIGIN_ONBOARD;
+}
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index a6e0045a7b..c2c05eb17d 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -33,7 +33,8 @@
 /**
  * @file mission_block.h
  *
- * Helper class to use mission items
+ * Base class for Mission class and special flight modes like
+ * RTL, Land, Loiter, Takeoff, Geofence, etc.
  *
  * @author Julian Oes <julian@oes.ch>
  */
@@ -53,6 +54,9 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vtol_vehicle_status.h>
 
+// cosine of maximal course error to exit loiter if exit course is enforced (fixed-wing only)
+static constexpr float kCosineExitCourseThreshold = 0.99619f; // cos(5°)
+
 class Navigator;
 
 class MissionBlock : public NavigatorMode
@@ -67,6 +71,8 @@ public:
 	MissionBlock(const MissionBlock &) = delete;
 	MissionBlock &operator=(const MissionBlock &) = delete;
 
+	void initialize() override;
+
 	/**
 	 * Check if the mission item contains a navigation position
 	 *
@@ -74,6 +80,11 @@ public:
 	 */
 	static bool item_contains_position(const mission_item_s &item);
 
+	/**
+	 * Returns true if the mission item is not an instant action, but has a delay / timeout
+	 */
+	bool item_has_timeout(const mission_item_s &item);
+
 	/**
 	 * Check if the mission item contains a gate condition
 	 *
@@ -88,12 +99,42 @@ public:
 	 */
 	static bool item_contains_marker(const mission_item_s &item);
 
+	/**
+	 * @brief Set the payload deployment successful flag object
+	 *
+	 * Function is accessed in Navigator (navigator_main.cpp) to flag when a successful
+	 * payload deployment ack command has been received. Which in turn allows the mission item
+	 * to continue to the next in the 'is_mission_item_reached_or_completed' function below
+	 */
+	void set_payload_depolyment_successful_flag(bool payload_deploy_result)
+	{
+		_payload_deploy_ack_successful = payload_deploy_result;
+	}
+
+	/**
+	 * @brief Set the payload deployment timeout
+	 *
+	 * Accessed in Navigator to set the appropriate timeout to wait for while waiting for a successful
+	 * payload delivery acknowledgement. If the payload deployment takes longer than timeout, mission will
+	 * continue into the next item automatically.
+	 *
+	 * @param timeout_s Timeout in seconds
+	 */
+	void set_payload_deployment_timeout(const float timeout_s)
+	{
+		_payload_deploy_timeout_s = timeout_s;
+	}
+
 protected:
 	/**
-	 * Check if mission item has been reached
-	 * @return true if successfully reached
+	 * Check if mission item has been reached (for Waypoint based mission items) or Completed (Action based mission items)
+	 *
+	 * Mission Item's 'nav_cmd' can be either Waypoint or Action based. In order to check whether current mission item's
+	 * execution was successful, we need to check either the waypoint was 'reached' or the action was 'completed'.
+	 *
+	 * @return true if successfully reached or completed
 	 */
-	bool is_mission_item_reached();
+	bool is_mission_item_reached_or_completed();
 
 	/**
 	 * Reset all reached flags
@@ -140,18 +181,39 @@ protected:
 	 */
 	void mission_apply_limitation(mission_item_s &item);
 
+	/**
+	 * @brief Issue a command for mission items with a nav_cmd that specifies an action
+	 *
+	 * Execute the specified command inside the mission item. The action depends on the nav_cmd
+	 * value (which correlates to the MAVLink's MAV_CMD enum values) and the params defined in the
+	 * mission item.
+	 *
+	 * This is used for commands like MAV_CMD_DO* in MAVLink, where immediate actions are defined.
+	 * For more information, refer to: https://mavlink.io/en/services/mission.html#mavlink_commands
+	 *
+	 * @param item Mission item to execute
+	 */
 	void issue_command(const mission_item_s &item);
 
-	float get_time_inside(const mission_item_s &item) const ;
+	/**
+	 * [s] Get the time to stay that's specified in the mission item
+	 */
+	float get_time_inside(const mission_item_s &item) const;
 
 	float get_absolute_altitude_for_item(const mission_item_s &mission_item) const;
 
-	mission_item_s _mission_item{};
+	mission_item_s _mission_item{}; // Current mission item that is being executed
 
 	bool _waypoint_position_reached{false};
 	bool _waypoint_yaw_reached{false};
 
 	hrt_abstime _time_wp_reached{0};
 
+	/* Payload Deploy internal states are used by two NAV_CMDs: DO_WINCH and DO_GRIPPER */
+	bool _payload_deploy_ack_successful{false};	// Flag to keep track of whether we received an acknowledgement for a successful payload deployment
+	hrt_abstime _payload_deployed_time{0};		// Last payload deployment start time to handle timeouts
+	float _payload_deploy_timeout_s{0.0f};		// Timeout for payload deployment in Mission class, to prevent endless loop if successful deployment ack is never received
+
+	// Used for MAV_CMD_DO_SET_SERVO command of a Mission item
 	uORB::Publication<actuator_controls_s>	_actuator_pub{ORB_ID(actuator_controls_2)};
 };
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 678b133e04..41e89f21c3 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -48,16 +48,13 @@
 #include <drivers/drv_pwm_output.h>
 #include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
-#include <lib/landing_slope/Landingslope.hpp>
 #include <systemlib/mavlink_log.h>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/position_controller_landing_status.h>
 #include <px4_platform_common/events.h>
 
 bool
 MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
-		float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
-		bool land_start_req)
+		float max_distance_to_1st_waypoint, float max_distance_between_waypoints)
 {
 	// Reset warning flag
 	_navigator->get_mission_result()->warning = false;
@@ -90,14 +87,18 @@ MissionFeasibilityChecker::checkMissionFeasible(const mission_s &mission,
 	failed = failed || !checkGeofence(mission, home_alt, home_valid);
 	failed = failed || !checkHomePositionAltitude(mission, home_alt, home_alt_valid);
 
+	// reset for next check
+	_has_takeoff = false;
+	_has_landing = false;
+
 	if (_navigator->get_vstatus()->is_vtol) {
-		failed = failed || !checkVTOL(mission, home_alt, false);
+		failed = failed || !checkVTOL(mission, home_alt);
 
 	} else if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
 		failed = failed || !checkRotarywing(mission, home_alt);
 
 	} else {
-		failed = failed || !checkFixedwing(mission, home_alt, land_start_req);
+		failed = failed || !checkFixedwing(mission, home_alt);
 	}
 
 	return !failed;
@@ -110,29 +111,52 @@ MissionFeasibilityChecker::checkRotarywing(const mission_s &mission, float home_
 	 * Perform check and issue feedback to the user
 	 * Mission is only marked as feasible if takeoff check passes
 	 */
-	return checkTakeoff(mission, home_alt);
+	bool resTakeoff = checkTakeoff(mission, home_alt);
+	bool resVTOLLanding = checkVTOLLanding(mission);
+	bool hasVTOLLanding = _has_landing && resVTOLLanding;
+
+	bool res = resTakeoff && !_has_vtol_takeoff && !hasVTOLLanding;
+
+	if (_has_vtol_takeoff || hasVTOLLanding) {
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+				     "Mission rejected: plan has VTOL items but vehicle isn't a VTOL.\t");
+		events::send(events::ID("navigator_vtol_mis_on_mc"), {events::Log::Error, events::LogInternal::Info},
+			     "Mission rejected: plan has VTOL items but vehicle isn't a VTOL");
+	}
+
+	return res;
 }
 
 bool
-MissionFeasibilityChecker::checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req)
+MissionFeasibilityChecker::checkFixedwing(const mission_s &mission, float home_alt)
 {
 	/* Perform checks and issue feedback to the user for all checks */
 	bool resTakeoff = checkTakeoff(mission, home_alt);
-	bool resLanding = checkFixedWingLanding(mission, land_start_req);
+	bool resLanding = checkFixedWingLanding(mission);
+
+	bool res = resTakeoff && !_has_vtol_takeoff && resLanding;
+
+	if (_has_vtol_takeoff) {
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+				     "Mission rejected: plan has VTOL items but vehicle isn't a VTOL.\t");
+		events::send(events::ID("navigator_vtol_mis_on_fw"), {events::Log::Error, events::LogInternal::Info},
+			     "Mission rejected: plan has VTOL items but vehicle isn't a VTOL");
+	}
 
 	/* Mission is only marked as feasible if all checks return true */
-	return (resTakeoff && resLanding);
+	return res;
 }
 
 bool
-MissionFeasibilityChecker::checkVTOL(const mission_s &mission, float home_alt, bool land_start_req)
+MissionFeasibilityChecker::checkVTOL(const mission_s &mission, float home_alt)
 {
 	/* Perform checks and issue feedback to the user for all checks */
 	bool resTakeoff = checkTakeoff(mission, home_alt);
-	bool resLanding = checkVTOLLanding(mission, land_start_req);
+	bool resLanding = checkVTOLLanding(mission);
+	bool resTakeoffLandReq = checkTakeoffLandAvailable();
 
 	/* Mission is only marked as feasible if all checks return true */
-	return (resTakeoff && resLanding);
+	return (resTakeoff && resLanding && resTakeoffLandReq);
 }
 
 bool
@@ -252,6 +276,8 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
 		    missionitem.nav_cmd != NAV_CMD_VTOL_LAND &&
 		    missionitem.nav_cmd != NAV_CMD_DELAY &&
 		    missionitem.nav_cmd != NAV_CMD_CONDITION_GATE &&
+		    missionitem.nav_cmd != NAV_CMD_DO_WINCH &&
+		    missionitem.nav_cmd != NAV_CMD_DO_GRIPPER &&
 		    missionitem.nav_cmd != NAV_CMD_DO_JUMP &&
 		    missionitem.nav_cmd != NAV_CMD_DO_CHANGE_SPEED &&
 		    missionitem.nav_cmd != NAV_CMD_DO_SET_HOME &&
@@ -278,7 +304,8 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
 		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
 		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
 		    missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
-		    missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION) {
+		    missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION &&
+		    missionitem.nav_cmd != NAV_CMD_WAYPOINT_USER_1) {
 
 			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: item %i: unsupported cmd: %d\t",
 					     (int)(i + 1),
@@ -326,7 +353,6 @@ MissionFeasibilityChecker::checkMissionItemValidity(const mission_s &mission)
 bool
 MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt)
 {
-	bool has_takeoff = false;
 	bool takeoff_first = false;
 	int takeoff_index = -1;
 
@@ -340,7 +366,7 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
 		}
 
 		// look for a takeoff waypoint
-		if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) {
+		if (missionitem.nav_cmd == NAV_CMD_TAKEOFF || missionitem.nav_cmd == NAV_CMD_VTOL_TAKEOFF) {
 			// make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius
 			// this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air
 
@@ -366,7 +392,8 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
 			}
 
 			// tell that mission has a takeoff waypoint
-			has_takeoff = true;
+			_has_takeoff = true;
+			_has_vtol_takeoff = (missionitem.nav_cmd == NAV_CMD_VTOL_TAKEOFF);
 
 			// tell that a takeoff waypoint is the first "waypoint"
 			// mission item
@@ -422,37 +449,30 @@ MissionFeasibilityChecker::checkTakeoff(const mission_s &mission, float home_alt
 					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_MODE &&
 					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_ZOOM &&
 					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
-					  missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION);
+					  missionitem.nav_cmd != NAV_CMD_SET_CAMERA_FOCUS &&
+					  missionitem.nav_cmd != NAV_CMD_DO_VTOL_TRANSITION &&
+					  missionitem.nav_cmd != NAV_CMD_WAYPOINT_USER_1);
 		}
 	}
 
-	if (_navigator->get_takeoff_required() && _navigator->get_land_detected()->landed) {
-		// check for a takeoff waypoint, after the above conditions have been met
-		// MIS_TAKEOFF_REQ param has to be set and the vehicle has to be landed - one can load a mission
-		// while the vehicle is flying and it does not require a takeoff waypoint
-		if (!has_takeoff) {
-			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff waypoint required.\t");
-			events::send(events::ID("navigator_mis_takeoff_missing"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: takeoff waypoint required");
-			return false;
 
-		} else if (!takeoff_first) {
-			// check if the takeoff waypoint is the first waypoint item on the mission
-			// i.e, an item with position/attitude change modification
-			// if it is not, the mission should be rejected
-			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff not first waypoint item\t");
-			events::send(events::ID("navigator_mis_takeoff_not_first"), {events::Log::Error, events::LogInternal::Info},
-				     "Mission rejected: takeoff is not the first waypoint item");
-			return false;
-		}
+	if (_has_takeoff && !takeoff_first) {
+		// check if the takeoff waypoint is the first waypoint item on the mission
+		// i.e, an item with position/attitude change modification
+		// if it is not, the mission should be rejected
+		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: takeoff not first waypoint item\t");
+		events::send(events::ID("navigator_mis_takeoff_not_first"), {events::Log::Error, events::LogInternal::Info},
+			     "Mission rejected: takeoff is not the first waypoint item");
+		return false;
 	}
 
+
 	// all checks have passed
 	return true;
 }
 
 bool
-MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool land_start_req)
+MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission)
 {
 	/* Go through all mission items and search for a landing waypoint
 	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
@@ -489,6 +509,19 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
 		if (missionitem.nav_cmd == NAV_CMD_LAND) {
 			mission_item_s missionitem_previous {};
 
+			float param_fw_lnd_ang = 0.0f;
+			const param_t param_handle_fw_lnd_ang = param_find("FW_LND_ANG");
+
+			if (param_handle_fw_lnd_ang == PARAM_INVALID) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: FW_LND_ANG parameter is missing.\t");
+				events::send(events::ID("navigator_mis_land_angle_param_missing"), {events::Log::Error, events::LogInternal::Info},
+					     "Mission rejected: FW_LND_ANG parameter is missing");
+				return false;
+
+			} else {
+				param_get(param_handle_fw_lnd_ang, &param_fw_lnd_ang);
+			}
+
 			if (i > 0) {
 				landing_approach_index = i - 1;
 
@@ -499,59 +532,87 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
 
 				if (MissionBlock::item_contains_position(missionitem_previous)) {
 
-					uORB::SubscriptionData<position_controller_landing_status_s> landing_status{ORB_ID(position_controller_landing_status)};
+					const float land_alt_amsl = missionitem.altitude_is_relative ? missionitem.altitude +
+								    _navigator->get_home_position()->alt : missionitem.altitude;
+					const float entrance_alt_amsl = missionitem_previous.altitude_is_relative ? missionitem_previous.altitude +
+									_navigator->get_home_position()->alt : missionitem_previous.altitude;
+					const float relative_approach_altitude = entrance_alt_amsl - land_alt_amsl;
 
-					const bool landing_status_valid = (landing_status.get().timestamp > 0);
-					const float wp_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon,
-								  missionitem.lat, missionitem.lon);
+					if (relative_approach_altitude < FLT_EPSILON) {
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+								     "Mission rejected: the approach waypoint must be above the landing point.\t");
+						events::send(events::ID("navigator_mis_approach_wp_below_land"), {events::Log::Error, events::LogInternal::Info},
+							     "Mission rejected: the approach waypoint must be above the landing point");
+						return false;
+					}
 
-					if (landing_status_valid && (wp_distance > landing_status.get().flare_length)) {
-						/* Last wp is before flare region */
+					float landing_approach_distance;
 
-						const float delta_altitude = missionitem.altitude - missionitem_previous.altitude;
+					if (missionitem_previous.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
+						// assume this is a fixed-wing landing pattern with orbit to alt followed
+						// by tangent exit to landing approach and touchdown at landing waypoint
 
-						if (delta_altitude < 0) {
+						const float distance_orbit_center_to_land = get_distance_to_next_waypoint(missionitem_previous.lat,
+								missionitem_previous.lon, missionitem.lat, missionitem.lon);
+						const float orbit_radius = fabsf(missionitem_previous.loiter_radius);
 
-							const float horizontal_slope_displacement = landing_status.get().horizontal_slope_displacement;
-							const float slope_angle_rad = landing_status.get().slope_angle_rad;
-							const float slope_alt_req = Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, missionitem.altitude,
-										    horizontal_slope_displacement, slope_angle_rad);
-
-							if (missionitem_previous.altitude > slope_alt_req + 1.0f) {
-								/* Landing waypoint is above altitude of slope at the given waypoint distance (with small tolerance for floating point discrepancies) */
-								const float wp_distance_req = Landingslope::getLandingSlopeWPDistance(missionitem_previous.altitude,
-											      missionitem.altitude, horizontal_slope_displacement, slope_angle_rad);
-
-								mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: adjust landing approach.\t");
-								mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Move down %d m or move further away by %d m.\t",
-										     (int)ceilf(slope_alt_req - missionitem_previous.altitude),
-										     (int)ceilf(wp_distance_req - wp_distance));
-								/* EVENT
-								 * @description
-								 * The landing waypoint must be above the altitude of slope at the given waypoint distance.
-								 * Move it down {1m_v} or move it further away by {2m}.
-								 */
-								events::send<int32_t, int32_t>(events::ID("navigator_mis_land_approach"), {events::Log::Error, events::LogInternal::Info},
-											       "Mission rejected: adjust landing approach",
-											       (int)ceilf(slope_alt_req - missionitem_previous.altitude),
-											       (int)ceilf(wp_distance_req - wp_distance));
-
-								return false;
-							}
-
-						} else {
-							/* Landing waypoint is above last waypoint */
-							mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing above last waypoint.\t");
-							events::send(events::ID("navigator_mis_land_too_high"), {events::Log::Error, events::LogInternal::Info},
-								     "Mission rejected: landing waypoint is above the last waypoint");
+						if (distance_orbit_center_to_land <= orbit_radius) {
+							mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+									     "Mission rejected: the landing point must be outside the orbit radius.\t");
+							events::send(events::ID("navigator_mis_land_wp_inside_orbit_radius"), {events::Log::Error, events::LogInternal::Info},
+								     "Mission rejected: the landing point must be outside the orbit radius");
 							return false;
 						}
 
+						landing_approach_distance = sqrtf(distance_orbit_center_to_land * distance_orbit_center_to_land - orbit_radius *
+										  orbit_radius);
+
+					} else if (missionitem_previous.nav_cmd == NAV_CMD_WAYPOINT) {
+						// approaching directly from waypoint position
+
+						const float waypoint_distance = get_distance_to_next_waypoint(missionitem_previous.lat, missionitem_previous.lon,
+										missionitem.lat, missionitem.lon);
+						landing_approach_distance = waypoint_distance;
+
 					} else {
-						/* Last wp is in flare region */
-						mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: waypoint within landing flare.\t");
-						events::send(events::ID("navigator_mis_land_within_flare"), {events::Log::Error, events::LogInternal::Info},
-							     "Mission rejected: waypoint is within landing flare");
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+								     "Mission rejected: unsupported landing approach entrance waypoint type. Only ORBIT_TO_ALT or WAYPOINT allowed.\t");
+						events::send(events::ID("navigator_mis_unsupported_landing_approach_wp"), {events::Log::Error, events::LogInternal::Info},
+							     "Mission rejected: unsupported landing approach entrance waypoint type. Only ORBIT_TO_ALT or WAYPOINT allowed");
+						return false;
+					}
+
+					const float glide_slope = relative_approach_altitude / landing_approach_distance;
+
+					// respect user setting as max glide slope, but account for floating point
+					// rounding on next check with small (arbitrary) 0.1 deg buffer, as the
+					// landing angle parameter is what is typically used for steepest glide
+					// in landing config
+					const float max_glide_slope = tanf(math::radians(param_fw_lnd_ang + 0.1f));
+
+					if (glide_slope > max_glide_slope) {
+
+						const uint8_t land_angle_left_of_decimal = (uint8_t)param_fw_lnd_ang;
+						const uint8_t land_angle_first_after_decimal = (uint8_t)((param_fw_lnd_ang - floorf(
+									param_fw_lnd_ang)) * 10.0f);
+
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+								     "Mission rejected: the landing glide slope is steeper than the vehicle setting of %d.%d degrees.\t",
+								     (int)land_angle_left_of_decimal, (int)land_angle_first_after_decimal);
+						events::send<uint8_t, uint8_t>(events::ID("navigator_mis_glide_slope_too_steep"), {events::Log::Error, events::LogInternal::Info},
+									       "Mission rejected: the landing glide slope is steeper than the vehicle setting of {1}.{2} degrees",
+									       land_angle_left_of_decimal, land_angle_first_after_decimal);
+
+						const uint32_t acceptable_entrance_alt = (uint32_t)(max_glide_slope * landing_approach_distance);
+						const uint32_t acceptable_landing_dist = (uint32_t)ceilf(relative_approach_altitude / max_glide_slope);
+
+						mavlink_log_critical(_navigator->get_mavlink_log_pub(),
+								     "Reduce the glide slope, lower the entrance altitude %d meters, or increase the landing approach distance %d meters.\t",
+								     (int)acceptable_entrance_alt, (int)acceptable_landing_dist);
+						events::send<uint32_t, uint32_t>(events::ID("navigator_mis_correct_glide_slope"), {events::Log::Error, events::LogInternal::Info},
+										 "Reduce the glide slope, lower the entrance altitude {1} meters, or increase the landing approach distance {2} meters",
+										 acceptable_entrance_alt, acceptable_landing_dist);
+
 						return false;
 					}
 
@@ -583,13 +644,6 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
 		}
 	}
 
-	if (land_start_req && !land_start_found) {
-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.\t");
-		events::send(events::ID("navigator_mis_land_missing"), {events::Log::Error, events::LogInternal::Info},
-			     "Mission rejected: landing pattern required");
-		return false;
-	}
-
 	if (land_start_found && (!landing_valid || (do_land_start_index > landing_approach_index))) {
 		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.\t");
 		events::send(events::ID("navigator_mis_invalid_land"), {events::Log::Error, events::LogInternal::Info},
@@ -602,12 +656,11 @@ MissionFeasibilityChecker::checkFixedWingLanding(const mission_s &mission, bool
 }
 
 bool
-MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_start_req)
+MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission)
 {
 	/* Go through all mission items and search for a landing waypoint
 	 * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */
 
-	bool land_start_found = false;
 	size_t do_land_start_index = 0;
 	size_t landing_approach_index = 0;
 
@@ -622,14 +675,14 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_
 
 		// if DO_LAND_START found then require valid landing AFTER
 		if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) {
-			if (land_start_found) {
+			if (_has_landing) {
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: more than one land start.\t");
 				events::send(events::ID("navigator_mis_multi_land"), {events::Log::Error, events::LogInternal::Info},
 					     "Mission rejected: more than one land start commands");
 				return false;
 
 			} else {
-				land_start_found = true;
+				_has_landing = true;
 				do_land_start_index = i;
 			}
 		}
@@ -654,7 +707,7 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_
 			}
 
 		} else if (missionitem.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-			if (land_start_found && do_land_start_index < i) {
+			if (_has_landing && do_land_start_index < i) {
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(),
 						     "Mission rejected: land start item before RTL item not possible.\t");
 				events::send(events::ID("navigator_mis_land_before_rtl2"), {events::Log::Error, events::LogInternal::Info},
@@ -664,14 +717,7 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_
 		}
 	}
 
-	if (land_start_req && !land_start_found) {
-		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: landing pattern required.\t");
-		events::send(events::ID("navigator_mis_land_missing2"), {events::Log::Error, events::LogInternal::Info},
-			     "Mission rejected: landing pattern required");
-		return false;
-	}
-
-	if (land_start_found && (do_land_start_index > landing_approach_index)) {
+	if (_has_landing && (do_land_start_index > landing_approach_index)) {
 		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: invalid land start.\t");
 		events::send(events::ID("navigator_mis_invalid_land2"), {events::Log::Error, events::LogInternal::Info},
 			     "Mission rejected: invalid land start");
@@ -682,6 +728,86 @@ MissionFeasibilityChecker::checkVTOLLanding(const mission_s &mission, bool land_
 	return true;
 }
 
+bool
+MissionFeasibilityChecker::checkTakeoffLandAvailable()
+{
+	bool resTakeoffLandReq = true;
+
+	switch (_navigator->get_takeoff_land_required()) {
+	case 0:
+		resTakeoffLandReq = true;
+		break;
+
+	case 1:
+		resTakeoffLandReq = _has_takeoff || !_navigator->get_land_detected()->landed;
+
+		if (!resTakeoffLandReq) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff item missing.");
+		}
+
+		break;
+
+	case 2:
+		resTakeoffLandReq = _has_landing;
+
+		if (!resTakeoffLandReq) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Landing item missing.");
+		}
+
+		break;
+
+	case 3:
+		resTakeoffLandReq = _has_takeoff && _has_landing;
+
+		if (!resTakeoffLandReq) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Takeoff or Landing item missing.");
+		}
+
+		break;
+
+	case 4:
+		resTakeoffLandReq = _has_takeoff == _has_landing;
+
+		if (!resTakeoffLandReq && (_has_takeoff)) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Add Landing item or remove Takeoff.");
+
+		} else if (!resTakeoffLandReq && (_has_landing)) {
+			mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Add Takeoff item or remove Landing.");
+		}
+
+		break;
+
+	case 5:
+		_navigator->readVtolHomeLandApproachesFromStorage();
+
+		if (!_navigator->get_land_detected()->landed && !_navigator->hasVtolHomeLandApproach()) {
+			resTakeoffLandReq = _has_landing;
+
+			if (!resTakeoffLandReq) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Landing item missing.");
+			}
+
+		} else {
+			resTakeoffLandReq = _has_takeoff == _has_landing;
+
+			if (!resTakeoffLandReq && (_has_takeoff)) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Add Landing item or remove Takeoff.");
+
+			} else if (!resTakeoffLandReq && (_has_landing)) {
+				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Mission rejected: Add Takeoff item or remove Landing.");
+			}
+		}
+
+		break;
+
+	default:
+		resTakeoffLandReq = true;
+		break;
+	}
+
+	return resTakeoffLandReq;
+}
+
 bool
 MissionFeasibilityChecker::checkDistanceToFirstWaypoint(const mission_s &mission, float max_distance)
 {
diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h
index 6091f31347..687fc4f26a 100644
--- a/src/modules/navigator/mission_feasibility_checker.h
+++ b/src/modules/navigator/mission_feasibility_checker.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-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
@@ -44,11 +44,12 @@
 
 #include <dataman/dataman.h>
 #include <uORB/topics/mission.h>
+#include <px4_platform_common/module_params.h>
 
 class Geofence;
 class Navigator;
 
-class MissionFeasibilityChecker
+class MissionFeasibilityChecker: public ModuleParams
 {
 private:
 	Navigator *_navigator{nullptr};
@@ -66,18 +67,23 @@ private:
 	bool checkTakeoff(const mission_s &mission, float home_alt);
 
 	/* Checks specific to fixedwing airframes */
-	bool checkFixedwing(const mission_s &mission, float home_alt, bool land_start_req);
-	bool checkFixedWingLanding(const mission_s &mission, bool land_start_req);
+	bool checkFixedwing(const mission_s &mission, float home_alt);
+	bool checkFixedWingLanding(const mission_s &mission);
 
 	/* Checks specific to rotarywing airframes */
 	bool checkRotarywing(const mission_s &mission, float home_alt);
 
 	/* Checks specific to VTOL airframes */
-	bool checkVTOL(const mission_s &mission, float home_alt, bool land_start_req);
-	bool checkVTOLLanding(const mission_s &mission, bool land_start_req);
+	bool checkVTOL(const mission_s &mission, float home_alt);
+	bool checkVTOLLanding(const mission_s &mission);
+	bool checkTakeoffLandAvailable();
+
+	bool _has_takeoff{false};
+	bool _has_landing{false};
+	bool _has_vtol_takeoff{false};
 
 public:
-	MissionFeasibilityChecker(Navigator *navigator) : _navigator(navigator) {}
+	MissionFeasibilityChecker(Navigator *navigator) : ModuleParams(nullptr), _navigator(navigator) {}
 	~MissionFeasibilityChecker() = default;
 
 	MissionFeasibilityChecker(const MissionFeasibilityChecker &) = delete;
@@ -86,8 +92,6 @@ public:
 	/*
 	 * Returns true if mission is feasible and false otherwise
 	 */
-	bool checkMissionFeasible(const mission_s &mission,
-				  float max_distance_to_1st_waypoint, float max_distance_between_waypoints,
-				  bool land_start_req);
-
+	bool checkMissionFeasible(const mission_s &mission, float max_distance_to_1st_waypoint,
+				  float max_distance_between_waypoints);
 };
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index 3d647d224b..341cf2b118 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014-2016 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2014-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
@@ -58,14 +58,19 @@
 PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 2.5f);
 
 /**
- * Take-off waypoint required
+ * Mission landing required
  *
- * If set, the mission feasibility checker will check for a takeoff waypoint on the mission.
+ * If set, the mission feasibility checker will check for a planned misson landing at the end of the mission.
  *
- * @boolean
+ * @value 0 Do not require a mission takeoff nor a mission landing
+ * @value 1 Require a mission takeoff, do not require a mission landing
+ * @value 2 Do not require a mission takeoff, require a mission landing
+ * @value 3 Require a mission takeoff and a mission landing
+ * @value 4 Require a mission takeoff and a mission landing, or neither of both
+ * @value 5 Same as previous, but require a landing if in air and no valid VTOL landing approach is present
  * @group Mission
  */
-PARAM_DEFINE_INT32(MIS_TAKEOFF_REQ, 0);
+PARAM_DEFINE_INT32(MIS_TKO_LAND_REQ, 0);
 
 /**
  * Minimum Loiter altitude
@@ -156,3 +161,14 @@ PARAM_DEFINE_FLOAT(MIS_YAW_TMT, -1.0f);
  * @group Mission
  */
 PARAM_DEFINE_FLOAT(MIS_YAW_ERR, 12.0f);
+
+/**
+ * Timeout for a successful payload deployment acknowledgement
+ *
+ * @unit s
+ * @min 0
+ * @decimal 1
+ * @increment 1
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_PD_TO, 5.0f);
diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h
index 964ee05405..3ebd22cfae 100644
--- a/src/modules/navigator/navigation.h
+++ b/src/modules/navigator/navigation.h
@@ -33,7 +33,9 @@
 
 /**
  * @file navigation.h
+ *
  * Definition of a mission consisting of mission items.
+ *
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
@@ -66,7 +68,6 @@ enum NAV_CMD {
 	NAV_CMD_LAND = 21,
 	NAV_CMD_TAKEOFF = 22,
 	NAV_CMD_LOITER_TO_ALT = 31,
-	NAV_CMD_DO_FOLLOW_REPOSITION = 33,
 	NAV_CMD_VTOL_TAKEOFF = 84,
 	NAV_CMD_VTOL_LAND = 85,
 	NAV_CMD_DELAY = 93,
@@ -83,6 +84,7 @@ enum NAV_CMD {
 	NAV_CMD_DO_DIGICAM_CONTROL = 203,
 	NAV_CMD_DO_MOUNT_CONFIGURE = 204,
 	NAV_CMD_DO_MOUNT_CONTROL = 205,
+	NAV_CMD_DO_GRIPPER = 211,
 	NAV_CMD_DO_SET_CAM_TRIGG_INTERVAL = 214,
 	NAV_CMD_DO_SET_CAM_TRIGG_DIST = 206,
 	NAV_CMD_OBLIQUE_SURVEY = 260,
@@ -102,7 +104,10 @@ enum NAV_CMD {
 	NAV_CMD_FENCE_POLYGON_VERTEX_EXCLUSION = 5002,
 	NAV_CMD_FENCE_CIRCLE_INCLUSION = 5003,
 	NAV_CMD_FENCE_CIRCLE_EXCLUSION = 5004,
+	NAV_CMD_RALLY_POINT = 5100,
 	NAV_CMD_CONDITION_GATE = 4501,
+	NAV_CMD_WAYPOINT_USER_1 = 31000,
+	NAV_CMD_DO_WINCH = 42600,
 	NAV_CMD_INVALID = UINT16_MAX /* ensure that casting a large number results in a specific error */
 };
 
@@ -127,38 +132,36 @@ enum NAV_FRAME {
 	NAV_FRAME_GLOBAL_TERRAIN_ALT_INT = 11
 };
 
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * Global position setpoint in WGS84 coordinates.
- *
- * This is the position the MAV is heading towards. If it is of type loiter,
- * the MAV is circling around it with the given loiter radius in meters.
- *
- * Corresponds to one of the DM_KEY_WAYPOINTS_OFFBOARD_* dataman items
- */
-
-// Mission Item structure
-//  We explicitly handle struct padding to ensure consistency between in memory and on disk formats across different platforms, toolchains, etc
-//  The use of #pragma pack is avoided to prevent the possibility of unaligned memory accesses.
-
 #if (__GNUC__ >= 5) || __clang__
 //  Disabled in GCC 4.X as the warning doesn't seem to "pop" correctly
 #pragma GCC diagnostic push
 #pragma GCC diagnostic error "-Wpadded"
 #endif // GCC >= 5 || Clang
 
+/**
+ * Mission Item structure
+ *
+ * We explicitly handle struct padding to ensure consistency between in memory and on disk formats
+ * across different platforms, toolchains, etc. The use of #pragma pack is avoided to prevent the
+ * possibility of unaligned memory accesses.
+ */
 struct mission_item_s {
 	double lat;					/**< latitude in degrees				*/
 	double lon;					/**< longitude in degrees				*/
+
+	// Union to support both Mission Item categories in MAVLink such as:
+	// 1. With Global coordinate (param5 ~ 7 corresponds to lat, lon and altitude)
+	// 2. Without global coordinate (when frame = MAV_FRAME_MISSION)
+
+	// Note: the structure and definition of params depends on the nav_cmd, which is
+	// compatible with MAVLink's MAV_CMD enum definitions.
 	union {
+		// Navigation command parameters
 		struct {
 			union {
 				float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
 				float circle_radius;		/**< geofence circle radius in meters (only used for NAV_CMD_NAV_FENCE_CIRCLE*) */
+				bool is_mission_rally_point;	/* only used for NAV_CMD_RALLY_POINT */
 			};
 			float acceptance_radius;		/**< default radius in which the mission is accepted as reached in meters */
 			float loiter_radius;			/**< loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise */
@@ -167,7 +170,9 @@ struct mission_item_s {
 			float ___lon_float;			/**< padding */
 			float altitude;				/**< altitude in meters	(AMSL)			*/
 		};
-		float params[7];				/**< array to store mission command values for MAV_FRAME_MISSION ***/
+
+		// Non-Navigation command parameters (implicit)
+		float params[7];				/**< array to store mission command values with no global coordinates (frame = MAV_FRAME_MISSION) */
 	};
 
 	uint16_t nav_cmd;				/**< navigation command					*/
@@ -224,19 +229,6 @@ struct mission_fence_point_s {
 	uint8_t _padding0[5];				/**< padding struct size to alignment boundary  */
 };
 
-/**
- * Safe Point (Rally Point).
- * Corresponds to the DM_KEY_SAFE_POINTS dataman item
- */
-struct mission_safe_point_s {
-	double lat;
-	double lon;
-	float alt;
-	uint8_t frame;					/**< MAV_FRAME */
-
-	uint8_t _padding0[3];				/**< padding struct size to alignment boundary  */
-};
-
 #if (__GNUC__ >= 5) || __clang__
 #pragma GCC diagnostic pop
 #endif // GCC >= 5 || Clang
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index aeffa50fda..224b07089e 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -41,7 +41,6 @@
 
 #pragma once
 
-#include "follow_target.h"
 #include "geofence.h"
 #include "land.h"
 #include "precland.h"
@@ -51,6 +50,7 @@
 #include "rtl.h"
 #include "takeoff.h"
 #include "vtol_takeoff.h"
+#include "vtol_land.h"
 
 #include "navigation.h"
 
@@ -62,6 +62,7 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/custom_action_status.h>
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/mission.h>
@@ -73,11 +74,13 @@
 #include <uORB/topics/transponder_report.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/vehicle_command_cancel.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/wind.h>
 #include <uORB/uORB.h>
 
 using namespace time_literals;
@@ -87,6 +90,19 @@ using namespace time_literals;
  */
 #define NAVIGATOR_MODE_ARRAY_SIZE 8
 
+struct CustomAction {
+	enum class FailureAction {
+		IGNORE = 0,
+		LOITER = 1,
+		RTL = 2
+	};
+
+	int8_t id{-1};
+	uint64_t timeout;
+	uint64_t start_time;
+	FailureAction failure_action;
+};
+
 class Navigator : public ModuleBase<Navigator>, public ModuleParams
 {
 public:
@@ -119,7 +135,16 @@ public:
 	 */
 	void load_fence_from_file(const char *filename);
 
+	/**
+	 * @brief Publish a given specified vehicle command
+	 *
+	 * Sets the target_component of the vehicle command accordingly depending on the
+	 * vehicle command value (e.g. For Camera control, sets target system component id)
+	 *
+	 * @param vcmd Vehicle command to execute
+	 */
 	void publish_vehicle_cmd(vehicle_command_s *vcmd);
+	void publish_vehicle_cmd_cancel(vehicle_command_cancel_s *vcmd_cancel);
 
 	/**
 	 * Generate an artificial traffic indication
@@ -164,9 +189,15 @@ public:
 	vehicle_land_detected_s     *get_land_detected() { return &_land_detected; }
 	vehicle_local_position_s    *get_local_position() { return &_local_pos; }
 	vehicle_status_s            *get_vstatus() { return &_vstatus; }
+	vehicle_command_ack_s       *get_cmd_ack() { return &_vehicle_cmd_ack; }
+	wind_s                      *get_wind() { return &_wind; }
 
 	PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */
 
+	bool getGroundSpeed(float &ground_speed);	// return true if groundspeed is valid
+
+	terrain::TerrainProvider *getTerrainProvider() { return _terrain_provider; }
+
 	const vehicle_roi_s &get_vroi() { return _vroi; }
 
 	void reset_vroi() { _vroi = {}; }
@@ -193,13 +224,6 @@ public:
 	 */
 	float get_acceptance_radius();
 
-	/**
-	 * Get the default altitude acceptance radius (i.e. from parameters)
-	 *
-	 * @return the distance from the target altitude before considering the waypoint reached
-	 */
-	float get_default_altitude_acceptance_radius();
-
 	/**
 	 * Get the altitude acceptance radius
 	 *
@@ -207,6 +231,15 @@ public:
 	 */
 	float get_altitude_acceptance_radius();
 
+	bool hasVtolHomeLandApproach()
+	{
+		return _vtol_home_land_approaches.isAnyApproachValid();
+	}
+
+	bool isFlyingVtolHomeLandApproach() { return _navigation_mode == &_vtol_land;}
+
+	const land_approaches_s &getVtolHomeLandArea() { return  _vtol_home_land_approaches;}
+
 	/**
 	 * Get the cruising speed
 	 *
@@ -232,6 +265,27 @@ public:
 	 */
 	void reset_cruising_speed();
 
+	/**
+	 * Store cruising speed
+	 *
+	 * For VTOL: store cruising speed for current mode only (multirotor or fixed-wing).
+	 */
+	void store_cruising_speed(float speed);
+
+	/**
+	 * Restore cruising speed from stored value
+	 *
+	 * For VTOL: restore both cruising speeds if speeds are stored.
+	 */
+	void restore_cruising_speed();
+
+	/**
+	 * Reset stored cruising speed to default values
+	 *
+	 * For VTOL: resets both stored cruising speeds.
+	 */
+	void reset_stored_cruising_speed();
+
 	/**
 	 *  Set triplets to invalid
 	 */
@@ -272,12 +326,18 @@ public:
 
 	void set_mission_failure_heading_timeout();
 
+	void setTerrainFollowerState();
+
 	void setMissionLandingInProgress(bool in_progress) { _mission_landing_in_progress = in_progress; }
 
 	bool getMissionLandingInProgress() { return _mission_landing_in_progress; }
 
 	bool is_planned_mission() const { return _navigation_mode == &_mission; }
 
+	bool get_in_custom_action() { return _custom_action.id != -1; }
+	CustomAction get_custom_action() { return _custom_action; }
+	void set_custom_action(const CustomAction &custom_action);
+
 	bool on_mission_landing() { return _mission.landing(); }
 
 	bool start_mission_landing() { return _mission.land_start(); }
@@ -294,6 +354,9 @@ public:
 	double get_mission_landing_lon() { return _mission.get_landing_lon(); }
 	float  get_mission_landing_alt() { return _mission.get_landing_alt(); }
 
+	TerrainFollowerWrapper &getTerrainFollower() { return _terrain_follower; }
+	float get_mission_landing_loiter_radius() { return _mission.get_landing_loiter_rad(); }
+
 	// RTL
 	bool mission_landing_required() { return _rtl.get_rtl_type() == RTL::RTL_TYPE_MISSION_LANDING; }
 
@@ -306,7 +369,7 @@ public:
 	// Param access
 	float get_loiter_min_alt() const { return _param_mis_ltrmin_alt.get(); }
 	float get_takeoff_min_alt() const { return _param_mis_takeoff_alt.get(); }
-	bool  get_takeoff_required() const { return _param_mis_takeoff_req.get(); }
+	int   get_takeoff_land_required() const { return _para_mis_takeoff_land_req.get(); }
 	float get_yaw_timeout() const { return _param_mis_yaw_tmt.get(); }
 	float get_yaw_threshold() const { return math::radians(_param_mis_yaw_err.get()); }
 	float get_lndmc_alt_max() const { return _param_lndmc_alt_max.get(); }
@@ -318,10 +381,23 @@ public:
 
 	void acquire_gimbal_control();
 	void release_gimbal_control();
+	void set_gimbal_neutral();
 
-	void 		calculate_breaking_stop(double &lat, double &lon, float &yaw);
+	void stop_capturing_images();
+	void disable_camera_trigger();
+
+	void calculate_breaking_stop(double &lat, double &lon, float &yaw);
+
+	void readVtolHomeLandApproachesFromStorage();
 
 private:
+	void handle_custom_action_failure();
+	void handle_custom_action_success();
+	void switch_to_auto_mission_mode();
+	void switch_to_auto_rtl_mode();
+	void switch_to_auto_loiter_mode();
+	void reset_custom_action();
+	void publish_custom_action_status(uint8_t status);
 
 	struct traffic_buffer_s {
 		uint32_t 	icao_address;
@@ -343,13 +419,17 @@ private:
 	uORB::Subscription _pos_ctrl_landing_status_sub{ORB_ID(position_controller_landing_status)};	/**< position controller landing status subscription */
 	uORB::Subscription _traffic_sub{ORB_ID(transponder_report)};		/**< traffic subscription */
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};	/**< vehicle commands (onboard and offboard) */
+	uORB::Subscription _vehicle_cmd_ack_sub{ORB_ID(vehicle_command_ack)};	/**< vehicle command acks (onboard and offboard) */
+	uORB::Subscription _wind_sub{ORB_ID(wind)};
 
 	uORB::Publication<geofence_result_s>		_geofence_result_pub{ORB_ID(geofence_result)};
 	uORB::Publication<mission_result_s>		_mission_result_pub{ORB_ID(mission_result)};
 	uORB::Publication<position_setpoint_triplet_s>	_pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)};
 	uORB::Publication<vehicle_command_ack_s>	_vehicle_cmd_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<vehicle_command_cancel_s>	_vehicle_cmd_cancel_pub{ORB_ID(vehicle_command_cancel)};
 	uORB::Publication<vehicle_command_s>		_vehicle_cmd_pub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_roi_s>		_vehicle_roi_pub{ORB_ID(vehicle_roi)};
+	uORB::Publication<custom_action_status_s> _custom_action_status_pub{ORB_ID(custom_action_status)};
 
 	orb_advert_t	_mavlink_log_pub{nullptr};	/**< the uORB advert to send messages over mavlink */
 
@@ -361,6 +441,8 @@ private:
 	vehicle_land_detected_s				_land_detected{};	/**< vehicle land_detected */
 	vehicle_local_position_s			_local_pos{};		/**< local vehicle position */
 	vehicle_status_s				_vstatus{};		/**< vehicle status */
+	vehicle_command_ack_s			_vehicle_cmd_ack{};		/**< vehicle command_ack */
+	wind_s 				_wind{};
 
 	uint8_t						_previous_nav_state{}; /**< nav_state of the previous iteration*/
 
@@ -374,10 +456,10 @@ private:
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	Geofence	_geofence;			/**< class that handles the geofence */
-
 	GeofenceBreachAvoidance _gf_breach_avoidance;
-
 	hrt_abstime _last_geofence_check = 0;
+	terrain::TerrainProvider *_terrain_provider{nullptr};
+	TerrainFollowerWrapper _terrain_follower;
 
 	bool		_geofence_violation_warning_sent{false};	/**< prevents spaming to mavlink */
 	bool		_can_loiter_at_sp{false};			/**< flags if current position SP can be used to loiter */
@@ -385,14 +467,18 @@ private:
 	bool 		_pos_sp_triplet_published_invalid_once{false};	/**< flags if position SP triplet has been published once to UORB */
 	bool		_mission_result_updated{false};			/**< flags if mission result has seen an update */
 
+	CustomAction	_custom_action{};			/**< current custom action **/
+
+	bool 		_use_vtol_land_navigation_mode_for_rtl = false;
+
 	Mission		_mission;			/**< class that handles the missions */
 	Loiter		_loiter;			/**< class that handles loiter */
 	Takeoff		_takeoff;			/**< class for handling takeoff commands */
 	VtolTakeoff	_vtol_takeoff;			/**< class for handling VEHICLE_CMD_NAV_VTOL_TAKEOFF command */
+	VtolLand	_vtol_land;				/**< class that handles VTOL landing */
 	Land		_land;			/**< class for handling land commands */
 	PrecLand	_precland;			/**< class for handling precision land commands */
 	RTL 		_rtl;				/**< class that handles RTL */
-	FollowTarget	_follow_target;
 
 	NavigatorMode *_navigation_mode{nullptr};	/**< abstract pointer to current navigation mode class */
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE] {};	/**< array of navigation modes */
@@ -409,12 +495,20 @@ private:
 
 	float _mission_cruising_speed_mc{-1.0f};
 	float _mission_cruising_speed_fw{-1.0f};
+	float _mission_stored_cruising_speed_mc{-1.0f};
+	float _mission_stored_cruising_speed_fw{-1.0f};
 	float _mission_throttle{NAN};
 
 	bool _mission_landing_in_progress{false};	/**< this flag gets set if the mission is currently executing on a landing pattern
 							 * if mission mode is inactive, this flag will be cleared after 2 seconds */
 
-	traffic_buffer_s _traffic_buffer{};
+	bool _is_capturing_images{false}; // keep track if we need to stop capturing images
+
+	traffic_buffer_s _traffic_buffer;
+
+	static constexpr float MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES =
+		10.0; // [m] We don't consider home land approaches valid if the distance from the current home to the land location is greater than this distance
+	land_approaches_s _vtol_home_land_approaches{};
 
 	// update subscriptions
 	void params_update();
@@ -432,6 +526,7 @@ private:
 	void publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_t result);
 
 	bool geofence_allows_position(const vehicle_global_position_s &pos);
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::NAV_LOITER_RAD>)   _param_nav_loiter_rad,	/**< loiter radius for fixedwing */
 		(ParamFloat<px4::params::NAV_ACC_RAD>)      _param_nav_acc_rad,		/**< acceptance for takeoff */
@@ -444,14 +539,14 @@ private:
 		(ParamFloat<px4::params::NAV_TRAFF_A_RADU>) _param_nav_traff_a_radu,	/**< avoidance Distance Unmanned*/
 		(ParamFloat<px4::params::NAV_TRAFF_A_RADM>) _param_nav_traff_a_radm,	/**< avoidance Distance Manned*/
 
-		// non-navigator parameters
-		// Mission (MIS_*)
+		// non-navigator parameters: Mission (MIS_*)
 		(ParamFloat<px4::params::MIS_LTRMIN_ALT>)  _param_mis_ltrmin_alt,
 		(ParamFloat<px4::params::MIS_TAKEOFF_ALT>) _param_mis_takeoff_alt,
-		(ParamBool<px4::params::MIS_TAKEOFF_REQ>)  _param_mis_takeoff_req,
+		(ParamInt<px4::params::MIS_TKO_LAND_REQ>)  _para_mis_takeoff_land_req,
 		(ParamFloat<px4::params::MIS_YAW_TMT>)     _param_mis_yaw_tmt,
 		(ParamFloat<px4::params::MIS_YAW_ERR>)     _param_mis_yaw_err,
-		(ParamFloat<px4::params::LNDMC_ALT_MAX>)   _param_lndmc_alt_max
-
+		(ParamFloat<px4::params::LNDMC_ALT_MAX>)   _param_lndmc_alt_max,
+		(ParamInt<px4::params::TF_TERRAIN_EN>)     _param_tf_terrain_en,
+		(ParamFloat<px4::params::MIS_PD_TO>)       _param_mis_payload_delivery_timeout
 	)
 };
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index ee52ed4a90..a8aa53faaa 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -52,12 +52,14 @@
 #include <drivers/drv_hrt.h>
 #include <lib/geo/geo.h>
 #include <lib/mathlib/mathlib.h>
+#include <commander/px4_custom_mode.h>
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/defines.h>
 #include <px4_platform_common/events.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/tasks.h>
 #include <systemlib/mavlink_log.h>
+#include <uORB/topics/gimbal_manager_set_attitude.h>
 
 using namespace time_literals;
 
@@ -71,14 +73,14 @@ Navigator::Navigator() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
 	_geofence(this),
 	_gf_breach_avoidance(this),
-	_mission(this),
+	_mission(this, _terrain_follower),
 	_loiter(this),
 	_takeoff(this),
 	_vtol_takeoff(this),
+	_vtol_land(this),
 	_land(this),
 	_precland(this),
-	_rtl(this),
-	_follow_target(this)
+	_rtl(this, _terrain_follower)
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
@@ -88,7 +90,14 @@ Navigator::Navigator() :
 	_navigation_mode_array[4] = &_land;
 	_navigation_mode_array[5] = &_precland;
 	_navigation_mode_array[6] = &_vtol_takeoff;
-	_navigation_mode_array[7] = &_follow_target;
+	_navigation_mode_array[7] = &_vtol_land;
+
+	/* iterate through navigation modes and initialize _mission_item for each */
+	for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
+		if (_navigation_mode_array[i]) {
+			_navigation_mode_array[i]->initialize();
+		}
+	}
 
 	_handle_back_trans_dec_mss = param_find("VT_B_DEC_MSS");
 	_handle_reverse_delay = param_find("VT_B_REV_DEL");
@@ -100,6 +109,9 @@ Navigator::Navigator() :
 	_mission_sub = orb_subscribe(ORB_ID(mission));
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
+	// Update the timeout used in mission_block (which can't hold it's own parameters)
+	_mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get());
+
 	reset_triplets();
 }
 
@@ -109,12 +121,16 @@ Navigator::~Navigator()
 	orb_unsubscribe(_local_pos_sub);
 	orb_unsubscribe(_mission_sub);
 	orb_unsubscribe(_vehicle_status_sub);
+
+	delete _terrain_provider;
 }
 
 void Navigator::params_update()
 {
 	updateParams();
 
+	_terrain_follower.updateParams();
+
 	if (_handle_back_trans_dec_mss != PARAM_INVALID) {
 		param_get(_handle_back_trans_dec_mss, &_param_back_trans_dec_mss);
 	}
@@ -130,6 +146,8 @@ void Navigator::params_update()
 	if (_handle_mpc_acc_hor != PARAM_INVALID) {
 		param_get(_handle_mpc_acc_hor, &_param_mpc_acc_hor);
 	}
+
+	_mission.set_payload_deployment_timeout(_param_mis_payload_delivery_timeout.get());
 }
 
 void Navigator::run()
@@ -147,6 +165,14 @@ void Navigator::run()
 
 	params_update();
 
+	orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vstatus);
+
+	if (_param_tf_terrain_en.get() > 0 && _vstatus.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROVER) {
+		_terrain_provider = new terrain::TerrainProvider(_param_tf_terrain_en.get());
+		_terrain_follower.enable();
+		_terrain_follower.setTerrainProvider(_terrain_provider);
+	}
+
 	/* wakeup source(s) */
 	px4_pollfd_struct_t fds[3] {};
 
@@ -205,7 +231,7 @@ void Navigator::run()
 			}
 		}
 
-		// check for parameter updates
+		/* check for parameter updates */
 		if (_parameter_update_sub.updated()) {
 			// clear update
 			parameter_update_s pupdate;
@@ -218,9 +244,62 @@ void Navigator::run()
 		_land_detected_sub.update(&_land_detected);
 		_position_controller_status_sub.update();
 		_home_pos_sub.update(&_home_pos);
+		_wind_sub.update(&_wind);
 
+		if (_vehicle_cmd_ack_sub.update(&_vehicle_cmd_ack) &&
+		    _vehicle_cmd_ack.command == vehicle_command_s::VEHICLE_CMD_NAV_WAYPOINT_USER_1 &&
+		    _custom_action.id != -1) {
+
+			// Custom action not started yet?
+			if (_custom_action.start_time == 0) {
+				_custom_action.start_time = hrt_absolute_time();
+				publish_custom_action_status(custom_action_status_s::STATUS_ACTION_STARTED);
+			}
+
+			if (_vehicle_cmd_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
+				mavlink_log_info(get_mavlink_log_pub(), "Custom action #%i finished successfully. Continuing mission...",
+						 _custom_action.id);
+				handle_custom_action_success();
+				publish_custom_action_status(custom_action_status_s::STATUS_ACTION_SUCCEEDED);
+
+			} else if (_vehicle_cmd_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED) {
+				mavlink_log_warning(get_mavlink_log_pub(), "Custom action #%i failed to be processed / executed.",
+						    _custom_action.id);
+				handle_custom_action_failure();
+				publish_custom_action_status(custom_action_status_s::STATUS_ACTION_FAILED);
+
+			} else if (_vehicle_cmd_ack.result == vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED) {
+				mavlink_log_warning(get_mavlink_log_pub(), "Custom action #%i cancelled.",
+						    _custom_action.id);
+				handle_custom_action_failure();
+				publish_custom_action_status(custom_action_status_s::STATUS_ACTION_CANCELLED);
+
+			}
+		}
+
+		// In case of timneout, send request to cancel the custom action
+		if (_custom_action.start_time > 0 && _custom_action.timeout > 0
+		    && (hrt_absolute_time() - _custom_action.start_time) >= _custom_action.timeout) {
+			mavlink_log_warning(get_mavlink_log_pub(), "Custom action #%i timed out.",
+					    _custom_action.id);
+
+			// send message to cancel the action process on the external system
+			// processing the action. Note that the external system component
+			// should be identified as MAV_COMP_ID_ONBOARD_COMPUTER3
+			vehicle_command_cancel_s vcmd_cancel = {};
+			vcmd_cancel.command = vehicle_command_s::VEHICLE_CMD_NAV_WAYPOINT_USER_1;
+			vcmd_cancel.target_system = _vstatus.system_id;;
+			vcmd_cancel.target_component = 193; // MAV_COMP_ID_ONBOARD_COMPUTER3
+			publish_vehicle_cmd_cancel(&vcmd_cancel);
+			handle_custom_action_failure();
+			publish_custom_action_status(custom_action_status_s::STATUS_ACTION_TIMED_OUT);
+
+		}
+
+		// Handle Vehicle commands
 		while (_vehicle_command_sub.updated()) {
 			const unsigned last_generation = _vehicle_command_sub.get_last_generation();
+
 			vehicle_command_s cmd{};
 			_vehicle_command_sub.copy(&cmd);
 
@@ -232,7 +311,7 @@ void Navigator::run()
 
 				// DO_GO_AROUND is currently handled by the position controller (unacknowledged)
 				// TODO: move DO_GO_AROUND handling to navigator
-				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION
 				   && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
@@ -242,8 +321,16 @@ void Navigator::run()
 				bool reposition_valid = true;
 
 				vehicle_global_position_s position_setpoint{};
-				position_setpoint.lat = cmd.param5;
-				position_setpoint.lon = cmd.param6;
+
+				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
+					position_setpoint.lat = cmd.param5;
+					position_setpoint.lon = cmd.param6;
+
+				} else {
+					position_setpoint.lat = get_global_position()->lat;
+					position_setpoint.lon = get_global_position()->lon;
+				}
+
 				position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt;
 
 				if (have_geofence_position_data) {
@@ -323,7 +410,7 @@ void Navigator::run()
 					}
 
 					if (only_alt_change_requested) {
-						if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > 0) {
+						if (PX4_ISFINITE(curr->current.loiter_radius) && curr->current.loiter_radius > FLT_EPSILON) {
 							rep->current.loiter_radius = curr->current.loiter_radius;
 
 
@@ -331,12 +418,28 @@ void Navigator::run()
 							rep->current.loiter_radius = get_loiter_radius();
 						}
 
-						if (curr->current.loiter_direction == 1 || curr->current.loiter_direction == -1) {
-							rep->current.loiter_direction = curr->current.loiter_direction;
+						if (PX4_ISFINITE(curr->current.loiter_minor_radius) && fabsf(curr->current.loiter_minor_radius) > FLT_EPSILON) {
+							rep->current.loiter_minor_radius = curr->current.loiter_minor_radius;
 
 						} else {
-							rep->current.loiter_direction = 1;
+							rep->current.loiter_minor_radius = NAN;
 						}
+
+						if (PX4_ISFINITE(curr->current.loiter_orientation) && fabsf(curr->current.loiter_minor_radius) > FLT_EPSILON) {
+							rep->current.loiter_orientation = curr->current.loiter_orientation;
+
+						} else {
+							rep->current.loiter_orientation = 0.0f;
+						}
+
+						if (curr->current.loiter_pattern > 0) {
+							rep->current.loiter_pattern = curr->current.loiter_pattern;
+
+						} else {
+							rep->current.loiter_pattern = position_setpoint_s::LOITER_TYPE_ORBIT;
+						}
+
+						rep->current.loiter_direction_counter_clockwise = curr->current.loiter_direction_counter_clockwise;
 					}
 
 					rep->previous.timestamp = hrt_absolute_time();
@@ -374,12 +477,14 @@ void Navigator::run()
 					position_setpoint_triplet_s *rep = get_reposition_triplet();
 					rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
 					rep->current.loiter_radius = get_loiter_radius();
-					rep->current.loiter_direction = 1;
+					rep->current.loiter_direction_counter_clockwise = false;
+					rep->current.loiter_orientation = 0.0f;
+					rep->current.loiter_pattern = position_setpoint_s::LOITER_TYPE_ORBIT;
 					rep->current.cruising_throttle = get_cruising_throttle();
 
 					if (PX4_ISFINITE(cmd.param1)) {
 						rep->current.loiter_radius = fabsf(cmd.param1);
-						rep->current.loiter_direction = math::signNoZero(cmd.param1);
+						rep->current.loiter_direction_counter_clockwise = cmd.param1 < 0;
 					}
 
 					rep->current.lat = position_setpoint.lat;
@@ -393,6 +498,58 @@ void Navigator::run()
 					mavlink_log_critical(&_mavlink_log_pub, "Orbit is outside geofence");
 				}
 
+			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_FIGUREEIGHT &&
+				   get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+				// Only valid for fixed wing mode
+
+				bool orbit_location_valid = true;
+
+				vehicle_global_position_s position_setpoint{};
+				position_setpoint.lat = PX4_ISFINITE(cmd.param5) ? cmd.param5 : get_global_position()->lat;
+				position_setpoint.lon = PX4_ISFINITE(cmd.param6) ? cmd.param6 : get_global_position()->lon;
+				position_setpoint.alt = PX4_ISFINITE(cmd.param7) ? cmd.param7 : get_global_position()->alt;
+
+				if (have_geofence_position_data) {
+					orbit_location_valid = geofence_allows_position(position_setpoint);
+				}
+
+				if (orbit_location_valid) {
+					position_setpoint_triplet_s *rep = get_reposition_triplet();
+					rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
+					rep->current.loiter_minor_radius = fabsf(get_loiter_radius());
+					rep->current.loiter_direction_counter_clockwise = get_loiter_radius() < 0;
+					rep->current.loiter_orientation = 0.0f;
+					rep->current.loiter_pattern = position_setpoint_s::LOITER_TYPE_FIGUREEIGHT;
+					rep->current.cruising_speed = get_cruising_speed();
+
+					if (PX4_ISFINITE(cmd.param2) && fabsf(cmd.param2) > FLT_EPSILON) {
+						rep->current.loiter_minor_radius = fabsf(cmd.param2);
+					}
+
+					rep->current.loiter_radius = 2.5f * rep->current.loiter_minor_radius;
+
+					if (PX4_ISFINITE(cmd.param1)) {
+						rep->current.loiter_radius = fabsf(cmd.param1);
+						rep->current.loiter_direction_counter_clockwise = cmd.param1 < 0;
+					}
+
+					rep->current.loiter_radius = math::max(rep->current.loiter_radius, 2.0f * rep->current.loiter_minor_radius);
+
+					if (PX4_ISFINITE(cmd.param4)) {
+						rep->current.loiter_orientation = cmd.param4;
+					}
+
+					rep->current.lat = position_setpoint.lat;
+					rep->current.lon = position_setpoint.lon;
+					rep->current.alt = position_setpoint.alt;
+
+					rep->current.valid = true;
+					rep->current.timestamp = hrt_absolute_time();
+
+				} else {
+					mavlink_log_critical(&_mavlink_log_pub, "Figure 8 is outside geofence");
+				}
+
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
 				position_setpoint_triplet_s *rep = get_takeoff_triplet();
 
@@ -403,7 +560,7 @@ void Navigator::run()
 				rep->previous.alt = get_global_position()->alt;
 
 				rep->current.loiter_radius = get_loiter_radius();
-				rep->current.loiter_direction = 1;
+				rep->current.loiter_direction_counter_clockwise = false;
 				rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
 
 				if (home_global_position_valid()) {
@@ -428,6 +585,7 @@ void Navigator::run()
 					// If one of them is non-finite set the current global position as target
 					rep->current.lat = get_global_position()->lat;
 					rep->current.lon = get_global_position()->lon;
+
 				}
 
 				rep->current.alt = cmd.param7;
@@ -464,7 +622,7 @@ void Navigator::run()
 					PX4_WARN("planned mission landing not available");
 				}
 
-				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
 				if (_mission_result.valid && PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0)) {
@@ -479,6 +637,7 @@ void Navigator::run()
 				if (cmd.param2 > FLT_EPSILON) {
 					// XXX not differentiating ground and airspeed yet
 					set_cruising_speed(cmd.param2);
+					store_cruising_speed(cmd.param2);
 
 				} else {
 					set_cruising_speed();
@@ -493,7 +652,7 @@ void Navigator::run()
 				}
 
 				// TODO: handle responses for supported DO_CHANGE_SPEED options?
-				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI
 				   || cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_ROI
@@ -535,19 +694,13 @@ void Navigator::run()
 
 				_vehicle_roi_pub.publish(_vroi);
 
-				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
+				publish_vehicle_command_ack(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);
 
 			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION
 				   && get_vstatus()->nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF) {
 				// reset cruise speed and throttle to default when transitioning (VTOL Takeoff handles it separately)
 				reset_cruising_speed();
 				set_cruising_throttle();
-
-				// need to update current setpooint with reset cruise speed and throttle
-				position_setpoint_triplet_s *rep = get_reposition_triplet();
-				*rep = *(get_position_setpoint_triplet());
-				rep->current.cruising_speed = get_cruising_speed();
-				rep->current.cruising_throttle = get_cruising_throttle();
 			}
 		}
 
@@ -579,99 +732,114 @@ void Navigator::run()
 
 				const bool rtl_activated = _previous_nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
 
-				switch (_rtl.get_rtl_type()) {
-				case RTL::RTL_TYPE_MISSION_LANDING:
-				case RTL::RTL_TYPE_CLOSEST:
+				if (rtl_activated) {
+					_use_vtol_land_navigation_mode_for_rtl = _vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
+					readVtolHomeLandApproachesFromStorage();
+				}
 
-					if (!rtl_activated && _rtl.getClimbAndReturnDone()
-					    && _rtl.getShouldEngageMissionForLanding()) {
-						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
+				if (hasVtolHomeLandApproach() && _use_vtol_land_navigation_mode_for_rtl && _rtl.getDestinationTypeHomeLanding()) {
+					if (_navigation_mode == &_vtol_takeoff && !get_mission_result()->finished) {
+						// if we are in the middle of a vtol takeoff then let's wait until we have established the loiter
+						navigation_mode_new = &_vtol_takeoff;
 
-						if (!getMissionLandingInProgress() && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED
-						    && !get_land_detected()->landed) {
-							start_mission_landing();
-						}
-
-						navigation_mode_new = &_mission;
+					} else if (!rtl_activated && _rtl.getRTLState() > RTL::RTLState::RTL_STATE_CLIMB) {
+						navigation_mode_new = &_vtol_land;
 
 					} else {
 						navigation_mode_new = &_rtl;
 					}
 
-					break;
+				} else {
 
-				case RTL::RTL_TYPE_MISSION_LANDING_REVERSED:
-					if (_mission.get_land_start_available() && !get_land_detected()->landed) {
-						// the mission contains a landing spot
-						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
+					switch (_rtl.get_rtl_type()) {
+					case RTL::RTL_TYPE_MISSION_LANDING:
+					case RTL::RTL_TYPE_CLOSEST:
 
-						if (_navigation_mode != &_mission) {
-							if (_navigation_mode == nullptr) {
-								// switching from an manual mode, go to landing if not already landing
-								if (!on_mission_landing()) {
-									start_mission_landing();
-								}
+						if (!rtl_activated && _rtl.getRTLState() > RTL::RTLState::RTL_STATE_LOITER && _rtl.getShouldEngageMissionForLanding()) {
+							_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
 
-							} else {
-								// switching from an auto mode, continue the mission from the closest item
-								_mission.set_closest_item_as_current();
-							}
-						}
-
-						if (rtl_activated) {
-							mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission\t");
-							events::send(events::ID("navigator_rtl_mission_activated"), events::Log::Info,
-								     "RTL Mission activated, continue mission");
-						}
-
-						navigation_mode_new = &_mission;
-
-					} else {
-						// fly the mission in reverse if switching from a non-manual mode
-						_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_REVERSE);
-
-						if ((_navigation_mode != nullptr && (_navigation_mode != &_rtl || _mission.get_mission_changed())) &&
-						    (! _mission.get_mission_finished()) &&
-						    (!get_land_detected()->landed)) {
-							// determine the closest mission item if switching from a non-mission mode, and we are either not already
-							// mission mode or the mission waypoints changed.
-							// The seconds condition is required so that when no mission was uploaded and one is available the closest
-							// mission item is determined and also that if the user changes the active mission index while rtl is active
-							// always that waypoint is tracked first.
-							if ((_navigation_mode != &_mission) && (rtl_activated || _mission.get_mission_waypoints_changed())) {
-								_mission.set_closest_item_as_current();
-							}
-
-							if (rtl_activated) {
-								mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse\t");
-								events::send(events::ID("navigator_rtl_mission_activated_rev"), events::Log::Info,
-									     "RTL Mission activated, fly mission in reverse");
+							if (!getMissionLandingInProgress() && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED
+							    && !get_land_detected()->landed) {
+								start_mission_landing();
 							}
 
 							navigation_mode_new = &_mission;
 
 						} else {
-							if (rtl_activated) {
-								mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home\t");
-								events::send(events::ID("navigator_rtl_mission_activated_home"), events::Log::Info,
-									     "RTL Mission activated, fly to home");
-							}
-
 							navigation_mode_new = &_rtl;
 						}
+
+						break;
+
+					case RTL::RTL_TYPE_MISSION_LANDING_REVERSED:
+						if (_mission.get_land_start_available() && !get_land_detected()->landed) {
+							// the mission contains a landing spot
+							_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD);
+
+							if (_navigation_mode != &_mission) {
+								if (_navigation_mode == nullptr) {
+									// switching from an manual mode, go to landing if not already landing
+									if (!on_mission_landing()) {
+										start_mission_landing();
+									}
+
+								} else {
+									// switching from an auto mode, continue the mission from the closest item
+									_mission.set_closest_item_as_current();
+								}
+							}
+
+							if (rtl_activated) {
+								mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, continue mission\t");
+								events::send(events::ID("navigator_rtl_mission_activated"), events::Log::Info,
+									     "RTL Mission activated, continue mission");
+							}
+
+							navigation_mode_new = &_mission;
+
+						} else {
+							// fly the mission in reverse if switching from a non-manual mode
+							_mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_REVERSE);
+
+							if ((_navigation_mode != nullptr && (_navigation_mode != &_rtl || _mission.get_mission_changed())) &&
+							    (! _mission.get_mission_finished()) &&
+							    (!get_land_detected()->landed)) {
+								// determine the closest mission item if switching from a non-mission mode, and we are either not already
+								// mission mode or the mission waypoints changed.
+								// The seconds condition is required so that when no mission was uploaded and one is available the closest
+								// mission item is determined and also that if the user changes the active mission index while rtl is active
+								// always that waypoint is tracked first.
+								if ((_navigation_mode != &_mission) && (rtl_activated || _mission.get_mission_waypoints_changed())) {
+									_mission.set_closest_item_as_current();
+								}
+
+								if (rtl_activated) {
+									mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly mission in reverse");
+								}
+
+								navigation_mode_new = &_mission;
+
+							} else {
+								if (rtl_activated) {
+									mavlink_log_info(get_mavlink_log_pub(), "RTL Mission activated, fly to home");
+								}
+
+								navigation_mode_new = &_rtl;
+							}
+						}
+
+						break;
+
+					default:
+						if (rtl_activated) {
+							mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated");
+						}
+
+						navigation_mode_new = &_rtl;
+						break;
+
 					}
 
-					break;
-
-				default:
-					if (rtl_activated) {
-						mavlink_log_info(get_mavlink_log_pub(), "RTL HOME activated\t");
-						events::send(events::ID("navigator_rtl_home_activated"), events::Log::Info, "RTL activated");
-					}
-
-					navigation_mode_new = &_rtl;
-					break;
-
 				}
 
 				break;
@@ -698,11 +866,6 @@ void Navigator::run()
 			_precland.set_mode(PrecLandMode::Required);
 			break;
 
-		case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
-			_pos_sp_triplet_published_invalid_once = false;
-			navigation_mode_new = &_follow_target;
-			break;
-
 		case vehicle_status_s::NAVIGATION_STATE_MANUAL:
 		case vehicle_status_s::NAVIGATION_STATE_ACRO:
 		case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
@@ -711,6 +874,7 @@ void Navigator::run()
 		case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
 		case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
 		case vehicle_status_s::NAVIGATION_STATE_STAB:
+
 		default:
 			navigation_mode_new = nullptr;
 			_can_loiter_at_sp = false;
@@ -720,6 +884,7 @@ void Navigator::run()
 		// Do not execute any state machine while we are disarmed
 		if (_vstatus.arming_state != vehicle_status_s::ARMING_STATE_ARMED) {
 			navigation_mode_new = nullptr;
+
 		}
 
 		// update the vehicle status
@@ -795,7 +960,19 @@ void Navigator::run()
 
 void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 {
+	bool on_home_land_approach = false;
+
+	if (isFlyingVtolHomeLandApproach() && _home_pos.valid_hpos) {
+		// if we are executing a vtol land close to the home positon we ignore the geofence
+		// otherwise it will disrupt the landing and cause a dangerous situation
+		// add 1.1 factor safety margin
+		const float dist_to_home = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _home_pos.lat,
+					   _home_pos.lon);
+		on_home_land_approach = dist_to_home < 1.1f * _vtol_home_land_approaches.getMaxDistLandToLoiterCircle();
+	}
+
 	if (have_geofence_position_data &&
+	    !on_home_land_approach &&
 	    (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
 	    (hrt_elapsed_time(&_last_geofence_check) > GEOFENCE_CHECK_INTERVAL_US)) {
 
@@ -911,7 +1088,6 @@ void Navigator::geofence_breach_check(bool &have_geofence_position_data)
 					rep->current.loiter_radius = get_loiter_radius();
 					rep->current.alt_valid = true;
 					rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
-					rep->current.loiter_direction = 1;
 					rep->current.cruising_throttle = get_cruising_throttle();
 					rep->current.acceptance_radius = get_acceptance_radius();
 					rep->current.cruising_speed = get_cruising_speed();
@@ -938,7 +1114,7 @@ int Navigator::task_spawn(int argc, char *argv[])
 	_task_id = px4_task_spawn_cmd("navigator",
 				      SCHED_DEFAULT,
 				      SCHED_PRIORITY_NAVIGATION,
-				      PX4_STACK_ADJUSTED(1952),
+				      PX4_STACK_ADJUSTED(2100),
 				      (px4_main_t)&run_trampoline,
 				      (char *const *)argv);
 
@@ -981,10 +1157,18 @@ float Navigator::get_default_acceptance_radius()
 	return _param_nav_acc_rad.get();
 }
 
-float Navigator::get_default_altitude_acceptance_radius()
+float Navigator::get_altitude_acceptance_radius()
 {
 	if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-		return _param_nav_fw_alt_rad.get();
+		const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next;
+
+		if (!force_vtol() && next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) {
+			// Use separate (tighter) altitude acceptance for clean altitude starting point before FW landing
+			return _param_nav_fw_altl_rad.get();
+
+		} else {
+			return _param_nav_fw_alt_rad.get();
+		}
 
 	} else if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROVER) {
 		return INFINITY;
@@ -1003,20 +1187,6 @@ float Navigator::get_default_altitude_acceptance_radius()
 	}
 }
 
-float Navigator::get_altitude_acceptance_radius()
-{
-	if (get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
-		const position_setpoint_s &next_sp = get_position_setpoint_triplet()->next;
-
-		if (next_sp.type == position_setpoint_s::SETPOINT_TYPE_LAND && next_sp.valid) {
-			// Use separate (tighter) altitude acceptance for clean altitude starting point before landing
-			return _param_nav_fw_altl_rad.get();
-		}
-	}
-
-	return get_default_altitude_acceptance_radius();
-}
-
 float Navigator::get_cruising_speed()
 {
 	/* there are three options: The mission-requested cruise speed, or the current hover / plane speed */
@@ -1038,6 +1208,16 @@ float Navigator::get_cruising_speed()
 	}
 }
 
+bool Navigator::getGroundSpeed(float &ground_speed)
+{
+	if (_local_pos.v_xy_valid) {
+		ground_speed = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
+		return true;
+	}
+
+	return false;
+}
+
 void Navigator::set_cruising_speed(float speed)
 {
 	if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
@@ -1054,6 +1234,35 @@ void Navigator::reset_cruising_speed()
 	_mission_cruising_speed_fw = -1.0f;
 }
 
+void Navigator::store_cruising_speed(float speed)
+{
+	if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
+		_mission_stored_cruising_speed_mc = speed;
+
+	} else {
+		_mission_stored_cruising_speed_fw = speed;
+	}
+}
+
+void Navigator::restore_cruising_speed()
+{
+	if (_mission_stored_cruising_speed_fw > 0.0f) {
+
+		_mission_cruising_speed_fw = _mission_stored_cruising_speed_fw;
+	}
+
+	if (_mission_stored_cruising_speed_mc > 0.0f) {
+
+		_mission_cruising_speed_mc = _mission_stored_cruising_speed_mc;
+	}
+}
+
+void Navigator::reset_stored_cruising_speed()
+{
+	_mission_stored_cruising_speed_fw = -1.0f;
+	_mission_stored_cruising_speed_mc = -1.0f;
+}
+
 void Navigator::reset_triplets()
 {
 	reset_position_setpoint(_pos_sp_triplet.previous);
@@ -1061,6 +1270,10 @@ void Navigator::reset_triplets()
 	reset_position_setpoint(_pos_sp_triplet.next);
 
 	_pos_sp_triplet_updated = true;
+	_pos_sp_triplet.previous.acceptance_radius = get_default_acceptance_radius();
+	_pos_sp_triplet.current.acceptance_radius = get_default_acceptance_radius();
+	_pos_sp_triplet.next.acceptance_radius = get_default_acceptance_radius();
+
 }
 
 void Navigator::reset_position_setpoint(position_setpoint_s &sp)
@@ -1076,11 +1289,12 @@ void Navigator::reset_position_setpoint(position_setpoint_s &sp)
 	sp.valid = false;
 	sp.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
 	sp.disable_weather_vane = false;
+	sp.loiter_direction_counter_clockwise = false;
 }
 
 float Navigator::get_cruising_throttle()
 {
-	/* Return the mission-requested cruise speed, or default FW_THR_CRUISE value */
+	/* Return the mission-requested cruise speed, or default FW_THR_TRIM value */
 	if (_mission_throttle > FLT_EPSILON) {
 		return _mission_throttle;
 
@@ -1413,7 +1627,7 @@ bool Navigator::abort_landing()
 			// landing status from position controller must be newer than navigator's last position setpoint
 			if (_pos_ctrl_landing_status_sub.copy(&landing_status)) {
 				if (landing_status.timestamp > _pos_sp_triplet.timestamp) {
-					should_abort = landing_status.abort_landing;
+					should_abort = (landing_status.abort_status > 0);
 				}
 			}
 		}
@@ -1479,6 +1693,101 @@ void Navigator::set_mission_failure_heading_timeout()
 	}
 }
 
+void Navigator::handle_custom_action_success()
+{
+	switch_to_auto_mission_mode();
+	reset_custom_action();
+}
+
+void Navigator::handle_custom_action_failure()
+{
+	switch (_custom_action.failure_action) {
+	case CustomAction::FailureAction::IGNORE: {
+			mavlink_log_warning(get_mavlink_log_pub(), "Custom action #%i continuing mission...",
+					    _custom_action.id);
+			switch_to_auto_mission_mode();
+			break;
+		}
+
+	case CustomAction::FailureAction::LOITER: {
+			mavlink_log_warning(get_mavlink_log_pub(), "Custom action #%i switching to loiter...",
+					    _custom_action.id);
+
+			switch_to_auto_loiter_mode();
+			break;
+		}
+
+	case CustomAction::FailureAction::RTL:
+	default: {
+			mavlink_log_warning(get_mavlink_log_pub(), "Custom action #%i switching to RTL...",
+					    _custom_action.id);
+
+			switch_to_auto_rtl_mode();
+			break;
+		}
+	}
+
+	reset_custom_action();
+}
+
+void Navigator::switch_to_auto_mission_mode()
+{
+	if (get_vstatus()->nav_state != vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
+		vehicle_command_s vcmd = {};
+
+		vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
+		vcmd.param1 = 1;
+		vcmd.param2 = PX4_CUSTOM_MAIN_MODE::PX4_CUSTOM_MAIN_MODE_AUTO;
+		vcmd.param3 = PX4_CUSTOM_SUB_MODE_AUTO::PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+
+		publish_vehicle_cmd(&vcmd);
+	}
+}
+
+void Navigator::switch_to_auto_rtl_mode()
+{
+	vehicle_command_s vcmd = {};
+	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
+	vcmd.param1 = 1;
+	vcmd.param2 = PX4_CUSTOM_MAIN_MODE::PX4_CUSTOM_MAIN_MODE_AUTO;
+	vcmd.param3 = PX4_CUSTOM_SUB_MODE_AUTO::PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+	publish_vehicle_cmd(&vcmd);
+}
+
+void Navigator::switch_to_auto_loiter_mode()
+{
+	vehicle_command_s vcmd = {};
+	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
+	vcmd.param1 = 1;
+	vcmd.param2 = PX4_CUSTOM_MAIN_MODE::PX4_CUSTOM_MAIN_MODE_AUTO;
+	vcmd.param3 = PX4_CUSTOM_SUB_MODE_AUTO::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+	publish_vehicle_cmd(&vcmd);
+}
+
+void Navigator::setTerrainFollowerState()
+{
+	_terrain_follower.setHomeAltitude(get_home_position()->alt);
+	_terrain_follower.setCurrentPosition(matrix::Vector2<double>(get_global_position()->lat,
+					     get_global_position()->lon), get_global_position()->alt);
+
+	_terrain_follower.setLoiterRadius(get_loiter_radius());
+
+	float groundspeed;
+
+	if (getGroundSpeed(groundspeed)) {
+		_terrain_follower.setGroundSpeed(groundspeed);
+	}
+
+	_terrain_follower.setIsHoverCraft(get_vstatus()->vehicle_type ==
+					  vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
+}
+
+void Navigator::set_custom_action(const CustomAction &custom_action)
+{
+	_custom_action = custom_action;
+	publish_custom_action_status(custom_action_status_s::STATUS_ACTION_ACCEPTED);
+}
+
 void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
 {
 	vcmd->timestamp = hrt_absolute_time();
@@ -1492,14 +1801,43 @@ void Navigator::publish_vehicle_cmd(vehicle_command_s *vcmd)
 	// sent to the mavlink links to other components.
 	switch (vcmd->command) {
 	case NAV_CMD_IMAGE_START_CAPTURE:
+
+		if (static_cast<int>(vcmd->param3) == 1) {
+			// When sending a single capture we need to include the sequence number, thus camera_trigger needs to handle this cmd
+			vcmd->param1 = 0.0f;
+			vcmd->param2 = 0.0f;
+			vcmd->param3 = 0.0f;
+			vcmd->param4 = 0.0f;
+			vcmd->param5 = 1.0;
+			vcmd->param6 = 0.0;
+			vcmd->param7 = 0.0f;
+			vcmd->command = vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL;
+
+		} else {
+			// We are only capturing multiple if param3 is 0 or > 1.
+			// For multiple pictures the sequence number does not need to be included, thus there is no need to go through camera_trigger
+			_is_capturing_images = true;
+		}
+
+		vcmd->target_component = 100; // MAV_COMP_ID_CAMERA
+		break;
+
 	case NAV_CMD_IMAGE_STOP_CAPTURE:
+		_is_capturing_images = false;
+		vcmd->target_component = 100; // MAV_COMP_ID_CAMERA
+		break;
+
 	case NAV_CMD_VIDEO_START_CAPTURE:
 	case NAV_CMD_VIDEO_STOP_CAPTURE:
 		vcmd->target_component = 100; // MAV_COMP_ID_CAMERA
 		break;
 
+	case NAV_CMD_WAYPOINT_USER_1:
+		vcmd->target_component = 193; // MAV_COMP_ID_ONBOARD_COMPUTER3 (arbitrarily selected)
+		break;
+
 	default:
-		vcmd->target_component = _vstatus.component_id;
+		vcmd->target_component = 0;
 		break;
 	}
 
@@ -1523,6 +1861,12 @@ void Navigator::publish_vehicle_command_ack(const vehicle_command_s &cmd, uint8_
 	_vehicle_cmd_ack_pub.publish(command_ack);
 }
 
+void Navigator::publish_vehicle_cmd_cancel(vehicle_command_cancel_s *vcmd_cancel)
+{
+	vcmd_cancel->timestamp = hrt_absolute_time();
+	_vehicle_cmd_cancel_pub.publish(*vcmd_cancel);
+}
+
 void Navigator::acquire_gimbal_control()
 {
 	vehicle_command_s vcmd = {};
@@ -1545,6 +1889,66 @@ void Navigator::release_gimbal_control()
 	publish_vehicle_cmd(&vcmd);
 }
 
+void
+Navigator::set_gimbal_neutral()
+{
+	vehicle_command_s vcmd = {};
+	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW;
+	vcmd.param1 = NAN;
+	vcmd.param2 = NAN;
+	vcmd.param3 = NAN;
+	vcmd.param4 = NAN;
+	vcmd.param5 = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_NEUTRAL;
+	publish_vehicle_cmd(&vcmd);
+}
+
+void
+Navigator::stop_capturing_images()
+{
+	if (_is_capturing_images) {
+		vehicle_command_s vcmd = {};
+		vcmd.command = NAV_CMD_IMAGE_STOP_CAPTURE;
+		vcmd.param1 = 0.0f;
+		publish_vehicle_cmd(&vcmd);
+
+		// _is_capturing_images is reset inside publish_vehicle_cmd.
+	}
+}
+
+void
+Navigator::disable_camera_trigger()
+{
+	// Disable camera trigger
+	vehicle_command_s cmd {};
+	cmd.command = vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL;
+	// Pause trigger
+	cmd.param1 = -1.0f;
+	cmd.param3 = 1.0f;
+	publish_vehicle_cmd(&cmd);
+}
+
+void
+Navigator::reset_custom_action()
+{
+	// reset custom action timer
+	_custom_action.start_time = 0;
+	_custom_action.timeout = 0;
+
+	// ID of -1 is read as an unset custom_action
+	_custom_action.id = -1;
+}
+
+void
+Navigator::publish_custom_action_status(uint8_t status)
+{
+	custom_action_status_s custom_action_status{};
+	custom_action_status.timestamp = hrt_absolute_time();
+	custom_action_status.action_id = _custom_action.id;
+	custom_action_status.status = status;
+	custom_action_status.failure_reaction = (uint8_t)_custom_action.failure_action;
+	_custom_action_status_pub.publish(custom_action_status);
+}
+
 bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos)
 {
 	if ((_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
@@ -1558,6 +1962,64 @@ bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos)
 	return true;
 }
 
+void Navigator::readVtolHomeLandApproachesFromStorage()
+{
+
+	// go through all mission items in the rally point storage. If we find a mission item of type NAV_CMD_RALLY_POINT
+	// which is within MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES  of our current home position then treat ALL following mission items of type NAV_CMD_LOITER_TO_ALT which come
+	// BEFORE the next mission item of type NAV_CMD_RALLY_POINT as land approaches for the home position
+	_vtol_home_land_approaches.resetAllApproaches();
+
+	if (!home_global_position_valid()) {
+		return;
+	}
+
+	mission_stats_entry_s stats;
+	int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
+	int num_mission_items = 0;
+	bool foundHomeLandApproaches = false;
+	uint8_t sector_counter = 0;
+
+	if (ret == sizeof(mission_stats_entry_s)) {
+		num_mission_items = stats.num_items;
+	}
+
+	for (int current_seq = 1; current_seq <= num_mission_items; ++current_seq) {
+		mission_item_s mission_item{};
+
+		if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_item, sizeof(mission_item_s)) !=
+		    sizeof(mission_item_s)) {
+			PX4_ERR("dm_read failed");
+			break;
+		}
+
+		if (mission_item.nav_cmd == NAV_CMD_RALLY_POINT) {
+
+			if (foundHomeLandApproaches) {
+				break;
+			}
+
+			const float dist_to_home = get_distance_to_next_waypoint(mission_item.lat, mission_item.lon, get_home_position()->lat,
+						   get_home_position()->lon);
+
+			if (!mission_item.is_mission_rally_point && dist_to_home < MAX_DIST_FROM_HOME_FOR_LAND_APPROACHES) {
+				foundHomeLandApproaches = true;
+				_vtol_home_land_approaches.land_location_lat_lon = matrix::Vector2d(mission_item.lat, mission_item.lon);
+			}
+
+			sector_counter = 0;
+		}
+
+		if (foundHomeLandApproaches && mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT) {
+			_vtol_home_land_approaches.approaches[sector_counter].lat = mission_item.lat;
+			_vtol_home_land_approaches.approaches[sector_counter].lon = mission_item.lon;
+			_vtol_home_land_approaches.approaches[sector_counter].height_m = mission_item.altitude;
+			_vtol_home_land_approaches.approaches[sector_counter].loiter_radius_m = mission_item.loiter_radius;
+			sector_counter++;
+		}
+	}
+}
+
 void Navigator::calculate_breaking_stop(double &lat, double &lon, float &yaw)
 {
 	// For multirotors we need to account for the braking distance, otherwise the vehicle will overshoot and go back
diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h
index a3a48a920a..3aa6f255f6 100644
--- a/src/modules/navigator/navigator_mode.h
+++ b/src/modules/navigator/navigator_mode.h
@@ -49,7 +49,8 @@ public:
 	NavigatorMode(Navigator *navigator);
 	virtual ~NavigatorMode() = default;
 	NavigatorMode(const NavigatorMode &) = delete;
-	NavigatorMode operator=(const NavigatorMode &) = delete;
+	NavigatorMode &operator=(const NavigatorMode &) = delete;
+	virtual void initialize() = 0;
 
 	void run(bool active);
 
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 5b40b01ca0..5906d9ed1d 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -53,9 +53,10 @@ static constexpr float DELAY_SIGMA = 0.01f;
 using namespace time_literals;
 using namespace math;
 
-RTL::RTL(Navigator *navigator) :
+RTL::RTL(Navigator *navigator, TerrainFollowerWrapper &terrain_follower) :
 	MissionBlock(navigator),
-	ModuleParams(navigator)
+	ModuleParams(navigator),
+	_terrain_follower(terrain_follower)
 {
 	_param_mpc_z_v_auto_up = param_find("MPC_Z_V_AUTO_UP");
 	_param_mpc_z_v_auto_dn = param_find("MPC_Z_V_AUTO_DN");
@@ -76,9 +77,6 @@ void RTL::on_inactivation()
 
 void RTL::on_inactive()
 {
-	// Reset RTL state.
-	_rtl_state = RTL_STATE_NONE;
-
 	// Limit inactive calculation to 1Hz
 	if ((hrt_absolute_time() - _destination_check_time) > 1_s) {
 		_destination_check_time = hrt_absolute_time();
@@ -87,7 +85,7 @@ void RTL::on_inactive()
 			find_RTL_destination();
 		}
 
-		calc_and_pub_rtl_time_estimate();
+		calc_and_pub_rtl_time_estimate(RTLState::RTL_STATE_NONE);
 	}
 }
 
@@ -127,7 +125,9 @@ void RTL::find_RTL_destination()
 		double mission_landing_lon;
 		float mission_landing_alt;
 
-		if (vtol_in_rw_mode) {
+		// If we're a VTOL in hover and not already on a mission landing, then RTL to home
+		// instead of flying the whole landing pattern in hover mode.
+		if (vtol_in_rw_mode && !_navigator->getMissionLandingInProgress()) {
 			mission_landing_lat = _navigator->get_mission_landing_lat();
 			mission_landing_lon = _navigator->get_mission_landing_lon();
 			mission_landing_alt = _navigator->get_mission_landing_alt();
@@ -170,7 +170,7 @@ void RTL::find_RTL_destination()
 	}
 
 	// compare to safe landing positions
-	mission_safe_point_s closest_safe_point {};
+	mission_item_s closest_safe_point {};
 	mission_stats_entry_s stats;
 	int ret = dm_read(DM_KEY_SAFE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
 	int num_safe_points = 0;
@@ -183,23 +183,26 @@ void RTL::find_RTL_destination()
 	int closest_index = 0;
 
 	for (int current_seq = 1; current_seq <= num_safe_points; ++current_seq) {
-		mission_safe_point_s mission_safe_point;
+		mission_item_s mission_safe_point;
 
-		if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_safe_point_s)) !=
-		    sizeof(mission_safe_point_s)) {
+		if (dm_read(DM_KEY_SAFE_POINTS, current_seq, &mission_safe_point, sizeof(mission_item_s)) !=
+		    sizeof(mission_item_s)) {
 			PX4_ERR("dm_read failed");
 			continue;
 		}
 
-		// TODO: take altitude into account for distance measurement
-		dlat = mission_safe_point.lat - global_position.lat;
-		dlon = mission_safe_point.lon - global_position.lon;
-		double dist_squared = coord_dist_sq(dlat, dlon);
+		if (mission_safe_point.nav_cmd == NAV_CMD_RALLY_POINT && mission_safe_point.is_mission_rally_point == 1) {
 
-		if (dist_squared < min_dist_squared) {
-			closest_index = current_seq;
-			min_dist_squared = dist_squared;
-			closest_safe_point = mission_safe_point;
+			// TODO: take altitude into account for distance measurement
+			dlat = mission_safe_point.lat - global_position.lat;
+			dlon = mission_safe_point.lon - global_position.lon;
+			double dist_squared = coord_dist_sq(dlat, dlon);
+
+			if (dist_squared < min_dist_squared) {
+				closest_index = current_seq;
+				min_dist_squared = dist_squared;
+				closest_safe_point = mission_safe_point;
+			}
 		}
 	}
 
@@ -212,14 +215,14 @@ void RTL::find_RTL_destination()
 		case 0: // MAV_FRAME_GLOBAL
 			_destination.lat = closest_safe_point.lat;
 			_destination.lon = closest_safe_point.lon;
-			_destination.alt = closest_safe_point.alt;
+			_destination.alt = closest_safe_point.altitude;
 			_destination.yaw = home_landing_position.yaw;
 			break;
 
 		case 3: // MAV_FRAME_GLOBAL_RELATIVE_ALT
 			_destination.lat = closest_safe_point.lat;
 			_destination.lon = closest_safe_point.lon;
-			_destination.alt = closest_safe_point.alt + home_landing_position.alt; // alt of safe point is rel to home
+			_destination.alt = closest_safe_point.altitude + home_landing_position.alt; // alt of safe point is rel to home
 			_destination.yaw = home_landing_position.yaw;
 			break;
 
@@ -235,14 +238,16 @@ void RTL::find_RTL_destination()
 		_rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get());
 
 	} else {
-		_rtl_alt = max(global_position.alt, max(_destination.alt,
-							_navigator->get_home_position()->alt + _param_rtl_return_alt.get()));
+		_rtl_alt = max(global_position.alt, _destination.alt + _param_rtl_return_alt.get());
 	}
 }
 
 void RTL::on_activation()
 {
-	setClimbAndReturnDone(false);
+	_rtl_state = RTL_STATE_NONE;
+
+	_terrain_follower.reset();
+	_navigator->setTerrainFollowerState();
 
 	// if a mission landing is desired we should only execute mission navigation mode if we currently are in fw mode
 	// In multirotor mode no landing pattern is required so we can just navigate to the land point directly and don't need to run mission
@@ -269,16 +274,14 @@ void RTL::on_activation()
 
 	const vehicle_global_position_s &global_position = *_navigator->get_global_position();
 
-	_rtl_loiter_rad = _param_rtl_loiter_rad.get();
-
 	if (_navigator->get_land_detected()->landed) {
 		// For safety reasons don't go into RTL if landed.
 		_rtl_state = RTL_STATE_LANDED;
 
 	} else if ((_destination.type == RTL_DESTINATION_MISSION_LANDING) && _navigator->getMissionLandingInProgress()) {
-		// we were just on a mission landing, set _rtl_state past RTL_STATE_RETURN such that navigator will engage mission mode,
+		// we were just on a mission landing, set _rtl_state past RTL_STATE_LOITER such that navigator will engage mission mode,
 		// which will continue executing the landing
-		_rtl_state = RTL_STATE_DESCEND;
+		_rtl_state = RTL_STATE_LAND;
 
 
 	} else if ((global_position.alt < _destination.alt + _param_rtl_return_alt.get()) || _rtl_alt_min) {
@@ -292,21 +295,48 @@ void RTL::on_activation()
 		_rtl_state = RTL_STATE_RETURN;
 	}
 
-	setClimbAndReturnDone(_rtl_state > RTL_STATE_RETURN);
+	// reset cruising speed and throttle to default for RTL
+	_navigator->set_cruising_speed();
+	_navigator->set_cruising_throttle();
+
+	set_rtl_item(true);
 
 	// reset cruising speed and throttle to default for RTL
 	_navigator->set_cruising_speed();
 	_navigator->set_cruising_throttle();
 
-	set_rtl_item();
-
+	_navigator->acquire_gimbal_control();
+	_navigator->set_gimbal_neutral();
+	_navigator->release_gimbal_control();
 }
 
 void RTL::on_active()
 {
-	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
-		advance_rtl();
-		set_rtl_item();
+	bool reload_mission_items = false;
+	bool mission_item_reached = is_mission_item_reached_or_completed();
+
+	_navigator->setTerrainFollowerState();
+
+	if (mission_item_reached || (hrt_absolute_time() - _time_last_terrain_checked > 3 * 1000 * 1000
+				     && !_terrain_follower.hasIntermediateMissionItem())) {
+
+		reload_mission_items = _terrain_follower.updateIntermediateMissionItem(mission_item_reached);
+		_time_last_terrain_checked = hrt_absolute_time();
+	}
+
+	if (_rtl_state != RTL_STATE_LANDED && (mission_item_reached || reload_mission_items)) {
+
+		if (!reload_mission_items) {
+			advance_rtl();
+		}
+
+		if (_terrain_follower.hasIntermediateMissionItem()) {
+			set_intermediate_item();
+
+		} else {
+			const bool do_user_feedback = !reload_mission_items;
+			set_rtl_item(do_user_feedback);
+		}
 	}
 
 	if (_rtl_state == RTL_STATE_LAND && _param_rtl_pld_md.get() > 0) {
@@ -319,11 +349,36 @@ void RTL::on_active()
 	// Limit rtl time calculation to 1Hz
 	if ((hrt_absolute_time() - _destination_check_time) > 1_s) {
 		_destination_check_time = hrt_absolute_time();
-		calc_and_pub_rtl_time_estimate();
+		calc_and_pub_rtl_time_estimate(_rtl_state);
 	}
 }
 
-void RTL::set_rtl_item()
+void RTL::set_intermediate_item()
+{
+	_mission_item = _terrain_follower.getIntermediateMissionItem();
+	publishMissionItem();
+}
+
+void RTL::publishMissionItem()
+{
+	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	reset_mission_item_reached();
+
+	// Execute command if set
+	if (!item_contains_position(_mission_item)) {
+		issue_command(_mission_item);
+	}
+
+	// Convert mission item to current position setpoint and make it valid.
+	mission_apply_limitation(_mission_item);
+
+	if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
+		_navigator->set_position_setpoint_triplet_updated();
+	}
+}
+
+void RTL::set_rtl_item(bool do_user_feedback)
 {
 	_navigator->set_can_loiter_at_sp(false);
 
@@ -332,8 +387,11 @@ void RTL::set_rtl_item()
 	position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 
 	const float destination_dist = get_distance_to_next_waypoint(_destination.lat, _destination.lon, gpos.lat, gpos.lon);
-	const float descend_altitude_target = min(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt);
-	const float loiter_altitude = min(descend_altitude_target, _rtl_alt);
+	const float loiter_altitude = math::min(_destination.alt + _param_rtl_descend_alt.get(), _rtl_alt);
+
+	// if we will switch to mission for landing, already set the loiter radius (incl. direction) from mission
+	const float landing_loiter_radius = _destination.type == RTL_DESTINATION_MISSION_LANDING ?
+					    _navigator->get_mission_landing_loiter_radius() : _param_rtl_loiter_rad.get();
 
 	const RTLHeadingMode rtl_heading_mode = static_cast<RTLHeadingMode>(_param_rtl_hdg_md.get());
 
@@ -376,11 +434,20 @@ void RTL::set_rtl_item()
 		}
 
 	case RTL_STATE_RETURN: {
-			// Don't change altitude.
-			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+
+			// For FW flight:set to LOITER_TIME (with 0s loiter time), such that the loiter (orbit) status
+			// can be displayed on groundstation and the WP is accepted once within loiter radius
+			if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
+				_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+
+
+			} else {
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+			}
+
 			_mission_item.lat = _destination.lat;
 			_mission_item.lon = _destination.lon;
-			_mission_item.altitude = _rtl_alt;
+			_mission_item.altitude = _rtl_alt; // Don't change altitude
 			_mission_item.altitude_is_relative = false;
 
 			if (rtl_heading_mode == RTLHeadingMode::RTL_NAVIGATION_HEADING &&
@@ -400,12 +467,15 @@ void RTL::set_rtl_item()
 			_mission_item.time_inside = 0.0f;
 			_mission_item.autocontinue = true;
 			_mission_item.origin = ORIGIN_ONBOARD;
+			_mission_item.loiter_radius = landing_loiter_radius;
 
-			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t",
-					 (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
-			events::send<int32_t, int32_t>(events::ID("rtl_return_at"), events::Log::Info,
-						       "RTL: return at {1m_v} ({2m_v} above destination)",
-						       (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt));
+			if (do_user_feedback) {
+				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t",
+						 (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
+				events::send<int32_t, int32_t>(events::ID("rtl_return_at"), events::Log::Info,
+							       "RTL: return at {1m_v} ({2m_v} above destination)",
+							       (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt));
+			}
 
 			break;
 		}
@@ -430,14 +500,11 @@ void RTL::set_rtl_item()
 				_mission_item.yaw = _destination.yaw;
 			}
 
-			if (_navigator->get_vstatus()->is_vtol) {
-				_mission_item.loiter_radius = _rtl_loiter_rad;
-			}
-
 			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 			_mission_item.time_inside = 0.0f;
 			_mission_item.autocontinue = true;
 			_mission_item.origin = ORIGIN_ONBOARD;
+			_mission_item.loiter_radius = landing_loiter_radius;
 
 			// Disable previous setpoint to prevent drift.
 			pos_sp_triplet->previous.valid = false;
@@ -451,13 +518,13 @@ void RTL::set_rtl_item()
 		}
 
 	case RTL_STATE_LOITER: {
-			const bool autoland = (_param_rtl_land_delay.get() > FLT_EPSILON);
+			const bool autocontinue = (_param_rtl_land_delay.get() > FLT_EPSILON);
 
-			if (autoland) {
+			if (autocontinue) {
 				_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
 				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t",
-						 (double)get_time_inside(_mission_item));
-				events::send<float>(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", get_time_inside(_mission_item));
+						 (double)_param_rtl_land_delay.get());
+				events::send<float>(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", _param_rtl_land_delay.get());
 
 			} else {
 				_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
@@ -477,11 +544,11 @@ void RTL::set_rtl_item()
 				_mission_item.yaw = _destination.yaw;
 			}
 
-			_mission_item.loiter_radius = _navigator->get_loiter_radius();
 			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 			_mission_item.time_inside = max(_param_rtl_land_delay.get(), 0.0f);
-			_mission_item.autocontinue = autoland;
+			_mission_item.autocontinue = autocontinue;
 			_mission_item.origin = ORIGIN_ONBOARD;
+			_mission_item.loiter_radius = landing_loiter_radius;
 
 			_navigator->set_can_loiter_at_sp(true);
 
@@ -587,19 +654,12 @@ void RTL::set_rtl_item()
 		break;
 	}
 
-	reset_mission_item_reached();
-
-	// Execute command if set. This is required for commands like VTOL transition.
-	if (!item_contains_position(_mission_item)) {
-		issue_command(_mission_item);
+	// give the terrain follower the current mission item if it contains a position
+	if (item_contains_position(_mission_item) && !_terrain_follower.isCurrentMissionItemValid()) {
+		_terrain_follower.setCurrentMissionItem(_mission_item);
 	}
 
-	// Convert mission item to current position setpoint and make it valid.
-	mission_apply_limitation(_mission_item);
-
-	if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) {
-		_navigator->set_position_setpoint_triplet_updated();
-	}
+	publishMissionItem();
 }
 
 void RTL::advance_rtl()
@@ -617,8 +677,6 @@ void RTL::advance_rtl()
 		break;
 
 	case RTL_STATE_RETURN:
-		setClimbAndReturnDone(true);
-
 		if (vtol_in_fw_mode || descend_and_loiter) {
 			_rtl_state = RTL_STATE_DESCEND;
 
@@ -643,6 +701,7 @@ void RTL::advance_rtl()
 		break;
 
 	case RTL_STATE_LOITER:
+
 		if (vtol_in_fw_mode) {
 			_rtl_state = RTL_STATE_TRANSITION_TO_MC;
 
@@ -717,7 +776,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
 	return max(return_altitude_amsl, gpos.alt);
 }
 
-void RTL::calc_and_pub_rtl_time_estimate()
+void RTL::calc_and_pub_rtl_time_estimate(const RTLState rtl_state)
 {
 	rtl_time_estimate_s rtl_time_estimate{};
 
@@ -732,7 +791,7 @@ void RTL::calc_and_pub_rtl_time_estimate()
 		const vehicle_global_position_s &gpos = *_navigator->get_global_position();
 
 		// Sum up time estimate for various segments of the landing procedure
-		switch (_rtl_state) {
+		switch (rtl_state) {
 		case RTL_STATE_NONE:
 		case RTL_STATE_CLIMB: {
 				// Climb segment is only relevant if the drone is below return altitude
@@ -758,7 +817,7 @@ void RTL::calc_and_pub_rtl_time_estimate()
 				float initial_altitude = 0;
 				float loiter_altitude = 0;
 
-				if (_rtl_state == RTL_STATE_DESCEND) {
+				if (rtl_state == RTL_STATE_DESCEND) {
 					// Take current vehicle altitude as the starting point for calculation
 					initial_altitude = gpos.alt;  // TODO: Check if this is in the right frame
 					loiter_altitude = _mission_item.altitude;  // Next waypoint = loiter
@@ -786,7 +845,7 @@ void RTL::calc_and_pub_rtl_time_estimate()
 				float initial_altitude;
 
 				// Add land segment (second landing phase) which comes after LOITER
-				if (_rtl_state == RTL_STATE_LAND) {
+				if (rtl_state == RTL_STATE_LAND) {
 					// If we are in this phase, use the current vehicle altitude  instead
 					// of the altitude paramteter to get a continous time estimate
 					initial_altitude = gpos.alt;
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index 42f8f320f5..939706ccfb 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -45,6 +45,7 @@
 
 #include "navigator_mode.h"
 #include "mission_block.h"
+#include "terrain_follower_wrapper.hpp"
 
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/home_position.h>
@@ -59,10 +60,6 @@ class Navigator;
 class RTL : public MissionBlock, public ModuleParams
 {
 public:
-	RTL(Navigator *navigator);
-
-	~RTL() = default;
-
 	enum RTLType {
 		RTL_TYPE_HOME_OR_RALLY = 0,
 		RTL_TYPE_MISSION_LANDING,
@@ -82,45 +79,6 @@ public:
 		RTL_CURRENT_HEADING,
 	};
 
-	void on_inactivation() override;
-	void on_inactive() override;
-	void on_activation() override;
-	void on_active() override;
-
-	void find_RTL_destination();
-
-	void set_return_alt_min(bool min) { _rtl_alt_min = min; }
-
-	int get_rtl_type() const { return _param_rtl_type.get(); }
-
-	void setClimbAndReturnDone(bool done) { _climb_and_return_done = done; }
-
-	bool getClimbAndReturnDone() { return _climb_and_return_done; }
-
-	void get_rtl_xy_z_speed(float &xy, float &z);
-	matrix::Vector2f get_wind();
-
-	bool getShouldEngageMissionForLanding() const { return _should_engange_mission_for_landing; }
-
-private:
-
-	void set_rtl_item();
-
-	void advance_rtl();
-
-	float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg);
-	void calc_and_pub_rtl_time_estimate();
-
-	float getCruiseGroundSpeed();
-
-	float getClimbRate();
-
-	float getDescendRate();
-
-	float getCruiseSpeed();
-
-	float getHoverLandSpeed();
-
 	enum RTLState {
 		RTL_STATE_NONE = 0,
 		RTL_STATE_CLIMB,
@@ -132,7 +90,55 @@ private:
 		RTL_STATE_LAND,
 		RTL_STATE_LANDED,
 		RTL_STATE_HEAD_TO_CENTER,
-	} _rtl_state{RTL_STATE_NONE};
+	};
+
+	RTL(Navigator *navigator, TerrainFollowerWrapper &terrain_follower);
+
+	~RTL() = default;
+
+	void on_inactivation() override;
+	void on_inactive() override;
+	void on_activation() override;
+	void on_active() override;
+
+	void find_RTL_destination();
+
+	void set_return_alt_min(bool min) { _rtl_alt_min = min; }
+
+	int get_rtl_type() const { return _param_rtl_type.get(); }
+
+	matrix::Vector2f get_wind();
+
+	RTLState getRTLState() { return _rtl_state; }
+
+	bool getDestinationTypeHomeLanding() { return _destination.type == RTL_DESTINATION_HOME; }
+
+	bool getShouldEngageMissionForLanding() const { return _should_engange_mission_for_landing; }
+
+private:
+
+	void set_rtl_item(bool do_user_feedback);
+
+	void set_intermediate_item();
+
+	void advance_rtl();
+
+	float calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg);
+	void calc_and_pub_rtl_time_estimate(const RTLState rtl_state);
+
+	float getCruiseGroundSpeed();
+
+	float getClimbRate();
+
+	float getDescendRate();
+
+	float getCruiseSpeed();
+
+	float getHoverLandSpeed();
+
+	void publishMissionItem();
+
+	RTLState _rtl_state{RTL_STATE_NONE};
 
 	struct RTLPosition {
 		double lat;
@@ -156,14 +162,15 @@ private:
 	RTLPosition _destination{}; ///< the RTL position to fly to (typically the home position or a safe point)
 
 	hrt_abstime _destination_check_time{0};
+	hrt_abstime _time_last_terrain_checked{0};
 
 	float _rtl_alt{0.0f};	// AMSL altitude at which the vehicle should return to the home position
-	float _rtl_loiter_rad{50.0f};		// radius at which a fixed wing would loiter while descending
 
-	bool _climb_and_return_done{false};	// this flag is set to true if RTL is active and we are past the climb state and return state
 	bool _rtl_alt_min{false};
 	bool _should_engange_mission_for_landing{false};
 
+	TerrainFollowerWrapper &_terrain_follower;
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::RTL_RETURN_ALT>)  _param_rtl_return_alt,
 		(ParamFloat<px4::params::RTL_DESCEND_ALT>) _param_rtl_descend_alt,
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index 0e0376c1b5..b307e1cad9 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -47,16 +47,16 @@
  * Return mode return altitude
  *
  * Default minimum altitude above destination (e.g. home, safe point, landing pattern) for return flight.
+ * The vehicle will climb to this altitude when Return mode is enganged, unless it currently is flying higher already.
  * This is affected by RTL_MIN_DIST and RTL_CONE_ANG.
  *
  * @unit m
  * @min 0
- * @max 150
  * @decimal 1
  * @increment 0.5
  * @group Return Mode
  */
-PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60);
+PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60.f);
 
 
 /**
@@ -64,15 +64,15 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60);
  *
  * Descend to this altitude (above destination position) after return, and wait for time defined in RTL_LAND_DELAY.
  * Land (i.e. slowly descend) from this altitude if autolanding allowed.
+ * VTOLs do transition to hover in this altitdue above the landing point.
  *
  * @unit m
- * @min 2
- * @max 100
+ * @min 0
  * @decimal 1
  * @increment 0.5
  * @group Return Mode
  */
-PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30);
+PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30.f);
 
 /**
  * Return mode delay
@@ -82,7 +82,6 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 30);
  *
  * @unit s
  * @min -1
- * @max 300
  * @decimal 1
  * @increment 0.5
  * @group Return Mode
@@ -98,7 +97,6 @@ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, 0.0f);
  *
  * @unit m
  * @min 0.5
- * @max 100
  * @decimal 1
  * @increment 0.5
  * @group Return Mode
@@ -156,7 +154,6 @@ PARAM_DEFINE_INT32(RTL_PLD_MD, 0);
  *
  * @unit m
  * @min 25
- * @max 1000
  * @decimal 1
  * @increment 0.5
  * @group Return Mode
diff --git a/src/modules/navigator/safe_point_land.hpp b/src/modules/navigator/safe_point_land.hpp
new file mode 100644
index 0000000000..6b80132175
--- /dev/null
+++ b/src/modules/navigator/safe_point_land.hpp
@@ -0,0 +1,110 @@
+/***************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+/**
+ * @file safe_point_land.hpp
+ * This file defines helper structs that are used to define land approaches which consists of a land location and a number of
+ * loiter circles. Each loiter circle defines a possible approach for landing at the land location.
+ *
+ */
+
+#pragma once
+
+#include <lib/mathlib/mathlib.h>
+#include <geo/geo.h>
+
+struct loiter_point_s {
+	loiter_point_s() { reset(); }
+	double lat;
+	double lon;
+	float height_m;
+	float loiter_radius_m;
+
+	void reset()
+	{
+		lat = lon = static_cast<double>(NAN);
+		height_m = NAN;
+		loiter_radius_m = NAN;
+	}
+
+	bool isValid() const { return PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(height_m); }
+};
+
+// defines one land location and a maximum of num_approaches_max loiter points
+struct land_approaches_s {
+
+	static constexpr uint8_t num_approaches_max = 8;
+	loiter_point_s approaches[num_approaches_max];
+	matrix::Vector2d land_location_lat_lon;
+
+	land_approaches_s()
+	{
+		resetAllApproaches();
+	}
+
+	void resetAllApproaches()
+	{
+		for (uint8_t i = 0; i < num_approaches_max; i++) {
+			approaches[i].reset();
+		}
+	}
+
+	bool isAnyApproachValid() const
+	{
+		for (uint8_t i = 0; i < num_approaches_max; i++) {
+			if (approaches[i].isValid()) {
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	float getMaxDistLandToLoiterCircle() const
+	{
+		// returns negative infinity if there is no valid approach
+		float dist_max = -INFINITY;
+
+		for (uint8_t i = 0; i < num_approaches_max; i++) {
+			if (approaches[i].isValid()) {
+				float dist = get_distance_to_next_waypoint(land_location_lat_lon(0), land_location_lat_lon(1), approaches[i].lat,
+						approaches[i].lon) + approaches[i].loiter_radius_m;
+
+				if (dist > dist_max) {
+					dist_max = dist;
+				}
+			}
+		}
+
+		return dist_max;
+	}
+};
diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp
index 0f6e9bdc2b..802756cb67 100644
--- a/src/modules/navigator/takeoff.cpp
+++ b/src/modules/navigator/takeoff.cpp
@@ -62,7 +62,7 @@ Takeoff::on_active()
 		// reset the position
 		set_takeoff_position();
 
-	} else if (is_mission_item_reached() && !_navigator->get_mission_result()->finished) {
+	} else if (is_mission_item_reached_or_completed() && !_navigator->get_mission_result()->finished) {
 		_navigator->get_mission_result()->finished = true;
 		_navigator->set_mission_result_updated();
 
diff --git a/src/modules/navigator/terrain_follower_wrapper.cpp b/src/modules/navigator/terrain_follower_wrapper.cpp
new file mode 100644
index 0000000000..1c83fe6121
--- /dev/null
+++ b/src/modules/navigator/terrain_follower_wrapper.cpp
@@ -0,0 +1,161 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 "terrain_follower_wrapper.hpp"
+
+TerrainFollowerWrapper::~TerrainFollowerWrapper()
+{
+	delete _terrain_follower;
+}
+
+void TerrainFollowerWrapper::enable()
+{
+	if (!_terrain_follower) {
+		_terrain_follower = new TerrainFollower();
+	}
+}
+
+void TerrainFollowerWrapper::setCurrentMissionItem(const mission_item_s &current_mission_item)
+{
+	if (_terrain_follower) {
+		_terrain_follower->setCurrentMissionItem(current_mission_item);
+	}
+}
+
+
+void TerrainFollowerWrapper::setCurrentPosition(const matrix::Vector2<double> &pos_lat_lon, float alt_amsl)
+{
+	if (_terrain_follower) {
+		_terrain_follower->setCurrentPosition(pos_lat_lon, alt_amsl);
+	}
+}
+
+void TerrainFollowerWrapper::setHomeAltitude(float home_alt_m)
+{
+	if (_terrain_follower) {
+		_terrain_follower->setHomeAltitude(home_alt_m);
+	}
+}
+
+void TerrainFollowerWrapper::setTerrainProvider(terrain::TerrainProvider *terrain_provider)
+{
+	if (_terrain_follower) {
+		_terrain_follower->setTerrainProvider(terrain_provider);
+	}
+}
+
+mission_item_s TerrainFollowerWrapper::getIntermediateMissionItem()
+{
+	if (_terrain_follower) {
+		return _terrain_follower->getIntermediateMissionItem();
+
+	} else {
+		mission_item_s empty = {};
+		return empty;
+	}
+}
+
+bool TerrainFollowerWrapper::updateIntermediateMissionItem(bool mission_item_reached)
+{
+	if (_terrain_follower) {
+		return _terrain_follower->updateIntermediateMissionItem(mission_item_reached);
+
+	} else {
+		return false;
+	}
+}
+
+bool TerrainFollowerWrapper::hasIntermediateMissionItem()
+{
+	if (_terrain_follower) {
+		return _terrain_follower->hasIntermediateMissionItem();
+
+	} else {
+		return false;
+	}
+}
+
+mission_item_s TerrainFollowerWrapper::getCurrentMissionItem()
+{
+	if (_terrain_follower) {
+		return _terrain_follower->getCurrentMissionItem();
+
+	} else {
+		mission_item_s empty = {};
+		return empty;
+	}
+}
+
+bool TerrainFollowerWrapper::isCurrentMissionItemValid()
+{
+	if (_terrain_follower) {
+		return _terrain_follower->isCurrentMissionItemValid();
+
+	} else {
+		return false;
+	}
+}
+
+void TerrainFollowerWrapper::reset()
+{
+	if (_terrain_follower) {
+		_terrain_follower->reset();
+	}
+}
+
+void TerrainFollowerWrapper::setGroundSpeed(float ground_speed_m_s)
+{
+	if (_terrain_follower) {
+		_terrain_follower->setGroundSpeed(ground_speed_m_s);
+	}
+}
+
+void TerrainFollowerWrapper::setIsHoverCraft(bool is_hovercraft)
+{
+	if (_terrain_follower) {
+		_terrain_follower->setIsHoverCraft(is_hovercraft);
+	}
+}
+
+void TerrainFollowerWrapper::setLoiterRadius(float radius)
+{
+	if (_terrain_follower) {
+		_terrain_follower->setLoiterRadius(radius);
+	}
+}
+void TerrainFollowerWrapper::updateParams()
+{
+	if (_terrain_follower) {
+		_terrain_follower->updateParams();
+	}
+}
\ No newline at end of file
diff --git a/src/modules/navigator/terrain_follower_wrapper.hpp b/src/modules/navigator/terrain_follower_wrapper.hpp
new file mode 100644
index 0000000000..b9158119a6
--- /dev/null
+++ b/src/modules/navigator/terrain_follower_wrapper.hpp
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 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 "TerrainFollower/terrain_follower.hpp"
+
+class TerrainFollowerWrapper
+{
+public:
+	TerrainFollowerWrapper() = default;
+
+	~TerrainFollowerWrapper();
+
+	void setCurrentMissionItem(const mission_item_s &current_mission_item);
+
+	void setCurrentPosition(const matrix::Vector2<double> &pos_lat_lon, float alt_amsl);
+
+	void setHomeAltitude(float home_alt_m);
+
+	void setTerrainProvider(terrain::TerrainProvider *terrain_provider);
+
+	struct mission_item_s getIntermediateMissionItem();
+
+	bool updateIntermediateMissionItem(bool mission_item_reached);
+
+	bool hasIntermediateMissionItem();
+
+	mission_item_s getCurrentMissionItem();
+
+	bool isCurrentMissionItemValid();
+
+	void reset();
+
+	void setGroundSpeed(float ground_speed_m_s);
+
+	void setIsHoverCraft(bool is_hovercraft);
+
+	void setLoiterRadius(float radius);
+
+	void updateParams();
+
+	void enable();
+
+private:
+
+	TerrainFollower *_terrain_follower = nullptr;
+
+};
\ No newline at end of file
diff --git a/src/modules/navigator/vtol_land.cpp b/src/modules/navigator/vtol_land.cpp
new file mode 100644
index 0000000000..cea945f687
--- /dev/null
+++ b/src/modules/navigator/vtol_land.cpp
@@ -0,0 +1,239 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+/**
+ * @file vtol_land.cpp
+ *
+ * Helper class to do a VTOL landing using a loiter down to altitude landing pattern.
+ *
+ */
+
+#include "vtol_land.h"
+#include "navigator.h"
+
+using matrix::wrap_pi;
+
+VtolLand::VtolLand(Navigator *navigator) :
+	MissionBlock(navigator),
+	ModuleParams(navigator)
+{
+}
+
+void
+VtolLand::on_activation()
+{
+	_land_pos_lat_lon = matrix::Vector2<double>(_navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
+	set_loiter_position();
+	_land_state = vtol_land_state::MOVE_TO_LOITER;
+
+}
+
+void
+VtolLand::on_active()
+{
+	if (is_mission_item_reached_or_completed()) {
+		switch	(_land_state) {
+		case vtol_land_state::MOVE_TO_LOITER: {
+				_mission_item.altitude = _navigator->get_home_position()->alt + _land_approach.height_m;
+				_mission_item.nav_cmd = NAV_CMD_LOITER_TO_ALT;
+				_mission_item.loiter_radius = _param_rtl_loiter_rad.get();
+
+				_navigator->get_mission_result()->finished = false;
+				_navigator->set_mission_result_updated();
+				reset_mission_item_reached();
+
+				// convert mission item to current setpoint
+				struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+				mission_apply_limitation(_mission_item);
+				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+				pos_sp_triplet->next.valid = true;
+				pos_sp_triplet->next.lat = _land_pos_lat_lon(0);
+				pos_sp_triplet->next.lon = _land_pos_lat_lon(1);
+				pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_LAND;
+
+				pos_sp_triplet->previous.valid = false;
+				pos_sp_triplet->current.yaw_valid = true;
+
+				_land_state = vtol_land_state::LOITER_DOWN;
+				break;
+			}
+
+		case vtol_land_state::LOITER_DOWN: {
+				_mission_item.lat = _land_pos_lat_lon(0);
+				_mission_item.lon = _land_pos_lat_lon(1);
+
+				_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+				_mission_item.vtol_back_transition = true;
+
+				struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+				// set previous item location to loiter location such that vehicle tracks line between loiter
+				// location and land location after exiting the loiter circle
+				pos_sp_triplet->previous.lat = _loiter_pos_lat_lon(0);
+				pos_sp_triplet->previous.lon = _loiter_pos_lat_lon(1);
+				pos_sp_triplet->previous.valid = true;
+
+				//publish_navigator_mission_item(); // for logging
+				_navigator->set_position_setpoint_triplet_updated();
+
+				// issue_command(_mission_item);
+				reset_mission_item_reached();
+
+				_land_state = vtol_land_state::TRANSITION_TO_MC;
+
+				break;
+			}
+
+		case vtol_land_state::TRANSITION_TO_MC: {
+				set_vtol_transition_item(&_mission_item, vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC);
+				_mission_item.vtol_back_transition = true;
+
+				issue_command(_mission_item);
+
+				struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+				//publish_navigator_mission_item(); // for logging
+				_navigator->set_position_setpoint_triplet_updated();
+
+				// issue_command(_mission_item);
+				reset_mission_item_reached();
+
+				_land_state = vtol_land_state::LAND;
+
+				break;
+			}
+
+		case vtol_land_state::LAND: {
+				_mission_item.nav_cmd = NAV_CMD_LAND;
+				struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+				mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+				//publish_navigator_mission_item(); // for logging
+				_navigator->set_position_setpoint_triplet_updated();
+
+				// issue_command(_mission_item);
+				reset_mission_item_reached();
+
+				_land_state = vtol_land_state::IDLE;
+
+				break;
+			}
+
+		default: {
+
+				break;
+			}
+		}
+	}
+}
+
+void
+VtolLand::set_loiter_position()
+{
+
+	_land_approach = chooseBestLandingApproach();
+
+	if (PX4_ISFINITE(_land_approach.lat) && PX4_ISFINITE(_land_approach.lon)) {
+		_loiter_pos_lat_lon(0) = _land_approach.lat;
+		_loiter_pos_lat_lon(1) = _land_approach.lon;
+
+	} else {
+		_loiter_pos_lat_lon(0) = _navigator->get_home_position()->lat;
+		_loiter_pos_lat_lon(1) = _navigator->get_home_position()->lon;
+	}
+
+
+	_mission_item.lat  = _loiter_pos_lat_lon(0);
+	_mission_item.lon = _loiter_pos_lat_lon(1);
+	_mission_item.altitude = math::max(_navigator->get_home_position()->alt + _param_return_alt_rel_m.get(),
+					   _navigator->get_global_position()->alt);
+	_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+	_mission_item.force_heading = true;
+	_mission_item.autocontinue = false;
+	_mission_item.time_inside = _min_loiter_time_before_land;
+
+	_mission_item.altitude_is_relative = false;
+
+	_mission_item.loiter_radius = _param_rtl_loiter_rad.get();
+	_mission_item.origin = ORIGIN_ONBOARD;
+
+	_navigator->get_mission_result()->finished = false;
+	_navigator->set_mission_result_updated();
+	reset_mission_item_reached();
+
+	// convert mission item to current setpoint
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	mission_apply_limitation(_mission_item);
+	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
+	_navigator->set_can_loiter_at_sp(true);
+
+	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+loiter_point_s VtolLand::chooseBestLandingApproach()
+{
+	wind_s *wind = _navigator->get_wind();
+	const float wind_direction = atan2f(wind->windspeed_east, wind->windspeed_north);
+	int8_t min_index = -1;
+	float wind_angle_prev = INFINITY;
+
+	land_approaches_s land_approaches = _navigator->getVtolHomeLandArea();
+
+	for (int i = 0; i < land_approaches.num_approaches_max; i++) {
+
+		if (land_approaches.approaches[i].isValid()) {
+			const float wind_angle = wrap_pi(get_bearing_to_next_waypoint(_navigator->get_home_position()->lat,
+							 _navigator->get_home_position()->lon, land_approaches.approaches[i].lat,
+							 land_approaches.approaches[i].lon) - wind_direction);
+
+			if (fabsf(wind_angle) < wind_angle_prev) {
+				min_index = i;
+				wind_angle_prev = fabsf(wind_angle);
+			}
+
+		}
+	}
+
+	if (min_index >= 0) {
+		return land_approaches.approaches[min_index];
+
+	} else {
+
+		return loiter_point_s();
+	}
+}
diff --git a/src/modules/navigator/vtol_land.h b/src/modules/navigator/vtol_land.h
new file mode 100644
index 0000000000..1049543e7a
--- /dev/null
+++ b/src/modules/navigator/vtol_land.h
@@ -0,0 +1,84 @@
+/***************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+/**
+ * @file vtol_land.h
+ *
+ * Helper class to do a VTOL landing using a loiter down to altitude landing pattern.
+ *
+ */
+
+#pragma once
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+#include "safe_point_land.hpp"
+
+#include <lib/mathlib/mathlib.h>
+
+#include <px4_platform_common/module_params.h>
+
+class VtolLand : public MissionBlock, public ModuleParams
+{
+public:
+	VtolLand(Navigator *navigator);
+	~VtolLand() = default;
+
+	void on_activation() override;
+	void on_active() override;
+private:
+
+	enum class vtol_land_state {
+		MOVE_TO_LOITER = 0,
+		LOITER_DOWN,
+		TRANSITION_TO_MC,
+		LAND,
+		IDLE
+	} _land_state;
+
+	matrix::Vector2<double> _land_pos_lat_lon;
+	matrix::Vector2<double> _loiter_pos_lat_lon;
+	loiter_point_s _land_approach;
+
+	static constexpr float _min_loiter_time_before_land = 10.0f;
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::VTO_LOITER_ALT>) _param_loiter_alt,
+		(ParamFloat<px4::params::RTL_RETURN_ALT>) _param_return_alt_rel_m,
+		(ParamFloat<px4::params::RTL_LOITER_RAD>) _param_rtl_loiter_rad
+	)
+
+	void set_loiter_position();
+
+	loiter_point_s chooseBestLandingApproach();
+
+};
diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp
index 9f642d1acf..efa175abfb 100644
--- a/src/modules/navigator/vtol_takeoff.cpp
+++ b/src/modules/navigator/vtol_takeoff.cpp
@@ -62,7 +62,7 @@ VtolTakeoff::on_activation()
 void
 VtolTakeoff::on_active()
 {
-	if (is_mission_item_reached()) {
+	if (is_mission_item_reached_or_completed()) {
 		reset_mission_item_reached();
 
 		switch	(_takeoff_state) {
@@ -142,6 +142,13 @@ VtolTakeoff::on_active()
 
 		case vtol_takeoff_state::CLIMB: {
 
+				// reset any potentially valid reposition triplet which was not handled
+				// we do this to avoid random loiter locations after switching to loiter mode after this
+				position_setpoint_triplet_s *reposition_triplet = _navigator->get_reposition_triplet();
+				_navigator->reset_position_setpoint(reposition_triplet->previous);
+				_navigator->reset_position_setpoint(reposition_triplet->current);
+				_navigator->reset_position_setpoint(reposition_triplet->next);
+
 				// the VTOL takeoff is done, proceed loitering and upate the navigation state to LOITER
 				_navigator->get_mission_result()->finished = true;
 				_navigator->set_mission_result_updated();
diff --git a/src/modules/payload_deliverer/CMakeLists.txt b/src/modules/payload_deliverer/CMakeLists.txt
new file mode 100644
index 0000000000..61b5c4bafb
--- /dev/null
+++ b/src/modules/payload_deliverer/CMakeLists.txt
@@ -0,0 +1,45 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__payload_deliverer
+	MAIN payload_deliverer
+	SRCS
+		payload_deliverer.cpp
+		gripper.cpp
+	MODULE_CONFIG
+		module.yaml
+	COMPILE_FLAGS
+		#-DDEBUG_BUILD
+	DEPENDS
+		px4_work_queue
+	)
diff --git a/src/modules/payload_deliverer/Kconfig b/src/modules/payload_deliverer/Kconfig
new file mode 100644
index 0000000000..c9a0756fe9
--- /dev/null
+++ b/src/modules/payload_deliverer/Kconfig
@@ -0,0 +1,5 @@
+menuconfig MODULES_PAYLOAD_DELIVERER
+	bool "payload_deliverer"
+	default n
+	---help---
+		Enable payload_deliverer module
diff --git a/src/modules/payload_deliverer/gripper.cpp b/src/modules/payload_deliverer/gripper.cpp
new file mode 100644
index 0000000000..5ceacd78b3
--- /dev/null
+++ b/src/modules/payload_deliverer/gripper.cpp
@@ -0,0 +1,184 @@
+/****************************************************************************
+ *
+ *   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 "gripper.h"
+
+void Gripper::init(const GripperConfig &config)
+{
+	switch (config.type) {
+	case GripperConfig::GripperType::SERVO:
+		break;
+
+	case GripperConfig::GripperType::UNDEFINED:
+
+	// FALL THROUGH
+	default:
+		_valid = false;
+		return;
+	}
+
+	switch (config.sensor) {
+	case GripperConfig::GripperSensorType::ENCODER:
+		// Handle encoder sensor setup
+		_has_feedback_sensor = true;
+		break;
+
+	case GripperConfig::GripperSensorType::NONE:
+	default:
+		// No feedback sensor
+		_has_feedback_sensor = false;
+		break;
+	}
+
+	_timeout_us = config.timeout_us;
+
+	// Reset internal states
+	_state = GripperState::IDLE;
+	_last_command_time = 0;
+	_released_state_cache = false;
+	_grabbed_state_cache = false;
+
+	// We have valid gripper type & sensor configuration
+	_valid = true;
+}
+
+void Gripper::deinit()
+{
+	// Reset Config variables
+	_has_feedback_sensor = false;
+	_timeout_us = 0;
+
+	// Reset internal states
+	_state = GripperState::IDLE;
+	_last_command_time = 0;
+	_released_state_cache = false;
+	_grabbed_state_cache = false;
+
+	// Mark gripper instance as invalid
+	_valid = false;
+}
+
+void Gripper::grab()
+{
+	if (_state == GripperState::GRABBING || _state == GripperState::GRABBED) {
+		return;
+	}
+
+	publish_gripper_command(gripper_s::COMMAND_GRAB);
+	_state = GripperState::GRABBING;
+	_last_command_time = hrt_absolute_time();
+}
+
+void Gripper::release()
+{
+	if (_state == GripperState::RELEASING || _state == GripperState::RELEASED) {
+		return;
+	}
+
+	publish_gripper_command(gripper_s::COMMAND_RELEASE);
+	_state = GripperState::RELEASING;
+	_last_command_time = hrt_absolute_time();
+}
+
+void Gripper::update()
+{
+	const bool command_timed_out = (hrt_elapsed_time(&_last_command_time) >= _timeout_us);
+
+	// Handle transition from intermediate state to definite state
+	switch (_state) {
+	case GripperState::GRABBING:
+		if (_has_feedback_sensor) {
+			// Handle feedback sensor input, return true for now (not supported)
+			_grabbed_state_cache = true;
+			_state = GripperState::GRABBED;
+			break;
+		}
+
+		if (command_timed_out) {
+			_grabbed_state_cache = true;
+			_state = GripperState::GRABBED;
+		}
+
+		break;
+
+	case GripperState::RELEASING:
+		if (_has_feedback_sensor) {
+			// Handle feedback sensor input, return true for now (not supported)
+			_released_state_cache = true;
+			_state = GripperState::RELEASED;
+			break;
+		}
+
+		if (command_timed_out) {
+			_released_state_cache = true;
+			_state = GripperState::RELEASED;
+		}
+
+		break;
+
+	default:
+		// DO NOTHING
+		break;
+	}
+}
+
+void Gripper::publish_gripper_command(const int8_t gripper_command)
+{
+	gripper_s gripper{};
+	gripper.timestamp = hrt_absolute_time();
+	gripper.command = gripper_command;
+	_gripper_pub.publish(gripper);
+}
+
+const char *Gripper::get_state_str() const
+{
+	switch (_state) {
+	case GripperState::GRABBING:
+		return "GRABBING";
+
+	case GripperState::GRABBED:
+		return "GRABBED";
+
+	case GripperState::RELEASING:
+		return "RELEASING";
+
+	case GripperState::RELEASED:
+		return "RELEASED";
+
+	case GripperState::IDLE:
+		return "IDLE";
+
+	default:
+		return "UNKNOWN";
+	}
+}
diff --git a/src/modules/payload_deliverer/gripper.h b/src/modules/payload_deliverer/gripper.h
new file mode 100644
index 0000000000..f90378647b
--- /dev/null
+++ b/src/modules/payload_deliverer/gripper.h
@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   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 <drivers/drv_hrt.h>
+
+#include <uORB/Publication.hpp>
+#include <uORB/topics/gripper.h>
+
+using namespace time_literals;
+
+/**
+ * Griper class to handle functionality of using a Gripper
+ */
+
+typedef struct GripperConfig {
+	enum class GripperType {
+		UNDEFINED = -1,
+		SERVO = 0,
+	};
+
+	GripperType type{GripperType::UNDEFINED};
+
+	// Gripper state feedback sensor type
+	enum class GripperSensorType {
+		NONE = -1,
+		ENCODER,
+	};
+
+	GripperSensorType sensor{GripperSensorType::NONE};
+
+	hrt_abstime timeout_us{0};
+
+} GripperConfig;
+
+enum class GripperState {
+	IDLE = 0,
+	GRABBING,
+	GRABBED,
+	RELEASING,
+	RELEASED
+};
+
+class Gripper
+{
+public:
+	Gripper() = default;
+
+	// Initialize the gripper
+	void init(const GripperConfig &config);
+
+	// De-initialize the gripper
+	void deinit();
+
+	// Actuate the gripper to the grab position
+	void grab();
+
+	// Actuate the gripper to release position
+	void release();
+
+	// Update gripper state
+	void update();
+
+	// Return griper's state in string format
+	const char *get_state_str() const;
+
+	// Returns true if in grabbed position either sensed or timeout based
+	bool grabbed() { return _state == GripperState::GRABBED; }
+
+	// Returns true if in grabbing position either sensed or timeout based
+	bool grabbing() { return _state == GripperState::GRABBING; }
+
+	// Returns true if in released position either sensed or timeout based
+	bool released() { return _state == GripperState::RELEASED; }
+
+	/**
+	 * @brief Returns true once after gripper succeededs in releasing the payload
+	 *
+	 * Note, that this is a single-read for a one successful release operation. Meaning that
+	 * if you command `release()` and then call this function multiple times in the future,
+	 * it will only return `true` only once, and then it will return `false` after it returns
+	 * `true` once.
+	 *
+	 * This is used to detect a single event of successful releasing of a gripper in `payload_deliverer`
+	 * module, which then would send `vehicle_command_ack` message, indicating the successful release.
+	 */
+	bool released_read_once()
+	{
+		if (_released_state_cache) {
+			_released_state_cache = false;
+			return true;
+
+		} else {
+			return false;
+		}
+	}
+
+	/**
+	 * @brief Returns true once after gripper succeededs in grabbing the payload
+	 */
+	bool grabbed_read_once()
+	{
+		if (_grabbed_state_cache) {
+			_grabbed_state_cache = false;
+			return true;
+
+		} else {
+			return false;
+		}
+	}
+
+	// Returns true if Gripper output function is assigned properly
+	bool is_valid() const { return _valid; }
+
+private:
+	// Internal function to send gripper command via uORB
+	void publish_gripper_command(const int8_t gripper_command);
+
+	// Internal settings
+	bool _valid{false};
+	bool _has_feedback_sensor{false};
+	hrt_abstime _timeout_us{0};
+
+	// Internal state
+	GripperState _state{GripperState::IDLE};
+	hrt_abstime _last_command_time{0};
+
+	// Cached flag that is set once gripper release/grab was successful, it must get reset when read.
+	bool _released_state_cache{false};
+	bool _grabbed_state_cache{false};
+
+	uORB::Publication<gripper_s> _gripper_pub{ORB_ID(gripper)};
+};
diff --git a/src/modules/payload_deliverer/module.yaml b/src/modules/payload_deliverer/module.yaml
new file mode 100644
index 0000000000..99bb2825ca
--- /dev/null
+++ b/src/modules/payload_deliverer/module.yaml
@@ -0,0 +1,37 @@
+module_name: payload_deliverer
+
+parameters:
+    - group: Payload Deliverer
+      definitions:
+
+        # Gripper configuration
+        PD_GRIPPER_EN:
+            description:
+                short: Enable Gripper actuation in Payload Deliverer
+            type: boolean
+            default: 0
+            reboot_required: true
+
+        PD_GRIPPER_TYPE:
+            description:
+                short: Type of Gripper (Servo, etc.)
+            type: enum
+            values:
+                -1: Undefined
+                0: Servo
+            min: -1
+            max: 0
+            default: 0
+
+        PD_GRIPPER_TO:
+            description:
+                short: Timeout for successful gripper actuation acknowledgement
+                long: |
+                  Maximum time Gripper will wait while the successful griper actuation isn't recognised.
+                  If the gripper has no feedback sensor, it will simply wait for
+                  this time before considering gripper actuation successful and publish a
+                  'vehicle_command_ack' signaling successful gripper action
+            type: float
+            unit: s
+            min: 0
+            default: 3
diff --git a/src/modules/payload_deliverer/payload_deliverer.cpp b/src/modules/payload_deliverer/payload_deliverer.cpp
new file mode 100644
index 0000000000..0edb3a5270
--- /dev/null
+++ b/src/modules/payload_deliverer/payload_deliverer.cpp
@@ -0,0 +1,387 @@
+/****************************************************************************
+ *
+ *   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 "payload_deliverer.h"
+
+PayloadDeliverer::PayloadDeliverer()
+	: ModuleParams(nullptr),
+	  ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default)
+{
+}
+
+bool PayloadDeliverer::init()
+{
+	ScheduleOnInterval(100_ms);
+
+	if (!_vehicle_command_sub.registerCallback()) {
+		PX4_ERR("callback registration failed");
+		return false;
+	}
+
+	configure_gripper();
+	return true;
+}
+
+void PayloadDeliverer::configure_gripper()
+{
+	// If gripper instance is invalid, and user enabled the gripper, try initializing it
+	if (!_gripper.is_valid() && _param_gripper_enable.get()) {
+		GripperConfig config{};
+		config.type = (GripperConfig::GripperType)_param_gripper_type.get();
+		config.sensor = GripperConfig::GripperSensorType::NONE; // Feedback sensor isn't supported for now
+		config.timeout_us = hrt_abstime(_param_gripper_timeout_s.get() * 1000000ULL);
+		_gripper.init(config);
+
+		if (!_gripper.is_valid()) {
+			PX4_DEBUG("Gripper object initialization failed!");
+			return;
+
+		} else {
+			// Command the gripper to grab position by default
+			if (!_gripper.grabbed() && !_gripper.grabbing()) {
+				PX4_DEBUG("Gripper intialize: putting to grab position!");
+				send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_GRAB);
+			}
+
+			return;
+		}
+	}
+
+	// TODO: Support changing gripper sensor type / gripper type configuration
+}
+
+void PayloadDeliverer::parameter_update()
+{
+	updateParams();
+	configure_gripper();
+}
+
+void PayloadDeliverer::Run()
+{
+	const hrt_abstime now = hrt_absolute_time();
+	vehicle_command_s vcmd{};
+
+	if (should_exit()) {
+		ScheduleClear();
+		_vehicle_command_sub.unregisterCallback();
+		exit_and_cleanup();
+		return;
+	}
+
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s param_update_dummy;
+		_parameter_update_sub.copy(&param_update_dummy);
+		parameter_update();
+	}
+
+	gripper_update(now);
+
+	if (_vehicle_command_sub.update(&vcmd)) {
+		handle_vehicle_command(now, &vcmd);
+
+	} else {
+		handle_vehicle_command(now);
+
+	}
+}
+
+void PayloadDeliverer::gripper_update(const hrt_abstime &now)
+{
+	if (!_gripper.is_valid()) {
+		return;
+	}
+
+	_gripper.update();
+
+	// Publish a successful gripper release / grab acknowledgement, and set the current gripper
+	// action to GRIPPER_ACTION_NONE to signify we aren't executing any vehicle command now
+	if (_gripper.released_read_once()) {
+		send_gripper_vehicle_command_ack(now, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED, _cur_vcmd_target_system,
+						 _cur_vcmd_target_component);
+		PX4_DEBUG("Payload Release Successful Ack Sent!");
+
+		_cur_vcmd_gripper_action = GRIPPER_ACTION_NONE;
+
+	} else if (_gripper.grabbed_read_once()) {
+		send_gripper_vehicle_command_ack(now, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED, _cur_vcmd_target_system,
+						 _cur_vcmd_target_component);
+		PX4_DEBUG("Payload Grab Successful Ack Sent!");
+
+		_cur_vcmd_gripper_action = GRIPPER_ACTION_NONE;
+
+	}
+}
+
+void PayloadDeliverer::handle_vehicle_command(const hrt_abstime &now,  const vehicle_command_s *vehicle_command)
+{
+	// If there's no vehicle command to process, just return
+	if (vehicle_command == nullptr) {
+		return;
+	}
+
+	// Process DO_GRIPPER vehicle command
+	if (vehicle_command->command == vehicle_command_s::VEHICLE_CMD_DO_GRIPPER) {
+		if (!_gripper.is_valid()) {
+			PX4_WARN("Gripper instance not valid but DO_GRIPPER vehicle command was received. Gripper won't work!");
+			send_gripper_vehicle_command_ack(now, vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED, vehicle_command->source_system,
+							 vehicle_command->source_component);
+			return;
+		}
+
+		const int32_t gripper_action = (int32_t)roundf(vehicle_command->param2);
+
+		// Flag to indicate if we can process the new gripper command
+		bool process_current_gripper_cmd{false};
+
+		// We are currently in the middle of executing a previous vehicle command. Handle the conflicts
+		if (_cur_vcmd_gripper_action != GRIPPER_ACTION_NONE) {
+
+			if (gripper_action != _cur_vcmd_gripper_action) {
+				// If different action is commanded, cancel the previous vehicle command
+				send_gripper_vehicle_command_ack(now, vehicle_command_ack_s::VEHICLE_CMD_RESULT_CANCELLED, _cur_vcmd_target_system,
+								 _cur_vcmd_target_component);
+
+				// NOTE: Does this make sense when the same entity sends two conflicting commands?
+				// For the previous command, the external entity will received CANCELLED result, but then
+				// would receive IN_PROGRESS for the new command. As the COMMAND_CANCEL isn't defined properly
+				// in MAVLink yet(https://mavlink.io/en/messages/common.html#COMMAND_CANCEL), this can lead to
+				// Ground stations to thinking it's new command has been canceled instead of the previous one!
+
+				process_current_gripper_cmd = true;
+
+			} else {
+				// If same action is commanded, temporarily reject current command (since it's already in progress)
+				send_gripper_vehicle_command_ack(now, vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED,
+								 vehicle_command->source_system, vehicle_command->source_component);
+			}
+
+		} else {
+			// If we aren't processing any gripper commands, always process the new command
+			process_current_gripper_cmd = true;
+
+		}
+
+		// Flag to indicate if we executed the current gripper command
+		bool current_gripper_command_executed{false};
+
+		if (process_current_gripper_cmd) {
+			if ((_gripper.grabbed() && (gripper_action == vehicle_command_s::GRIPPER_ACTION_GRAB)) ||
+			    (_gripper.released() && (gripper_action == vehicle_command_s::GRIPPER_ACTION_RELEASE))) {
+				// First check if we already satisfied the requested command. If so, acknowledge as accepted and don't execute the command
+				send_gripper_vehicle_command_ack(now, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED,
+								 vehicle_command->source_system, vehicle_command->source_component);
+
+			} else {
+				// Check if the gripper action is valid
+				switch (gripper_action) {
+				case vehicle_command_s::GRIPPER_ACTION_GRAB:
+					_gripper.grab();
+					current_gripper_command_executed = true;
+					break;
+
+				case vehicle_command_s::GRIPPER_ACTION_RELEASE:
+					_gripper.release();
+					current_gripper_command_executed = true;
+					break;
+
+				default:
+					// INVALID GRIPPER ACTION. Deny the command.
+					send_gripper_vehicle_command_ack(now, vehicle_command_ack_s::VEHICLE_CMD_RESULT_DENIED, vehicle_command->source_system,
+									 vehicle_command->source_component);
+				}
+			}
+		}
+
+		// Send acknowledgement for successfully executed gripper commands
+		if (current_gripper_command_executed) {
+			send_gripper_vehicle_command_ack(now, vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS,
+							 vehicle_command->source_system, vehicle_command->source_component);
+
+			// Cache the last executed vehicle command information for later acknowledgement
+			_cur_vcmd_gripper_action = gripper_action;
+			_cur_vcmd_target_system = vehicle_command->source_system;
+			_cur_vcmd_target_component = vehicle_command->source_component;
+		}
+	}
+}
+
+bool PayloadDeliverer::send_gripper_vehicle_command(const int32_t gripper_action)
+{
+	vehicle_command_s vcmd;
+	vcmd.timestamp = hrt_absolute_time();
+	vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_GRIPPER;
+	vcmd.param2 = gripper_action;
+	// Note: Integer type GRIPPER_ACTION gets formatted into a floating point here.
+	return _vehicle_command_pub.publish(vcmd);
+}
+
+bool PayloadDeliverer::send_gripper_vehicle_command_ack(const hrt_abstime now, const uint8_t command_result,
+		const uint8_t target_system, const uint8_t target_component)
+{
+	vehicle_command_ack_s vcmd_ack{};
+	vcmd_ack.timestamp = now;
+	vcmd_ack.command = vehicle_command_s::VEHICLE_CMD_DO_GRIPPER;
+	vcmd_ack.result = command_result;
+
+	switch (command_result) {
+	case vehicle_command_ack_s::VEHICLE_CMD_RESULT_IN_PROGRESS:
+		// Fill in the progress percentage field for IN_PROGRESS ack message
+		vcmd_ack.result_param1 = UINT8_MAX;
+		break;
+	}
+
+	vcmd_ack.target_system = target_system;
+	vcmd_ack.target_component = target_component;
+	return _vehicle_command_ack_pub.publish(vcmd_ack);
+}
+
+void PayloadDeliverer::gripper_test()
+{
+	if (!_gripper.is_valid()) {
+		PX4_INFO("Gripper is not initialized correctly!");
+		return;
+	}
+
+	PX4_INFO("Test: Opening the Gripper!");
+	send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_RELEASE);
+
+	px4_usleep(5_s);
+
+	PX4_INFO("Test: Closing the Gripper!");
+	send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_GRAB);
+}
+
+void PayloadDeliverer::gripper_open()
+{
+	if (!_gripper.is_valid()) {
+		PX4_INFO("Gripper is not initialized correctly!");
+		return;
+	}
+
+	send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_RELEASE);
+}
+
+void PayloadDeliverer::gripper_close()
+{
+	if (!_gripper.is_valid()) {
+		PX4_INFO("Gripper is not initialized correctly!");
+		return;
+	}
+
+	send_gripper_vehicle_command(vehicle_command_s::GRIPPER_ACTION_GRAB);
+}
+
+int PayloadDeliverer::print_status()
+{
+	// Gripper status
+	PX4_INFO("Gripper valid: %s", _gripper.is_valid() ? "True" : "False");
+
+	if (_gripper.is_valid()) {
+		PX4_INFO("Gripper state: %s", _gripper.get_state_str());
+	}
+
+	return 0;
+}
+
+int PayloadDeliverer::custom_command(int argc, char *argv[])
+{
+	if (argc >= 1) {
+		// Tests the basic payload open / close ability
+		if (strcmp(argv[0], "gripper_test") == 0) {
+			get_instance()->gripper_test();
+			return 0;
+
+		} else if (strcmp(argv[0], "gripper_open") == 0) {
+			get_instance()->gripper_open();
+			return 0;
+
+		} else if (strcmp(argv[0], "gripper_close") == 0) {
+			get_instance()->gripper_close();
+			return 0;
+		}
+	}
+
+	return print_usage("Unrecognized command");
+}
+
+int PayloadDeliverer::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Handles payload delivery with either Gripper or a Winch with an appropriate timeout / feedback sensor setting,
+and communicates back the delivery result as an acknowledgement internally
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("payload_deliverer", "command");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("gripper_test", "Tests the Gripper's release & grabbing sequence");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("gripper_open", "Opens the gripper");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("gripper_close", "Closes the gripper");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+int PayloadDeliverer::task_spawn(int argc, char *argv[])
+{
+	PayloadDeliverer *instance = new PayloadDeliverer();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("Alloc failed");
+	}
+
+	// Cleanup instance in memory and mark this module as invalid to run
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int payload_deliverer_main(int argc, char *argv[])
+{
+	return PayloadDeliverer::main(argc, argv);
+}
diff --git a/src/modules/payload_deliverer/payload_deliverer.h b/src/modules/payload_deliverer/payload_deliverer.h
new file mode 100644
index 0000000000..bfe7eb2971
--- /dev/null
+++ b/src/modules/payload_deliverer/payload_deliverer.h
@@ -0,0 +1,166 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file payload_deliverer.h
+ * @author Junwoo Hwang (junwoo091400@gmail.com)
+ */
+#include "gripper.h"
+
+#include <drivers/drv_hrt.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/Subscription.hpp>
+#include <uORB/SubscriptionCallback.hpp>
+
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_command_ack.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/parameter_update.h>
+
+using namespace time_literals;
+
+extern "C" __EXPORT int payload_deliverer_main(int argc, char *argv[]);
+
+// If cached gripper action is set to this value, it means we aren't running any vehicle command
+static constexpr int8_t GRIPPER_ACTION_NONE = -1;
+
+/**
+ * @brief Payload Deliverer Module
+ *
+ * Activates a winch / gripper when the DO_WINCH or DO_GRIPPER vehicle command is received,
+ * after which the vehicle_command_ack command gets sent upon successful confirmation
+ * of the payload deployment.
+ *
+ * The module runs based on having a subscription callback mechanism to the 'vehicle_command' topic
+ * as it only needs to run when we receive the relevant vehicle command.
+ *
+ * This module communicates with the Navigator which handles publishing such vehicle commands.
+ */
+class PayloadDeliverer : public ModuleBase<PayloadDeliverer>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+	PayloadDeliverer();
+
+	/** @see ModuleBase **/
+	static int task_spawn(int argc, char *argv[]);
+	static int custom_command(int argc, char *argv[]);
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase. Override "status" output when invoked via Commandline, to give detailed status **/
+	int print_status() override;
+
+	// Initializes the module
+	bool init();
+
+	// Gripper test commands provoked by custom_command() via CLI interface
+	void gripper_test();
+	void gripper_open();
+	void gripper_close();
+
+private:
+	/**
+	 * @brief Main Run function that runs when subscription callback is triggered
+	 */
+	void Run() override;
+
+	/**
+	 * @brief Called to update the parameters and initialize gripper instance if necessary
+	 */
+	void parameter_update();
+
+	/**
+	 * @brief Initialize or deinitialize gripper instance based on parameter settings
+	 *
+	 * Depending on `PD_GRIPPER_EN` and the state of `_gripper` instance, this function will
+	 * either try to initialize or de-initialize the gripper appropriately.
+	 */
+	void configure_gripper();
+
+	/**
+	 * @brief Update gipper instance's state and send vehicle command ack
+	 *
+	 * This updates the gripper instance to check if the gripper has reached the desired state.
+	 * And if so, it sends a vehicle command ack to the navigator.
+	 *
+	 * If the gripper instance isn't valid (i.e. not initialized), it doesn't do anything
+	 */
+	void gripper_update(const hrt_abstime &now);
+
+	/**
+	 * @brief Commands the payload delivery mechanism based on the received vehicle command
+	 *
+	 * Also handle vehicle command acknowledgement once the delivery is confirmed from the mechanism
+	 */
+	void handle_vehicle_command(const hrt_abstime &now, const vehicle_command_s *vehicle_command = nullptr);
+
+	/**
+	 * @brief Send DO_GRIPPER vehicle command with specified gripper action
+	 *
+	 * @param gripper_command GRIPPER_ACTION_GRAB or GRIPPER_ACTION_RELEASE
+	 */
+	bool send_gripper_vehicle_command(const int32_t gripper_action);
+
+	/**
+	 * @brief Send ack response to DO_GRIPPER vehicle command with specified parameters
+	 *
+	 * For the case of VEHICLE_CMD_RESULT_IN_PROGRESS, progress percentage parameter will be filled out
+	 */
+	bool send_gripper_vehicle_command_ack(const hrt_abstime now, const uint8_t command_result, const uint8_t target_system,
+					      const uint8_t target_component);
+
+	Gripper _gripper;
+
+	// Cached values of the currently running vehicle command for the gripper action
+	// used for conflicting vehicle commands & successful vehicle command acknowledgements
+	int8_t _cur_vcmd_gripper_action{GRIPPER_ACTION_NONE};
+	uint8_t _cur_vcmd_target_system{0};
+	uint8_t _cur_vcmd_target_component{0};
+
+	// Subscription
+	uORB::SubscriptionCallbackWorkItem _vehicle_command_sub{this, ORB_ID(vehicle_command)};
+	uORB::SubscriptionInterval         _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updates
+
+	// Publications
+	uORB::Publication<vehicle_command_ack_s> _vehicle_command_ack_pub{ORB_ID(vehicle_command_ack)};
+	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::PD_GRIPPER_TO>)	_param_gripper_timeout_s,
+		(ParamInt<px4::params::PD_GRIPPER_TYPE>)	_param_gripper_type,
+		(ParamBool<px4::params::PD_GRIPPER_EN>)		_param_gripper_enable
+	)
+};
diff --git a/src/modules/rc_update/params.c b/src/modules/rc_update/params.c
index 2ddb20f6db..457ba71e5b 100644
--- a/src/modules/rc_update/params.c
+++ b/src/modules/rc_update/params.c
@@ -1855,6 +1855,34 @@ PARAM_DEFINE_INT32(RC_MAP_PARAM2, 0);
  */
 PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0);
 
+/**
+ * RC channel to engage the main motor (for helicopters)
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ * @value 0 Unassigned
+ * @value 1 Channel 1
+ * @value 2 Channel 2
+ * @value 3 Channel 3
+ * @value 4 Channel 4
+ * @value 5 Channel 5
+ * @value 6 Channel 6
+ * @value 7 Channel 7
+ * @value 8 Channel 8
+ * @value 9 Channel 9
+ * @value 10 Channel 10
+ * @value 11 Channel 11
+ * @value 12 Channel 12
+ * @value 13 Channel 13
+ * @value 14 Channel 14
+ * @value 15 Channel 15
+ * @value 16 Channel 16
+ * @value 17 Channel 17
+ * @value 18 Channel 18
+ */
+PARAM_DEFINE_INT32(RC_MAP_ENG_MOT, 0);
+
 /**
  * Failsafe channel PWM threshold.
  *
@@ -1985,6 +2013,22 @@ PARAM_DEFINE_FLOAT(RC_TRANS_TH, 0.75f);
  */
 PARAM_DEFINE_FLOAT(RC_GEAR_TH, 0.75f);
 
+/**
+ * Threshold for selecting main motor engage
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ * @min -1
+ * @max 1
+ * @group Radio Switches
+ */
+PARAM_DEFINE_FLOAT(RC_ENG_MOT_TH, 0.75f);
+
 /**
  * PWM input channel that provides RSSI.
  *
diff --git a/src/modules/rc_update/rc_update.cpp b/src/modules/rc_update/rc_update.cpp
index ab58f0259a..44ce9a0589 100644
--- a/src/modules/rc_update/rc_update.cpp
+++ b/src/modules/rc_update/rc_update.cpp
@@ -56,7 +56,8 @@ static bool operator ==(const manual_control_switches_s &a, const manual_control
 		a.transition_switch == b.transition_switch &&
 		a.gear_switch == b.gear_switch &&
 		a.photo_switch == b.photo_switch &&
-		a.video_switch == b.video_switch);
+		a.video_switch == b.video_switch &&
+		a.engage_main_motor_switch == b.engage_main_motor_switch);
 }
 
 static bool operator !=(const manual_control_switches_s &a, const manual_control_switches_s &b) { return !(a == b); }
@@ -231,6 +232,8 @@ void RCUpdate::update_rc_functions()
 		_rc.function[rc_channels_s::FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
 	}
 
+	_rc.function[rc_channels_s::FUNCTION_ENGAGE_MAIN_MOTOR] = _param_rc_map_eng_mot.get() - 1;
+
 	map_flight_modes_buttons();
 }
 
@@ -643,7 +646,8 @@ void RCUpdate::UpdateManualSwitches(const hrt_abstime &timestamp_sample)
 	switches.arm_switch        = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ARMSWITCH,  _param_rc_armswitch_th.get());
 	switches.transition_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_TRANSITION, _param_rc_trans_th.get());
 	switches.gear_switch       = get_rc_sw2pos_position(rc_channels_s::FUNCTION_GEAR,       _param_rc_gear_th.get());
-
+	switches.engage_main_motor_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_ENGAGE_MAIN_MOTOR,
+					    _param_rc_eng_mot_th.get());
 #if defined(ATL_MANTIS_RC_INPUT_HACKS)
 	switches.photo_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_AUX_3, 0.5f);
 	switches.video_switch = get_rc_sw2pos_position(rc_channels_s::FUNCTION_AUX_4, 0.5f);
diff --git a/src/modules/rc_update/rc_update.h b/src/modules/rc_update/rc_update.h
index 29bf2fa544..1bbd8a5e11 100644
--- a/src/modules/rc_update/rc_update.h
+++ b/src/modules/rc_update/rc_update.h
@@ -226,6 +226,8 @@ public:
 		(ParamInt<px4::params::RC_MAP_AUX5>) _param_rc_map_aux5,
 		(ParamInt<px4::params::RC_MAP_AUX6>) _param_rc_map_aux6,
 
+		(ParamInt<px4::params::RC_MAP_ENG_MOT>) _param_rc_map_eng_mot,
+
 		(ParamInt<px4::params::RC_FAILS_THR>) _param_rc_fails_thr,
 
 		(ParamFloat<px4::params::RC_LOITER_TH>) _param_rc_loiter_th,
@@ -235,6 +237,7 @@ public:
 		(ParamFloat<px4::params::RC_TRANS_TH>) _param_rc_trans_th,
 		(ParamFloat<px4::params::RC_GEAR_TH>) _param_rc_gear_th,
 		(ParamFloat<px4::params::RC_RETURN_TH>) _param_rc_return_th,
+		(ParamFloat<px4::params::RC_ENG_MOT_TH>) _param_rc_eng_mot_th,
 
 		(ParamInt<px4::params::RC_CHAN_CNT>) _param_rc_chan_cnt
 	)
diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp
index eb3e39c0e9..7b3c7c1acf 100644
--- a/src/modules/rover_pos_control/RoverPositionControl.cpp
+++ b/src/modules/rover_pos_control/RoverPositionControl.cpp
@@ -152,13 +152,10 @@ RoverPositionControl::manual_control_setpoint_poll()
 					_attitude_sp_pub.publish(_att_sp);
 
 				} else {
-					_act_controls.control[actuator_controls_s::INDEX_ROLL] = 0.0f; // Nominally roll: _manual_control_setpoint.y;
-					_act_controls.control[actuator_controls_s::INDEX_PITCH] = 0.0f; // Nominally pitch: -_manual_control_setpoint.x;
-					// Set heading from the manual roll input channel
-					_act_controls.control[actuator_controls_s::INDEX_YAW] =
-						_manual_control_setpoint.y; // Nominally yaw: _manual_control_setpoint.r;
-					// Set throttle from the manual throttle channel
-					_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = _manual_control_setpoint.z;
+					_act_controls.control[actuator_controls_s::INDEX_ROLL] = _manual_control_setpoint.r;
+					_act_controls.control[actuator_controls_s::INDEX_PITCH] = -_manual_control_setpoint.x;
+					_act_controls.control[actuator_controls_s::INDEX_YAW] = _manual_control_setpoint.y;
+					_act_controls.control[actuator_controls_s::INDEX_THROTTLE] = (_manual_control_setpoint.z - 0.5f) * 2.f;
 					_reset_yaw_sp = true;
 				}
 
diff --git a/src/modules/sensors/module.yaml b/src/modules/sensors/module.yaml
index d64b87de93..a00f045b18 100644
--- a/src/modules/sensors/module.yaml
+++ b/src/modules/sensors/module.yaml
@@ -14,6 +14,7 @@ parameters:
             category: System
             type: int32
             default: 0
+            decimal: 3
             num_instances: *max_num_sensor_instances
             instance_start: 0
 
@@ -31,6 +32,7 @@ parameters:
                 75: High
                 100: Max
             default: -1
+            decimal: 3
             num_instances: *max_num_sensor_instances
             instance_start: 0
 
@@ -96,7 +98,8 @@ parameters:
             category: System
             type: float
             default: 0.0
-            unit: m
+            decimal: 3
+            unit: m/s^2
             volatile: true
             num_instances: *max_num_sensor_instances
             instance_start: 0
@@ -107,7 +110,8 @@ parameters:
             category: System
             type: float
             default: 0.0
-            unit: m
+            decimal: 3
+            unit: m/s^2
             volatile: true
             num_instances: *max_num_sensor_instances
             instance_start: 0
@@ -118,7 +122,8 @@ parameters:
             category: System
             type: float
             default: 0.0
-            unit: m
+            decimal: 3
+            unit: m/s^2
             volatile: true
             num_instances: *max_num_sensor_instances
             instance_start: 0
@@ -129,6 +134,7 @@ parameters:
             category: System
             type: float
             default: 1.0
+            decimal: 3
             min: 0.1
             max: 3.0
             volatile: true
@@ -141,6 +147,7 @@ parameters:
             category: System
             type: float
             default: 1.0
+            decimal: 3
             min: 0.1
             max: 3.0
             volatile: true
@@ -153,6 +160,7 @@ parameters:
             category: System
             type: float
             default: 1.0
+            decimal: 3
             min: 0.1
             max: 3.0
             volatile: true
diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp
index b37d91c6ba..ab7f258308 100644
--- a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp
+++ b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp
@@ -462,6 +462,8 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
 		}
 	}
 
+	gps_blended_state.time_utc_usec = _gps_state[gps_best_index].time_utc_usec;
+
 	// Add the sum of weighted offsets to the reference position to obtain the blended position
 	const double lat_deg_now = (double)gps_blended_state.lat * 1.0e-7;
 	const double lon_deg_now = (double)gps_blended_state.lon * 1.0e-7;
diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
index 261b09a6ff..a86d79808e 100644
--- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
@@ -620,15 +620,18 @@ bool VehicleIMU::Publish()
 
 			// vehicle_imu_status
 			//  publish before vehicle_imu so that error counts are available synchronously if needed
-			if (_raw_accel_mean.valid() && _raw_gyro_mean.valid()
-			    && (_publish_status || (hrt_elapsed_time(&_status.timestamp) >= 100_ms))) {
+			const bool imu_status_publishing_interval_exceeded = hrt_elapsed_time(&_status.timestamp) >=
+					kIMUStatusPublishingInterval;
 
+			if (_raw_accel_mean.valid() && _raw_gyro_mean.valid()
+			    && (_publish_status || imu_status_publishing_interval_exceeded)) {
+
+				// Accel
 				_status.accel_device_id = _accel_calibration.device_id();
 
 				// accel mean and variance
 				Vector3f(_accel_calibration.rotation() * _raw_accel_mean.mean()).copyTo(_status.mean_accel);
 				Vector3f(_accel_calibration.rotation() * _raw_accel_mean.variance()).copyTo(_status.var_accel);
-				_raw_accel_mean.reset();
 
 				// accel temperature
 				_status.temperature_accel = _accel_temperature_sum / _accel_temperature_sum_count;
@@ -636,12 +639,12 @@ bool VehicleIMU::Publish()
 				_accel_temperature_sum_count = 0;
 
 
+				// Gyro
 				_status.gyro_device_id = _gyro_calibration.device_id();
 
 				// gyro mean and variance
 				Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.mean()).copyTo(_status.mean_gyro);
 				Vector3f(_gyro_calibration.rotation() * _raw_gyro_mean.variance()).copyTo(_status.var_gyro);
-				_raw_gyro_mean.reset();
 
 				// Gyro delta angle coning metric = length of coning corrections averaged since last status publication
 				_status.delta_angle_coning_metric = _coning_norm_accum / _coning_norm_accum_total_time_s;
@@ -653,11 +656,16 @@ bool VehicleIMU::Publish()
 				_gyro_temperature_sum = NAN;
 				_gyro_temperature_sum_count = 0;
 
-
+				// publish
 				_status.timestamp = hrt_absolute_time();
 				_vehicle_imu_status_pub.publish(_status);
 
 				_publish_status = false;
+
+				if (imu_status_publishing_interval_exceeded) {
+					_raw_accel_mean.reset();
+					_raw_gyro_mean.reset();
+				}
 			}
 
 			// publish vehicle_imu
diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
index 6420fbb13b..3b5b46dc56 100644
--- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
+++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
@@ -95,6 +95,8 @@ private:
 	uORB::PublicationMulti<vehicle_imu_s> _vehicle_imu_pub{ORB_ID(vehicle_imu)};
 	uORB::PublicationMulti<vehicle_imu_status_s> _vehicle_imu_status_pub{ORB_ID(vehicle_imu_status)};
 
+	static constexpr hrt_abstime kIMUStatusPublishingInterval{100_ms};
+
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	// Used to check, save and use learned magnetometer biases
@@ -117,13 +119,13 @@ private:
 	hrt_abstime _gyro_timestamp_sample_last{0};
 	hrt_abstime _gyro_timestamp_last{0};
 
-	math::WelfordMean<matrix::Vector3f> _raw_accel_mean{};
-	math::WelfordMean<matrix::Vector3f> _raw_gyro_mean{};
+	math::WelfordMean<float, 3> _raw_accel_mean{};
+	math::WelfordMean<float, 3> _raw_gyro_mean{};
 
-	math::WelfordMean<matrix::Vector2f> _accel_interval_mean{};
-	math::WelfordMean<matrix::Vector2f> _gyro_interval_mean{};
+	math::WelfordMean<float, 2> _accel_interval_mean{};
+	math::WelfordMean<float, 2> _gyro_interval_mean{};
 
-	math::WelfordMean<matrix::Vector2f> _gyro_update_latency_mean{};
+	math::WelfordMean<float, 2> _gyro_update_latency_mean{};
 
 	float _accel_interval_best_variance{(float)INFINITY};
 	float _gyro_interval_best_variance{(float)INFINITY};
diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
index 48c50365c4..2fd4b3e8b3 100644
--- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
+++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
@@ -261,9 +261,8 @@ void VehicleMagnetometer::UpdateMagCalibration()
 
 							_mag_cal[i].device_id = estimator_sensor_bias.mag_device_id;
 
-							// readd estimated bias that was removed before publishing vehicle_magnetometer
-							_mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias) +
-									     _calibration_estimator_bias[mag_index];
+							// readd estimated bias that was removed before publishing vehicle_magnetometer (_calibration_estimator_bias)
+							_mag_cal[i].offset = _calibration[mag_index].BiasCorrectedSensorOffset(bias + _calibration_estimator_bias[mag_index]);
 
 							_mag_cal[i].variance = bias_variance;
 
@@ -312,16 +311,12 @@ void VehicleMagnetometer::UpdateMagCalibration()
 
 						_calibration[mag_index].ParametersSave();
 
+						_calibration_estimator_bias[mag_index].zero();
+
 						calibration_param_save_needed = true;
 					}
-
 				}
-
-				// clear
-				_mag_cal[i] = {};
 			}
-
-			_calibration_estimator_bias[mag_index].zero();
 		}
 
 		if (calibration_param_save_needed) {
@@ -329,6 +324,11 @@ void VehicleMagnetometer::UpdateMagCalibration()
 			_last_calibration_update = hrt_absolute_time();
 		}
 
+		// clear all
+		for (int i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+			_mag_cal[i] = {};
+		}
+
 		_in_flight_mag_cal_available = false;
 	}
 }
@@ -428,13 +428,6 @@ void VehicleMagnetometer::Run()
 							}
 						}
 
-						// advertise outputs in order if publishing all
-						if (!_param_sens_mag_mode.get()) {
-							for (int instance = 0; instance < uorb_index; instance++) {
-								_vehicle_magnetometer_pub[instance].advertise();
-							}
-						}
-
 						if (_selected_sensor_sub_index < 0) {
 							_sensor_sub[uorb_index].registerCallback();
 						}
@@ -515,6 +508,16 @@ void VehicleMagnetometer::Run()
 						out.timestamp = hrt_absolute_time();
 
 						if (multi_mode) {
+
+							if (!_vehicle_magnetometer_pub[instance].advertised()) {
+								// prefer to maintain vehicle_magneometer instance numbering in sensor order
+								for (int mag_instance = 0; mag_instance < instance; mag_instance++) {
+									if (_calibration[instance].enabled()) {
+										_vehicle_magnetometer_pub[mag_instance].advertise();
+									}
+								}
+							}
+
 							_vehicle_magnetometer_pub[instance].publish(out);
 
 						} else {
diff --git a/src/modules/sensors/vehicle_magnetometer/mag_compensation/python/mag_compensation.py b/src/modules/sensors/vehicle_magnetometer/mag_compensation/python/mag_compensation.py
old mode 100755
new mode 100644
diff --git a/src/modules/simulator/CMakeLists.txt b/src/modules/simulator/CMakeLists.txt
index fee2a1b709..79fa1f4539 100644
--- a/src/modules/simulator/CMakeLists.txt
+++ b/src/modules/simulator/CMakeLists.txt
@@ -56,7 +56,7 @@ px4_add_module(
 		-Wno-address-of-packed-member # TODO: fix in c_library_v2
 	INCLUDES
 		${CMAKE_BINARY_DIR}/mavlink
-		${CMAKE_BINARY_DIR}/mavlink/development
+		${CMAKE_BINARY_DIR}/mavlink/auterion
 	SRCS
 		simulator.cpp
 		simulator_mavlink.cpp
diff --git a/src/modules/simulator/battery_simulator/BatterySimulator.cpp b/src/modules/simulator/battery_simulator/BatterySimulator.cpp
index bb765e637f..54588eac94 100644
--- a/src/modules/simulator/battery_simulator/BatterySimulator.cpp
+++ b/src/modules/simulator/battery_simulator/BatterySimulator.cpp
@@ -97,7 +97,8 @@ void BatterySimulator::Run()
 	float ibatt = -1.0f; // no current sensor in simulation
 
 	_battery_percentage = math::max(_battery_percentage, _param_bat_min_pct.get() / 100.f);
-	float vbatt = math::gradual(_battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(), _battery.full_cell_voltage());
+	float vbatt = math::interpolate(_battery_percentage, 0.f, 1.f, _battery.empty_cell_voltage(),
+					_battery.full_cell_voltage());
 	vbatt *= _battery.cell_count();
 
 	_battery.setConnected(true);
diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp
index 09b8925092..3f6cb38282 100644
--- a/src/modules/simulator/simulator_mavlink.cpp
+++ b/src/modules/simulator/simulator_mavlink.cpp
@@ -163,6 +163,13 @@ void Simulator::actuator_controls_from_outputs(mavlink_hil_actuator_controls_t *
 			is_fixed_wing = true;
 			break;
 
+		case MAV_TYPE_VTOL_RESERVED4:
+			// this is the tricopter VTOL / quad plane with 3 motors and 2 servos
+			pos_thrust_motors_count = 3;
+			is_fixed_wing = false;
+			break;
+
+
 		default:
 			pos_thrust_motors_count = 0;
 			is_fixed_wing = false;
@@ -1301,8 +1308,8 @@ void Simulator::check_failure_injections()
 			ack.command = vehicle_command.command;
 			ack.from_external = false;
 			ack.result = supported ?
-				     vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED :
-				     vehicle_command_ack_s::VEHICLE_RESULT_UNSUPPORTED;
+				     vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED :
+				     vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 			ack.timestamp = hrt_absolute_time();
 			_command_ack_pub.publish(ack);
 		}
diff --git a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp
index 161ce9ef07..f11c2bba1f 100644
--- a/src/modules/temperature_compensation/TemperatureCompensationModule.cpp
+++ b/src/modules/temperature_compensation/TemperatureCompensationModule.cpp
@@ -226,10 +226,10 @@ void TemperatureCompensationModule::Run()
 					vehicle_command_ack_s command_ack{};
 
 					if (ret == 0) {
-						command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
+						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 					} else {
-						command_ack.result = vehicle_command_s::VEHICLE_CMD_RESULT_FAILED;
+						command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_FAILED;
 					}
 
 					command_ack.timestamp = hrt_absolute_time();
diff --git a/src/modules/temperature_compensation/temperature_calibration/accel.cpp b/src/modules/temperature_compensation/temperature_calibration/accel.cpp
index d9d4ef2ae6..9dc6c071f8 100644
--- a/src/modules/temperature_compensation/temperature_calibration/accel.cpp
+++ b/src/modules/temperature_compensation/temperature_calibration/accel.cpp
@@ -225,5 +225,16 @@ int TemperatureCalibrationAccel::finish_sensor_instance(PerSensorData &data, int
 	set_parameter("TC_A%d_TMAX", sensor_index, &data.high_temp);
 	set_parameter("TC_A%d_TMIN", sensor_index, &data.low_temp);
 	set_parameter("TC_A%d_TREF", sensor_index, &data.ref_temp);
+
+	// reset current calibration (covered by TC parameters)
+	float offset = 0.0f;
+	float scale = 1.0f;
+	set_parameter("CAL_ACC%u_XOFF", sensor_index, &offset);
+	set_parameter("CAL_ACC%u_YOFF", sensor_index, &offset);
+	set_parameter("CAL_ACC%u_ZOFF", sensor_index, &offset);
+	set_parameter("CAL_ACC%u_XSCALE", sensor_index, &scale);
+	set_parameter("CAL_ACC%u_YSCALE", sensor_index, &scale);
+	set_parameter("CAL_ACC%u_ZSCALE", sensor_index, &scale);
+
 	return 0;
 }
diff --git a/src/modules/temperature_compensation/temperature_calibration/baro.cpp b/src/modules/temperature_compensation/temperature_calibration/baro.cpp
index bd535e3432..4d1dc7be90 100644
--- a/src/modules/temperature_compensation/temperature_calibration/baro.cpp
+++ b/src/modules/temperature_compensation/temperature_calibration/baro.cpp
@@ -209,5 +209,10 @@ int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int
 	set_parameter("TC_B%d_TMAX", sensor_index, &data.high_temp);
 	set_parameter("TC_B%d_TMIN", sensor_index, &data.low_temp);
 	set_parameter("TC_B%d_TREF", sensor_index, &data.ref_temp);
+
+	// reset current calibration (covered by TC parameters)
+	float offset = 0.0f;
+	set_parameter("CAL_BARO%u_OFF", sensor_index, &offset);
+
 	return 0;
 }
diff --git a/src/modules/temperature_compensation/temperature_calibration/gyro.cpp b/src/modules/temperature_compensation/temperature_calibration/gyro.cpp
index 22cb0f1fc0..0f0f9c4a1e 100644
--- a/src/modules/temperature_compensation/temperature_calibration/gyro.cpp
+++ b/src/modules/temperature_compensation/temperature_calibration/gyro.cpp
@@ -211,5 +211,11 @@ int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int
 	set_parameter("TC_G%d_TMIN", sensor_index, &data.low_temp);
 	set_parameter("TC_G%d_TREF", sensor_index, &data.ref_temp);
 
+	// reset current calibration (covered by TC parameters)
+	float offset = 0.0f;
+	set_parameter("CAL_GYRO%u_XOFF", sensor_index, &offset);
+	set_parameter("CAL_GYRO%u_YOFF", sensor_index, &offset);
+	set_parameter("CAL_GYRO%u_ZOFF", sensor_index, &offset);
+
 	return 0;
 }
diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp
index a6c86b48f1..a50bb36ef8 100644
--- a/src/modules/vtol_att_control/standard.cpp
+++ b/src/modules/vtol_att_control/standard.cpp
@@ -52,13 +52,6 @@ using namespace matrix;
 Standard::Standard(VtolAttitudeControl *attc) :
 	VtolType(attc)
 {
-	_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
-	_vtol_schedule.transition_start = 0;
-
-	_mc_roll_weight = 1.0f;
-	_mc_pitch_weight = 1.0f;
-	_mc_yaw_weight = 1.0f;
-	_mc_throttle_weight = 1.0f;
 }
 
 void
@@ -78,11 +71,10 @@ void Standard::update_vtol_state()
 	 */
 
 	float mc_weight = _mc_roll_weight;
-	float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 	if (_vtol_vehicle_status->vtol_transition_failsafe) {
 		// Failsafe event, engage mc motors immediately
-		_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+		_vtol_mode = vtol_mode::MC_MODE;
 		_pusher_throttle = 0.0f;
 		_reverse_output = 0.0f;
 
@@ -94,27 +86,28 @@ void Standard::update_vtol_state()
 	} else if (!_attc->is_fixed_wing_requested()) {
 
 		// the transition to fw mode switch is off
-		if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+		if (_vtol_mode == vtol_mode::MC_MODE) {
 			// in mc mode
-			_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+			_vtol_mode = vtol_mode::MC_MODE;
 			mc_weight = 1.0f;
-			_pusher_throttle = 0.0f;
 			_reverse_output = 0.0f;
 
-		} else if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
+		} else if (_vtol_mode == vtol_mode::FW_MODE) {
 			// Regular backtransition
-			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_MC;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			_vtol_mode = vtol_mode::TRANSITION_TO_MC;
+			_transition_start_timestamp = hrt_absolute_time();
+			// TODO: rework this
+			_time_since_trans_start = 0.f;
 			_reverse_output = _param_vt_b_rev_out.get();
 
-		} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
+		} else if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) {
 			// failsafe back to mc mode
-			_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+			_vtol_mode = vtol_mode::MC_MODE;
 			mc_weight = 1.0f;
 			_pusher_throttle = 0.0f;
 			_reverse_output = 0.0f;
 
-		} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
+		} else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) {
 			// speed exit condition: use ground if valid, otherwise airspeed
 			bool exit_backtransition_speed_condition = false;
 
@@ -127,33 +120,35 @@ void Standard::update_vtol_state()
 				exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get();
 			}
 
-			const bool exit_backtransition_time_condition = time_since_trans_start > _param_vt_b_trans_dur.get();
+			const bool exit_backtransition_time_condition = _time_since_trans_start > _param_vt_b_trans_dur.get();
 
 			if (can_transition_on_ground() || exit_backtransition_speed_condition || exit_backtransition_time_condition) {
-				_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+				_vtol_mode = vtol_mode::MC_MODE;
 			}
 		}
 
 	} else {
 		// the transition to fw mode switch is on
-		if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE || _vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
+		if (_vtol_mode == vtol_mode::MC_MODE || _vtol_mode == vtol_mode::TRANSITION_TO_MC) {
 			// start transition to fw mode
 			/* NOTE: The failsafe transition to fixed-wing was removed because it can result in an
 			 * unsafe flying state. */
-			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_FW;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			_vtol_mode = vtol_mode::TRANSITION_TO_FW;
+			_transition_start_timestamp = hrt_absolute_time();
+			// TODO: rework this
+			_time_since_trans_start = 0.f;
 
-		} else if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
+		} else if (_vtol_mode == vtol_mode::FW_MODE) {
 			// in fw mode
-			_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
+			_vtol_mode = vtol_mode::FW_MODE;
 			mc_weight = 0.0f;
 
-		} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
+		} else if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) {
 			// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
 
 			const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
 					&& !_param_fw_arsp_mode.get();
-			const bool minimum_trans_time_elapsed = time_since_trans_start > getMinimumFrontTransitionTime();
+			const bool minimum_trans_time_elapsed = _time_since_trans_start > getMinimumFrontTransitionTime();
 
 			bool transition_to_fw = false;
 
@@ -169,7 +164,7 @@ void Standard::update_vtol_state()
 			transition_to_fw |= can_transition_on_ground();
 
 			if (transition_to_fw) {
-				_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
+				_vtol_mode = vtol_mode::FW_MODE;
 
 				// don't set pusher throttle here as it's being ramped up elsewhere
 				_trans_finished_ts = hrt_absolute_time();
@@ -183,51 +178,61 @@ void Standard::update_vtol_state()
 	_mc_throttle_weight = mc_weight;
 
 	// map specific control phases to simple control modes
-	switch (_vtol_schedule.flight_mode) {
+	switch (_vtol_mode) {
 	case vtol_mode::MC_MODE:
-		_vtol_mode = mode::ROTARY_WING;
+		_common_vtol_mode = mode::ROTARY_WING;
 		break;
 
 	case vtol_mode::FW_MODE:
-		_vtol_mode = mode::FIXED_WING;
+		_common_vtol_mode = mode::FIXED_WING;
 		break;
 
 	case vtol_mode::TRANSITION_TO_FW:
-		_vtol_mode = mode::TRANSITION_TO_FW;
+		_common_vtol_mode = mode::TRANSITION_TO_FW;
 		break;
 
 	case vtol_mode::TRANSITION_TO_MC:
-		_vtol_mode = mode::TRANSITION_TO_MC;
+		_common_vtol_mode = mode::TRANSITION_TO_MC;
 		break;
 	}
 }
 
 void Standard::update_transition_state()
 {
+	const hrt_abstime now = hrt_absolute_time();
 	float mc_weight = 1.0f;
-	float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 	VtolType::update_transition_state();
 
-	// we get attitude setpoint from a multirotor flighttask if altitude is controlled.
+	// we get attitude setpoint from a multirotor flighttask if climbrate is controlled.
 	// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
 	if (_v_control_mode->flag_control_climb_rate_enabled) {
+		// we need the incoming (virtual) attitude setpoints (both mc and fw) to be recent, otherwise return (means the previous setpoint stays active)
+		if (_mc_virtual_att_sp->timestamp < (now - 1_s) || _fw_virtual_att_sp->timestamp < (now - 1_s)) {
+			return;
+		}
+
 		memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 		_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
 
 	} else {
+		// we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active)
+		if (_fw_virtual_att_sp->timestamp < (now - 1_s)) {
+			return;
+		}
+
 		memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 		_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
 	}
 
-	if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
-		if (_param_vt_psher_rmp_dt.get() <= 0.0f) {
+	if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) {
+		if (_param_vt_psher_rmp_dt.get() <= FLT_EPSILON) {
 			// just set the final target throttle value
 			_pusher_throttle = _param_vt_f_trans_thr.get();
 
 		} else if (_pusher_throttle <= _param_vt_f_trans_thr.get()) {
 			// ramp up throttle to the target throttle value
-			_pusher_throttle = _param_vt_f_trans_thr.get() * time_since_trans_start / _param_vt_psher_rmp_dt.get();
+			_pusher_throttle = _param_vt_f_trans_thr.get() * _time_since_trans_start / _param_vt_psher_rmp_dt.get();
 		}
 
 		_airspeed_trans_blend_margin = _param_vt_arsp_trans.get() - _param_vt_arsp_blend.get();
@@ -237,14 +242,14 @@ void Standard::update_transition_state()
 		    PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
 		    _airspeed_validated->calibrated_airspeed_m_s > 0.0f &&
 		    _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get() &&
-		    time_since_trans_start > getMinimumFrontTransitionTime()) {
+		    _time_since_trans_start > getMinimumFrontTransitionTime()) {
 
 			mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) /
 				    _airspeed_trans_blend_margin;
 			// time based blending when no airspeed sensor is set
 
 		} else if (_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
-			mc_weight = 1.0f - time_since_trans_start / getMinimumFrontTransitionTime();
+			mc_weight = 1.0f - _time_since_trans_start / getMinimumFrontTransitionTime();
 			mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
 
 		}
@@ -255,19 +260,11 @@ void Standard::update_transition_state()
 		const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
 		q_sp.copyTo(_v_att_sp->q_d);
 
-		// check front transition timeout
-		if (_param_vt_trans_timeout.get() > FLT_EPSILON) {
-			if (time_since_trans_start > _param_vt_trans_timeout.get()) {
-				// transition timeout occured, abort transition
-				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
-			}
-		}
-
 		// set spoiler and flaps to 0
 		_flaps_setpoint_with_slewrate.update(0.f, _dt);
 		_spoiler_setpoint_with_slewrate.update(0.f, _dt);
 
-	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) {
+	} else if (_vtol_mode == vtol_mode::TRANSITION_TO_MC) {
 
 		if (_v_control_mode->flag_control_climb_rate_enabled) {
 			// control backtransition deceleration using pitch.
@@ -279,16 +276,16 @@ void Standard::update_transition_state()
 
 		_pusher_throttle = 0.0f;
 
-		if (time_since_trans_start >= _param_vt_b_rev_del.get()) {
+		if (_time_since_trans_start >= _param_vt_b_rev_del.get()) {
 			// Handle throttle reversal for active breaking
-			float thrscale = (time_since_trans_start - _param_vt_b_rev_del.get()) / (_param_vt_psher_rmp_dt.get());
+			float thrscale = (_time_since_trans_start - _param_vt_b_rev_del.get()) / (_param_vt_psher_rmp_dt.get());
 			thrscale = math::constrain(thrscale, 0.0f, 1.0f);
 			_pusher_throttle = thrscale * _param_vt_b_trans_thr.get();
 		}
 
 		// continually increase mc attitude control as we transition back to mc mode
 		if (_param_vt_b_trans_ramp.get() > FLT_EPSILON) {
-			mc_weight = time_since_trans_start / _param_vt_b_trans_ramp.get();
+			mc_weight = _time_since_trans_start / _param_vt_b_trans_ramp.get();
 		}
 
 		set_all_motor_state(motor_state::ENABLED);
@@ -331,7 +328,7 @@ void Standard::fill_actuator_outputs()
 	auto &mc_out = _actuators_out_0->control;
 	auto &fw_out = _actuators_out_1->control;
 
-	switch (_vtol_schedule.flight_mode) {
+	switch (_vtol_mode) {
 	case vtol_mode::MC_MODE:
 
 		// MC out = MC in
@@ -339,7 +336,7 @@ void Standard::fill_actuator_outputs()
 		mc_out[actuator_controls_s::INDEX_PITCH]        = mc_in[actuator_controls_s::INDEX_PITCH];
 		mc_out[actuator_controls_s::INDEX_YAW]          = mc_in[actuator_controls_s::INDEX_YAW];
 		mc_out[actuator_controls_s::INDEX_THROTTLE]     = mc_in[actuator_controls_s::INDEX_THROTTLE];
-		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN;
+		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = mc_in[actuator_controls_s::INDEX_LANDING_GEAR];
 
 		// FW out = 0, other than roll and pitch depending on elevon lock
 		fw_out[actuator_controls_s::INDEX_ROLL]         = _param_vt_elev_mc_lock.get() ? 0 :
@@ -422,6 +419,10 @@ void Standard::fill_actuator_outputs()
 	_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
 	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
 
+	if (_in_actuator_test_mode) {
+		_in_actuator_test_mode = override_controls_for_test_mode();
+	}
+
 	_actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time();
 }
 
diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h
index 70963e008f..73f7a7da48 100644
--- a/src/modules/vtol_att_control/standard.h
+++ b/src/modules/vtol_att_control/standard.h
@@ -72,10 +72,7 @@ private:
 		FW_MODE
 	};
 
-	struct {
-		vtol_mode flight_mode;			// indicates in which mode the vehicle is in
-		hrt_abstime transition_start;	// at what time did we start a transition (front- or backtransition)
-	} _vtol_schedule;
+	vtol_mode _vtol_mode{vtol_mode::MC_MODE};			/**< vtol flight mode, defined by enum vtol_mode */
 
 	float _pusher_throttle{0.0f};
 	float _reverse_output{0.0f};
diff --git a/src/modules/vtol_att_control/standard_params.c b/src/modules/vtol_att_control/standard_params.c
index 3129619cec..0e1eeec0d9 100644
--- a/src/modules/vtol_att_control/standard_params.c
+++ b/src/modules/vtol_att_control/standard_params.c
@@ -41,21 +41,23 @@
 
 
 /**
- * Enable/disable usage of fixed-wing actuators in hover to generate forward force (instead of pitching down).
+ * Enable usage of fixed-wing actuators in hover to generate forward force (instead of pitching down).
  *
- * This technique can be used to avoid the plane having to pitch down in order to move forward.
- * This prevents large, negative lift values being created when facing strong winds.
+ * This feature can be used to avoid the plane having to pitch down in order to move forward,
+ * and prevents large, negative lift values being created when facing strong winds.
  * Fixed-wing forward actuators refers to puller/pusher (standard VTOL), or forward-tilt (tiltrotor VTOL).
- * Only active if demaded down pitch is below VT_PITCH_MIN, and uses VT_FWD_THRUST_SC to get from
- * demanded down pitch to fixed-wing actuation.
+ * Only active if demanded down pitch is below VT_PITCH_MIN.
+ * Use VT_FWD_THRUST_SC to tune it.
  *
- * @value 0 Disable FW forward actuation in hover.
- * @value 1 Enable FW forward actuation in hover in altitude, position and auto modes (except LANDING).
- * @value 2 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT1.
- * @value 3 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT2.
- * @value 4 Enable FW forward actuation in hover in altitude, position and auto modes.
- * @value 5 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT1 (except LANDING).
- * @value 6 Enable FW forward actuation in hover in altitude, position and auto modes if above MPC_LAND_ALT2 (except LANDING).
+ * Only active (if enabled) in Altitude, Position and Auto modes, not in Stabilized.
+ *
+ * @value 0 Disabled
+ * @value 1 Enabled (except LANDING)
+ * @value 2 Enabled if distance to ground above MPC_LAND_ALT1
+ * @value 3 Enabled if distance to ground above MPC_LAND_ALT2
+ * @value 4 Enabled constantly
+ * @value 5 Enabled if distance to ground above MPC_LAND_ALT1 (except LANDING)
+ * @value 6 Enabled if distance to ground above MPC_LAND_ALT2 (except LANDING)
  *
  * @group VTOL Attitude Control
  */
diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp
index a4209531dc..9b56f7a2de 100644
--- a/src/modules/vtol_att_control/tailsitter.cpp
+++ b/src/modules/vtol_att_control/tailsitter.cpp
@@ -52,10 +52,6 @@ using namespace matrix;
 Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
 	VtolType(attc)
 {
-	_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
-	_vtol_schedule.transition_start = 0;
-
-	_flag_was_in_trans_mode = false;
 }
 
 void
@@ -77,7 +73,7 @@ void Tailsitter::update_vtol_state()
 
 	if (_vtol_vehicle_status->vtol_transition_failsafe) {
 		// Failsafe event, switch to MC mode immediately
-		_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+		_vtol_mode = vtol_mode::MC_MODE;
 
 		//reset failsafe when FW is no longer requested
 		if (!_attc->is_fixed_wing_requested()) {
@@ -86,26 +82,27 @@ void Tailsitter::update_vtol_state()
 
 	} else if (!_attc->is_fixed_wing_requested()) {
 
-		switch (_vtol_schedule.flight_mode) { // user switchig to MC mode
+		switch (_vtol_mode) { // user switchig to MC mode
 		case vtol_mode::MC_MODE:
 			break;
 
 		case vtol_mode::FW_MODE:
-			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_BACK;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			_vtol_mode = vtol_mode::TRANSITION_BACK;
+			_transition_start_timestamp = hrt_absolute_time();
+			// TODO: rework this
+			_time_since_trans_start = 0.f;
 			break;
 
 		case vtol_mode::TRANSITION_FRONT_P1:
 			// failsafe into multicopter mode
-			_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+			_vtol_mode = vtol_mode::MC_MODE;
 			break;
 
 		case vtol_mode::TRANSITION_BACK:
-			float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 			// check if we have reached pitch angle to switch to MC mode
-			if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _param_vt_b_trans_dur.get()) {
-				_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+			if (pitch >= PITCH_TRANSITION_BACK || _time_since_trans_start > _param_vt_b_trans_dur.get()) {
+				_vtol_mode = vtol_mode::MC_MODE;
 			}
 
 			break;
@@ -113,11 +110,13 @@ void Tailsitter::update_vtol_state()
 
 	} else {  // user switchig to FW mode
 
-		switch (_vtol_schedule.flight_mode) {
+		switch (_vtol_mode) {
 		case vtol_mode::MC_MODE:
 			// initialise a front transition
-			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P1;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			_vtol_mode = vtol_mode::TRANSITION_FRONT_P1;
+			_transition_start_timestamp = hrt_absolute_time();
+			// TODO: rework this
+			_time_since_trans_start = 0.f;
 			break;
 
 		case vtol_mode::FW_MODE:
@@ -125,9 +124,6 @@ void Tailsitter::update_vtol_state()
 
 		case vtol_mode::TRANSITION_FRONT_P1: {
 
-				const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
-
-
 				const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
 						&& !_param_fw_arsp_mode.get() ;
 
@@ -145,15 +141,7 @@ void Tailsitter::update_vtol_state()
 				transition_to_fw |= can_transition_on_ground();
 
 				if (transition_to_fw) {
-					_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
-				}
-
-				// check front transition timeout
-				if (_param_vt_trans_timeout.get()  > FLT_EPSILON) {
-					if (time_since_trans_start > _param_vt_trans_timeout.get()) {
-						// transition timeout occured, abort transition
-						_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
-					}
+					_vtol_mode = vtol_mode::FW_MODE;
 				}
 
 				break;
@@ -161,41 +149,46 @@ void Tailsitter::update_vtol_state()
 
 		case vtol_mode::TRANSITION_BACK:
 			// failsafe into fixed wing mode
-			_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
+			_vtol_mode = vtol_mode::FW_MODE;
 			break;
 		}
 	}
 
 	// map tailsitter specific control phases to simple control modes
-	switch (_vtol_schedule.flight_mode) {
+	switch (_vtol_mode) {
 	case vtol_mode::MC_MODE:
-		_vtol_mode = mode::ROTARY_WING;
+		_common_vtol_mode = mode::ROTARY_WING;
 		_flag_was_in_trans_mode = false;
 		break;
 
 	case vtol_mode::FW_MODE:
-		_vtol_mode = mode::FIXED_WING;
+		_common_vtol_mode = mode::FIXED_WING;
 		_flag_was_in_trans_mode = false;
 		break;
 
 	case vtol_mode::TRANSITION_FRONT_P1:
-		_vtol_mode = mode::TRANSITION_TO_FW;
+		_common_vtol_mode = mode::TRANSITION_TO_FW;
 		break;
 
 	case vtol_mode::TRANSITION_BACK:
-		_vtol_mode = mode::TRANSITION_TO_MC;
+		_common_vtol_mode = mode::TRANSITION_TO_MC;
 		break;
 	}
 }
 
 void Tailsitter::update_transition_state()
 {
-	const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
+	const hrt_abstime now = hrt_absolute_time();
+
+	// we need the incoming (virtual) mc attitude setpoints to be recent, otherwise return (means the previous setpoint stays active)
+	if (_mc_virtual_att_sp->timestamp < (now - 1_s)) {
+		return;
+	}
 
 	if (!_flag_was_in_trans_mode) {
 		_flag_was_in_trans_mode = true;
 
-		if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
+		if (_vtol_mode == vtol_mode::TRANSITION_BACK) {
 			// calculate rotation axis for transition.
 			_q_trans_start = Quatf(_v_att->q);
 			Vector3f z = -_q_trans_start.dcm_z();
@@ -205,14 +198,21 @@ void Tailsitter::update_transition_state()
 			float yaw_sp = atan2f(z(1), z(0));
 
 			// the intial attitude setpoint for a backtransition is a combination of the current fw pitch setpoint,
-			// the yaw setpoint and zero roll since we want wings level transition
-			_q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp);
+			// the yaw setpoint and zero roll since we want wings level transition.
+			// If for some reason the fw attitude setpoint is not recent then don't sue it and assume 0 pitch
+			if (_fw_virtual_att_sp->timestamp > (now - 1_s)) {
+				_q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp);
+
+			} else {
+				_q_trans_start = Eulerf(0.0f, 0.f, yaw_sp);
+			}
+
 
 			// attitude during transitions are controlled by mc attitude control so rotate the desired attitude to the
 			// multirotor frame
 			_q_trans_start = _q_trans_start * Quatf(Eulerf(0, -M_PI_2_F, 0));
 
-		} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
+		} else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) {
 			// initial attitude setpoint for the transition should be with wings level
 			_q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, _mc_virtual_att_sp->yaw_body);
 			Vector3f x = Dcmf(Quatf(_v_att->q)) * Vector3f(1, 0, 0);
@@ -232,25 +232,17 @@ void Tailsitter::update_transition_state()
 	cos_tilt = cos_tilt < -1.0f ? -1.0f : cos_tilt;
 	const float tilt = acosf(cos_tilt);
 
-	if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
+	if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) {
 
 		// calculate pitching rate - and constrain to at least 0.1s transition time
 		const float trans_pitch_rate = M_PI_2_F / math::max(_param_vt_f_trans_dur.get(), 0.1f);
 
 		if (tilt < M_PI_2_F - math::radians(_param_fw_psp_off.get())) {
 			_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
-						       time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
+						       _time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
 		}
 
-		// check front transition timeout
-		if (_param_vt_trans_timeout.get()  > FLT_EPSILON) {
-			if (time_since_trans_start > _param_vt_trans_timeout.get()) {
-				// transition timeout occured, abort transition
-				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
-			}
-		}
-
-	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
+	} else if (_vtol_mode == vtol_mode::TRANSITION_BACK) {
 
 		// calculate pitching rate - and constrain to at least 0.1s transition time
 		const float trans_pitch_rate = M_PI_2_F / math::max(_param_vt_b_trans_dur.get(), 0.1f);
@@ -261,7 +253,7 @@ void Tailsitter::update_transition_state()
 
 		if (tilt > 0.01f) {
 			_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
-						       time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
+						       _time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
 		}
 	}
 
@@ -286,7 +278,6 @@ void Tailsitter::waiting_on_tecs()
 void Tailsitter::update_fw_state()
 {
 	VtolType::update_fw_state();
-
 }
 
 /**
@@ -329,7 +320,7 @@ void Tailsitter::fill_actuator_outputs()
 	mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH];
 	mc_out[actuator_controls_s::INDEX_YAW]   = mc_in[actuator_controls_s::INDEX_YAW];
 
-	if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
+	if (_vtol_mode == vtol_mode::FW_MODE) {
 		mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
 
 		// FW thrust is allocated on mc_thrust_sp[0] for tailsitter with dynamic control allocation
@@ -351,14 +342,14 @@ void Tailsitter::fill_actuator_outputs()
 	}
 
 	// Landing Gear
-	if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+	if (_vtol_mode == vtol_mode::MC_MODE) {
 		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN;
 
 	} else {
 		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_UP;
 	}
 
-	if (_param_vt_elev_mc_lock.get() && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+	if (_param_vt_elev_mc_lock.get() && _vtol_mode == vtol_mode::MC_MODE) {
 		fw_out[actuator_controls_s::INDEX_ROLL]  = 0;
 		fw_out[actuator_controls_s::INDEX_PITCH] = 0;
 
@@ -369,6 +360,10 @@ void Tailsitter::fill_actuator_outputs()
 		_torque_setpoint_1->xyz[2] = -fw_in[actuator_controls_s::INDEX_ROLL];
 	}
 
+	if (_in_actuator_test_mode) {
+		_in_actuator_test_mode = override_controls_for_test_mode();
+	}
+
 	_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
 	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
 
diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h
index 36dd05b39a..e8eceb1260 100644
--- a/src/modules/vtol_att_control/tailsitter.h
+++ b/src/modules/vtol_att_control/tailsitter.h
@@ -69,10 +69,7 @@ private:
 		FW_MODE					/**< vtol is in fixed wing mode */
 	};
 
-	struct {
-		vtol_mode flight_mode;			/**< vtol flight mode, defined by enum vtol_mode */
-		hrt_abstime transition_start;	/**< absoulte time at which front transition started */
-	} _vtol_schedule;
+	vtol_mode _vtol_mode{vtol_mode::MC_MODE};			/**< vtol flight mode, defined by enum vtol_mode */
 
 	matrix::Quatf _q_trans_start;
 	matrix::Quatf _q_trans_sp;
diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp
index 84e6e4adc6..696a900b5b 100644
--- a/src/modules/vtol_att_control/tiltrotor.cpp
+++ b/src/modules/vtol_att_control/tiltrotor.cpp
@@ -51,18 +51,13 @@ using namespace time_literals;
 #define BACKTRANS_THROTTLE_DOWNRAMP_DUR_S 1.0f
 #define BACKTRANS_THROTTLE_UPRAMP_DUR_S 1.0f;
 #define BACKTRANS_MOTORS_UPTILT_DUR_S 1.0f;
+#define ELEVATOR_SATURATION_LIMIT 0.9f  	// the elevator is considered to saturate if above this value (used for main/alternate motor switch logic)
+#define TIME_IN_SATURATION_LIMIT 0.5f 		// the time in elevator saturation required to trigger the saturation flag
+#define MIN_TIME_ALTERNATE_CONDITIONS_MET 10  	// time the conditions need to be met to switch to alternate motor
 
 Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
 	VtolType(attc)
 {
-	_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
-	_vtol_schedule.transition_start = 0;
-
-	_mc_roll_weight = 1.0f;
-	_mc_pitch_weight = 1.0f;
-	_mc_yaw_weight = 1.0f;
-
-	_flag_was_in_trans_mode = false;
 }
 
 void
@@ -81,7 +76,7 @@ void Tiltrotor::update_vtol_state()
 
 	if (_vtol_vehicle_status->vtol_transition_failsafe) {
 		// Failsafe event, switch to MC mode immediately
-		_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+		_vtol_mode = vtol_mode::MC_MODE;
 
 		//reset failsafe when FW is no longer requested
 		if (!_attc->is_fixed_wing_requested()) {
@@ -91,23 +86,25 @@ void Tiltrotor::update_vtol_state()
 	} else 	if (!_attc->is_fixed_wing_requested()) {
 
 		// plane is in multicopter mode
-		switch (_vtol_schedule.flight_mode) {
+		switch (_vtol_mode) {
 		case vtol_mode::MC_MODE:
 			break;
 
 		case vtol_mode::FW_MODE:
-			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_BACK;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			_vtol_mode = vtol_mode::TRANSITION_BACK;
+			_transition_start_timestamp = hrt_absolute_time();
+			// TODO: rework this
+			_time_since_trans_start = 0.f;
 			break;
 
 		case vtol_mode::TRANSITION_FRONT_P1:
 			// failsafe into multicopter mode
-			_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+			_vtol_mode = vtol_mode::MC_MODE;
 			break;
 
 		case vtol_mode::TRANSITION_FRONT_P2:
 			// failsafe into multicopter mode
-			_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+			_vtol_mode = vtol_mode::MC_MODE;
 			break;
 
 		case vtol_mode::TRANSITION_BACK:
@@ -125,11 +122,10 @@ void Tiltrotor::update_vtol_state()
 				exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get() ;
 			}
 
-			const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
-			const bool exit_backtransition_time_condition = time_since_trans_start > _param_vt_b_trans_dur.get() ;
+			const bool exit_backtransition_time_condition = _time_since_trans_start > _param_vt_b_trans_dur.get() ;
 
 			if (exit_backtransition_tilt_condition && (exit_backtransition_speed_condition || exit_backtransition_time_condition)) {
-				_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
+				_vtol_mode = vtol_mode::MC_MODE;
 			}
 
 			break;
@@ -137,11 +133,13 @@ void Tiltrotor::update_vtol_state()
 
 	} else {
 
-		switch (_vtol_schedule.flight_mode) {
+		switch (_vtol_mode) {
 		case vtol_mode::MC_MODE:
 			// initialise a front transition
-			_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P1;
-			_vtol_schedule.transition_start = hrt_absolute_time();
+			_vtol_mode = vtol_mode::TRANSITION_FRONT_P1;
+			_transition_start_timestamp = hrt_absolute_time();
+			// TODO: rework this
+			_time_since_trans_start = 0.f;
 			break;
 
 		case vtol_mode::FW_MODE:
@@ -149,36 +147,28 @@ void Tiltrotor::update_vtol_state()
 
 		case vtol_mode::TRANSITION_FRONT_P1: {
 
-				float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
-
 				const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
 						&& !_param_fw_arsp_mode.get() ;
 
 				bool transition_to_p2 = false;
 
-				if (time_since_trans_start > getMinimumFrontTransitionTime()) {
+				if (_time_since_trans_start > getMinimumFrontTransitionTime()) {
 					if (airspeed_triggers_transition) {
 						transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
 
 					} else {
 						transition_to_p2 = _tilt_control >= _param_vt_tilt_trans.get() &&
-								   time_since_trans_start > getOpenLoopFrontTransitionTime();
+								   _time_since_trans_start > getOpenLoopFrontTransitionTime();
 					}
 				}
 
 				transition_to_p2 |= can_transition_on_ground();
 
 				if (transition_to_p2) {
-					_vtol_schedule.flight_mode = vtol_mode::TRANSITION_FRONT_P2;
-					_vtol_schedule.transition_start = hrt_absolute_time();
-				}
-
-				// check front transition timeout
-				if (_param_vt_trans_timeout.get()  > FLT_EPSILON) {
-					if (time_since_trans_start > _param_vt_trans_timeout.get()) {
-						// transition timeout occured, abort transition
-						_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
-					}
+					_vtol_mode = vtol_mode::TRANSITION_FRONT_P2;
+					_transition_start_timestamp = hrt_absolute_time();
+					// TODO: rework this
+					_time_since_trans_start = 0.f;
 				}
 
 				break;
@@ -188,7 +178,7 @@ void Tiltrotor::update_vtol_state()
 
 			// if the rotors have been tilted completely we switch to fw mode
 			if (_tilt_control >= _param_vt_tilt_fw.get()) {
-				_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
+				_vtol_mode = vtol_mode::FW_MODE;
 				_tilt_control = _param_vt_tilt_fw.get();
 			}
 
@@ -196,35 +186,35 @@ void Tiltrotor::update_vtol_state()
 
 		case vtol_mode::TRANSITION_BACK:
 			// failsafe into fixed wing mode
-			_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
+			_vtol_mode = vtol_mode::FW_MODE;
 			break;
 		}
 	}
 
 	// map tiltrotor specific control phases to simple control modes
-	switch (_vtol_schedule.flight_mode) {
+	switch (_vtol_mode) {
 	case vtol_mode::MC_MODE:
-		_vtol_mode = mode::ROTARY_WING;
+		_common_vtol_mode = mode::ROTARY_WING;
 		break;
 
 	case vtol_mode::FW_MODE:
-		_vtol_mode = mode::FIXED_WING;
+		_common_vtol_mode = mode::FIXED_WING;
 		break;
 
 	case vtol_mode::TRANSITION_FRONT_P1:
 	case vtol_mode::TRANSITION_FRONT_P2:
-		_vtol_mode = mode::TRANSITION_TO_FW;
+		_common_vtol_mode = mode::TRANSITION_TO_FW;
 		break;
 
 	case vtol_mode::TRANSITION_BACK:
-		_vtol_mode = mode::TRANSITION_TO_MC;
+		_common_vtol_mode = mode::TRANSITION_TO_MC;
 		break;
 	}
 }
 
-void Tiltrotor::update_mc_state()
+void Tiltrotor::update_mc_generic()
 {
-	VtolType::update_mc_state();
+	_alternate_motor_on = false;
 
 	/*Motor spin up: define the first second after arming as motor spin up time, during which
 	* the tilt is set to the value of VT_TILT_SPINUP. This allows the user to set a spin up
@@ -258,8 +248,14 @@ void Tiltrotor::update_mc_state()
 		}
 
 		_mc_yaw_weight = 0.0f; //disable yaw control during spinup
+	}
+}
 
-	} else {
+void Tiltrotor::update_mc_state()
+{
+	VtolType::update_mc_state();
+
+	if (!_tilt_motors_for_startup) {
 		// normal operation
 		_tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get();
 		_mc_yaw_weight = 1.0f;
@@ -269,7 +265,6 @@ void Tiltrotor::update_mc_state()
 			_v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt();
 		}
 	}
-
 }
 
 void Tiltrotor::update_fw_state()
@@ -280,6 +275,31 @@ void Tiltrotor::update_fw_state()
 	// a zero throttle value
 	_v_att_sp->thrust_body[2] = -_v_att_sp->thrust_body[0];
 
+	select_fixed_wing_motors(); // check if main or alternate motors should be used (based on throttle sp)
+
+	if (_alternate_motor_on) {
+		// fade out main motors from 90% max to 0 in 3 seconds
+		// note that the actual PWM applied on the motors is always below (PWM = throttle * (PWM_MAX - PWM_MIN) + PWM_MIN)
+		// fader startes at 1.5 such that it only begins to have effect after 1s.
+		const float max = 0.85f; // leads to a small throttle increase on the main to compensate for rear motor spin up drag
+		const float min = 0.4f; // makes the main motors switch off immediately if (approximately) below idle PWM
+		const float fader = math::constrain(1.5f - (hrt_elapsed_time(&_last_time_switch_to_alternate) * 1e-6f / 2.f), 0.f, 1.f);
+		const float ramp_down_main_max_PWM = fader * max * (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN;
+
+		if (fader > min) {
+			set_main_motor_state(motor_state::VALUE, ramp_down_main_max_PWM);
+
+		} else {
+			set_main_motor_state(motor_state::DISABLED);
+		}
+
+		set_alternate_motor_state(motor_state::ENABLED);
+
+	} else {
+		set_main_motor_state(motor_state::ENABLED);
+		set_alternate_motor_state(motor_state::DISABLED);
+	}
+
 	// make sure motors are tilted forward
 	_tilt_control = _param_vt_tilt_fw.get();
 }
@@ -288,26 +308,37 @@ void Tiltrotor::update_transition_state()
 {
 	VtolType::update_transition_state();
 
+	const hrt_abstime now = hrt_absolute_time();
+
 	// we get attitude setpoint from a multirotor flighttask if altitude is controlled.
 	// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
 	if (_v_control_mode->flag_control_climb_rate_enabled) {
+		// we need the incoming (virtual) attitude setpoints (both mc and fw) to be recent, otherwise return (means the previous setpoint stays active)
+		if (_mc_virtual_att_sp->timestamp < (now - 1_s) || _fw_virtual_att_sp->timestamp < (now - 1_s)) {
+			return;
+		}
+
 		memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 		_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
 		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];
 
 	} else {
+		// we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active)
+		if (_fw_virtual_att_sp->timestamp < (now - 1_s)) {
+			return;
+		}
+
 		memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 		_thrust_transition = _fw_virtual_att_sp->thrust_body[0];
 	}
 
-	float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
 
 	if (!_flag_was_in_trans_mode) {
 		// save desired heading for transition and last thrust value
 		_flag_was_in_trans_mode = true;
 	}
 
-	if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
+	if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) {
 		// for the first part of the transition all rotors are enabled
 		set_all_motor_state(motor_state::ENABLED);
 
@@ -315,7 +346,7 @@ void Tiltrotor::update_transition_state()
 		if (_tilt_control <= _param_vt_tilt_trans.get()) {
 			const float ramped_up_tilt = _param_vt_tilt_mc.get() +
 						     fabsf(_param_vt_tilt_trans.get() - _param_vt_tilt_mc.get()) *
-						     time_since_trans_start / _param_vt_f_trans_dur.get() ;
+						     _time_since_trans_start / _param_vt_f_trans_dur.get() ;
 
 			// only allow increasing tilt (tilt in hover can already be non-zero)
 			_tilt_control = math::max(_tilt_control, ramped_up_tilt);
@@ -335,8 +366,8 @@ void Tiltrotor::update_transition_state()
 
 		// without airspeed do timed weight changes
 		if ((_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
-		    time_since_trans_start > getMinimumFrontTransitionTime()) {
-			_mc_roll_weight = 1.0f - (time_since_trans_start - getMinimumFrontTransitionTime()) /
+		    _time_since_trans_start > getMinimumFrontTransitionTime()) {
+			_mc_roll_weight = 1.0f - (_time_since_trans_start - getMinimumFrontTransitionTime()) /
 					  (getOpenLoopFrontTransitionTime() - getMinimumFrontTransitionTime());
 			_mc_yaw_weight = _mc_roll_weight;
 		}
@@ -348,17 +379,17 @@ void Tiltrotor::update_transition_state()
 		_flaps_setpoint_with_slewrate.update(0.f, _dt);
 		_spoiler_setpoint_with_slewrate.update(0.f, _dt);
 
-	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) {
+	} else if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P2) {
 		// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
 		_tilt_control = math::constrain(_param_vt_tilt_trans.get() +
-						fabsf(_param_vt_tilt_fw.get() - _param_vt_tilt_trans.get()) * time_since_trans_start /
+						fabsf(_param_vt_tilt_fw.get() - _param_vt_tilt_trans.get()) * _time_since_trans_start /
 						_param_vt_trans_p2_dur.get(), _param_vt_tilt_trans.get(), _param_vt_tilt_fw.get());
 
 		_mc_roll_weight = 0.0f;
 		_mc_yaw_weight = 0.0f;
 
 		// ramp down motors not used in fixed-wing flight (setting MAX_PWM down scales the given output into the new range)
-		int ramp_down_value = (1.0f - time_since_trans_start / _param_vt_trans_p2_dur.get()) *
+		int ramp_down_value = (1.0f - _time_since_trans_start / _param_vt_trans_p2_dur.get()) *
 				      (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN;
 
 		set_alternate_motor_state(motor_state::VALUE, ramp_down_value);
@@ -374,7 +405,7 @@ void Tiltrotor::update_transition_state()
 		_flaps_setpoint_with_slewrate.update(0.f, _dt);
 		_spoiler_setpoint_with_slewrate.update(0.f, _dt);
 
-	} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
+	} else if (_vtol_mode == vtol_mode::TRANSITION_BACK) {
 
 		// set idle speed for rotary wing mode
 		if (!_flag_idle_mc) {
@@ -382,9 +413,9 @@ void Tiltrotor::update_transition_state()
 		}
 
 		// tilt rotors back once motors are idle
-		if (time_since_trans_start > BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
+		if (_time_since_trans_start > BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
 
-			float progress = (time_since_trans_start - BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) / BACKTRANS_MOTORS_UPTILT_DUR_S;
+			float progress = (_time_since_trans_start - BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) / BACKTRANS_MOTORS_UPTILT_DUR_S;
 			progress = math::constrain(progress, 0.0f, 1.0f);
 			_tilt_control = moveLinear(_param_vt_tilt_fw.get(), _param_vt_tilt_mc.get(), progress);
 		}
@@ -396,14 +427,14 @@ void Tiltrotor::update_transition_state()
 			_v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp();
 		}
 
-		if (time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
+		if (_time_since_trans_start < BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) {
 			// blend throttle from FW value to 0
 			_mc_throttle_weight = 1.0f;
 			const float target_throttle = 0.0f;
-			const float progress = time_since_trans_start / BACKTRANS_THROTTLE_DOWNRAMP_DUR_S;
+			const float progress = _time_since_trans_start / BACKTRANS_THROTTLE_DOWNRAMP_DUR_S;
 			blendThrottleDuringBacktransition(progress, target_throttle);
 
-		} else if (time_since_trans_start < timeUntilMotorsAreUp()) {
+		} else if (_time_since_trans_start < timeUntilMotorsAreUp()) {
 			// while we quickly rotate back the motors keep throttle at idle
 
 			// turn on all MC motors
@@ -416,12 +447,14 @@ void Tiltrotor::update_transition_state()
 			_mc_roll_weight = 1.0f;
 			_mc_pitch_weight = 1.0f;
 			// slowly ramp up throttle to avoid step inputs
-			float progress = (time_since_trans_start - timeUntilMotorsAreUp()) / BACKTRANS_THROTTLE_UPRAMP_DUR_S;
+			float progress = (_time_since_trans_start - timeUntilMotorsAreUp()) / BACKTRANS_THROTTLE_UPRAMP_DUR_S;
 			progress = math::constrain(progress, 0.0f, 1.0f);
 			_mc_throttle_weight = moveLinear(0.0f, 1.0f, progress);
 		}
 	}
 
+	_last_time_alternate_conditions_violated = hrt_absolute_time();
+	_alternate_motor_on = false;
 
 	_v_att_sp->thrust_body[2] = -_thrust_transition;
 
@@ -483,10 +516,17 @@ void Tiltrotor::fill_actuator_outputs()
 	_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
 	_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW]   * _mc_yaw_weight;
 
-	if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
+	float throttle_fw = fw_in[actuator_controls_s::INDEX_THROTTLE];
+
+	if (_vtol_mode == vtol_mode::FW_MODE) {
 
 		// for the legacy mixing system pubish FW throttle on the MC output
-		mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
+		// if we're currently using the alternate motors, adapt the throttle to account for difference in thrust to main ones
+		if (_alternate_motor_on) {
+			throttle_fw = alternate_motors_throttle_adaption(throttle_fw);
+		}
+
+		mc_out[actuator_controls_s::INDEX_THROTTLE] = throttle_fw;
 
 		// Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC),
 		// pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they
@@ -508,7 +548,7 @@ void Tiltrotor::fill_actuator_outputs()
 	}
 
 	// Landing gear
-	if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+	if (_vtol_mode == vtol_mode::MC_MODE) {
 		mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN;
 
 	} else {
@@ -518,15 +558,24 @@ void Tiltrotor::fill_actuator_outputs()
 	// Fixed wing output
 	fw_out[actuator_controls_s::INDEX_COLLECTIVE_TILT] = _tilt_control;
 
-	if (_param_vt_elev_mc_lock.get()  && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
+	if (_param_vt_elev_mc_lock.get()  && _vtol_mode == vtol_mode::MC_MODE) {
 		fw_out[actuator_controls_s::INDEX_ROLL]  = 0;
 		fw_out[actuator_controls_s::INDEX_PITCH] = 0;
 		fw_out[actuator_controls_s::INDEX_YAW]   = 0;
 
 	} else {
-		fw_out[actuator_controls_s::INDEX_ROLL]  = fw_in[actuator_controls_s::INDEX_ROLL];
-		fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH];
-		fw_out[actuator_controls_s::INDEX_YAW]   = fw_in[actuator_controls_s::INDEX_YAW];
+		fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL];
+
+		// use pitch trimming when alternate motors are used for fixed-wing flight
+		float pitch_trim_offset = 0.0f;
+
+		if (_vtol_mode == vtol_mode::FW_MODE && _alternate_motor_on) {
+			pitch_trim_offset = alternate_motors_elevator_trim(throttle_fw);
+		}
+
+		fw_out[actuator_controls_s::INDEX_PITCH] = pitch_trim_offset + fw_in[actuator_controls_s::INDEX_PITCH];
+
+		fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW];
 		_torque_setpoint_1->xyz[0] = fw_in[actuator_controls_s::INDEX_ROLL];
 		_torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH];
 		_torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW];
@@ -540,6 +589,10 @@ void Tiltrotor::fill_actuator_outputs()
 	_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
 
 	_actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time();
+
+	if (_in_actuator_test_mode) {
+		_in_actuator_test_mode = override_controls_for_test_mode();
+	}
 }
 
 /*
@@ -555,6 +608,76 @@ float Tiltrotor::thrust_compensation_for_tilt()
 	return math::constrain(_v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F), -1.0f, 0.0f);
 }
 
+void Tiltrotor::select_fixed_wing_motors()
+{
+	// do nothing and disable the alternate motor if one of the thresholds is set to a negative value
+	if (_param_vt_thr_alter_max.get() < FLT_EPSILON) {
+		_alternate_motor_on = false;
+		return;
+	}
+
+	// declare elevator saturation if above specified limit
+	const bool elevator_not_saturated =
+		_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] < ELEVATOR_SATURATION_LIMIT;
+
+	if (elevator_not_saturated) {
+		_last_time_elevator_not_saturated = hrt_absolute_time();
+	}
+
+	// check if saturation limit is hit (saturation for certain amount of time)
+	const bool elevator_saturated =
+		hrt_elapsed_time(&_last_time_elevator_not_saturated) * 1e-6f  > TIME_IN_SATURATION_LIMIT;
+
+	// check if throttle condition is met (throttle below max alternate throttle)
+	const bool throttle_exceeding_alternate_limit =
+		_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE] > _param_vt_thr_alter_max.get();
+
+	// reset counter if outside of thrust limits or the elevator is in saturation
+	if (throttle_exceeding_alternate_limit || elevator_saturated) {
+		_last_time_alternate_conditions_violated = hrt_absolute_time();
+	}
+
+	// check if throttle is below threshold for specified amount of time, incl. no elevator saturation
+	const bool alternate_conditions_met = hrt_elapsed_time(&_last_time_alternate_conditions_violated) * 1e-6f >
+					      MIN_TIME_ALTERNATE_CONDITIONS_MET;
+
+	// save the time of the main-->alternate switch (for elevator compensation)
+	if (alternate_conditions_met && !_alternate_motor_on) {
+		_last_time_switch_to_alternate = hrt_absolute_time();
+	}
+
+	_alternate_motor_on = alternate_conditions_met;
+}
+
+float Tiltrotor::alternate_motors_throttle_adaption(float throttle_in)
+{
+	return (throttle_in - _param_vt_thr_n_main.get()) * _param_vt_thr_alter_sc.get() + _param_vt_thr_n_alter.get();
+}
+
+float Tiltrotor::alternate_motors_elevator_trim(const float throttle_alternate)
+{
+	float airspeed = 17.0f;
+	float rho = 1.2f;
+
+	if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
+		airspeed = _airspeed_validated->calibrated_airspeed_m_s;
+	}
+
+	if (PX4_ISFINITE(_attc->getAirDensity())) {
+		rho = _attc->getAirDensity();
+	}
+
+	const float dynamic_pressure = math::constrain(0.5f * airspeed * airspeed * rho, 120.0f, 250.0f);
+
+	const float fade_in_time = 1.0f; // the elevator compensation is faded in this time after main-->alternate switch
+	const float fader = math::constrain(hrt_elapsed_time(&_last_time_switch_to_alternate) * 1e-6f, 0.f, fade_in_time);
+
+	const float elev_comp = _param_vt_elev_comp_off.get() + _param_vt_elev_comp_k.get() / dynamic_pressure *
+				throttle_alternate;
+
+	return math::constrain(fader * elev_comp, -0.2f, 0.7f);
+}
+
 void Tiltrotor::blendThrottleAfterFrontTransition(float scale)
 {
 	const float tecs_throttle = _v_att_sp->thrust_body[0];
diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h
index a5601f723f..9a420d372a 100644
--- a/src/modules/vtol_att_control/tiltrotor.h
+++ b/src/modules/vtol_att_control/tiltrotor.h
@@ -56,6 +56,7 @@ public:
 	void update_transition_state() override;
 	void fill_actuator_outputs() override;
 	void update_mc_state() override;
+	void update_mc_generic() override;
 	void update_fw_state() override;
 	void waiting_on_tecs() override;
 	float thrust_compensation_for_tilt();
@@ -76,11 +77,7 @@ private:
 	 * they need to idle otherwise they need too much time to spin up for mc mode.
 	 */
 
-
-	struct {
-		vtol_mode flight_mode;			/**< vtol flight mode, defined by enum vtol_mode */
-		hrt_abstime transition_start;	/**< absoulte time at which front transition started */
-	} _vtol_schedule;
+	vtol_mode _vtol_mode{vtol_mode::MC_MODE};			/**< vtol flight mode, defined by enum vtol_mode */
 
 	float _tilt_control{0.0f};		/**< actuator value for the tilt servo */
 
@@ -94,13 +91,29 @@ private:
 	hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */
 	bool _tilt_motors_for_startup{false};
 
+	// APX4 custom
+	void select_fixed_wing_motors();
+	float alternate_motors_throttle_adaption(float throttle_in);
+	float alternate_motors_elevator_trim(const float throttle_alternate);
+	bool _alternate_motor_on = false;
+	hrt_abstime _last_time_switch_to_alternate = 0;
+	hrt_abstime _last_time_alternate_conditions_violated = 0;
+	hrt_abstime _last_time_elevator_not_saturated{0};
+	// APX4 custom end
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
 					(ParamFloat<px4::params::VT_TILT_MC>) _param_vt_tilt_mc,
 					(ParamFloat<px4::params::VT_TILT_TRANS>) _param_vt_tilt_trans,
 					(ParamFloat<px4::params::VT_TILT_FW>) _param_vt_tilt_fw,
 					(ParamFloat<px4::params::VT_TILT_SPINUP>) _param_vt_tilt_spinup,
-					(ParamFloat<px4::params::VT_TRANS_P2_DUR>) _param_vt_trans_p2_dur
-				       )
+					(ParamFloat<px4::params::VT_TRANS_P2_DUR>) _param_vt_trans_p2_dur,
+					(ParamFloat<px4::params::VT_THR_ALTER_MAX>) _param_vt_thr_alter_max,  // APX4 custom
+					(ParamFloat<px4::params::VT_THR_N_MAIN>) _param_vt_thr_n_main,  // APX4 custom
+					(ParamFloat<px4::params::VT_THR_N_ALTER>) _param_vt_thr_n_alter,  // APX4 custom
+					(ParamFloat<px4::params::VT_THR_ALTER_SC>) _param_vt_thr_alter_sc,  // APX4 custom
+					(ParamFloat<px4::params::VT_ELEV_COMP_K>) _param_vt_elev_comp_k,   // APX4 custom
+					(ParamFloat<px4::params::VT_ELEV_COMP_OFF>) _param_vt_elev_comp_off // APX4 custom
 
+				       )
 };
 #endif
diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c
index 31a6d549c0..ecc1ca0d95 100644
--- a/src/modules/vtol_att_control/tiltrotor_params.c
+++ b/src/modules/vtol_att_control/tiltrotor_params.c
@@ -99,3 +99,77 @@ PARAM_DEFINE_FLOAT(VT_TILT_SPINUP, 0.0f);
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);
+
+
+/**
+ * Throttle threshold to switch on/off alternative motor.
+ *
+ * The alternative fixed-wing motors are switched off if the throttle setpoint raises above this threshold.
+ *
+ * @min -1.0
+ * @max 1.0
+ * @increment 0.01
+ * @decimal 2
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_THR_ALTER_MAX, -1.0f);
+
+/**
+ * Neutral throttle of main motors. Used for throttle scaling in case alternative throttle is used.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @increment 0.01
+ * @decimal 2
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_THR_N_MAIN, 0.0f);
+
+/**
+ * Neutral throttle of alternative motors. Used for throttle scaling in case alternative throttle is used.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @increment 0.01
+ * @decimal 2
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_THR_N_ALTER, 0.0f);
+
+/**
+ * Scle factor for alternative throttle. Used for throttle scaling in case alternative throttle is used.
+ *
+ * @min 0.0
+ * @max 10
+ * @increment 0.01
+ * @decimal 2
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_THR_ALTER_SC, 1.0f);
+
+/**
+ * Pitch actuator compensation when alternative motor is on.
+ *
+ * Pitch_actuator_offset = VT_ELEV_COMP_OFF + throttle_alt * comp_elev_k / p_dynamic.
+ *
+ * @min 0.0
+ * @max 500.0
+ * @increment 0.1
+ * @decimal 1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_ELEV_COMP_K, 0.0f);
+
+
+/**
+ * Static offset added to the elevator control as part of the non-zero thrust line offset compensation for the alternate motor.
+ *
+ * Pitch_actuator_offset = VT_ELEV_COMP_OFF + throttle_alt * comp_elev_k / p_dynamic.
+ *
+ * @min -1.0
+ * @max 1.0
+ * @increment 0.01
+ * @decimal 2
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_ELEV_COMP_OFF, 0.0f);
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 819034df16..4a5167bae2 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -135,7 +135,7 @@ void VtolAttitudeControl::vehicle_cmd_poll()
 			vehicle_status_s vehicle_status{};
 			_vehicle_status_sub.copy(&vehicle_status);
 
-			uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
+			uint8_t result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 
 			const int transition_command_param1 = int(vehicle_command.param1 + 0.5f);
 
@@ -146,7 +146,7 @@ void VtolAttitudeControl::vehicle_cmd_poll()
 			     || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
 			     ||  vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)) {
 
-				result = vehicle_command_ack_s::VEHICLE_RESULT_TEMPORARILY_REJECTED;
+				result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 
 			} else {
 				_transition_command = transition_command_param1;
@@ -164,6 +164,10 @@ void VtolAttitudeControl::vehicle_cmd_poll()
 				uORB::Publication<vehicle_command_ack_s> command_ack_pub{ORB_ID(vehicle_command_ack)};
 				command_ack_pub.publish(command_ack);
 			}
+
+		} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_ACTUATOR_TEST) {
+			const actuator_test_type test_type = (actuator_test_type)static_cast<int>(vehicle_command.param1 + 0.5f);
+			_vtol_type->activate_actuator_test_mode(test_type);
 		}
 	}
 }
@@ -214,6 +218,10 @@ VtolAttitudeControl::quadchute(QuadchuteReason reason)
 			events::send(events::ID("vtol_att_ctrl_quadchute_max_roll"), events::Log::Critical,
 				     "Quadchute triggered, due to maximum roll angle exceeded");
 			break;
+
+		case QuadchuteReason::None:
+			// should never get in here
+			return;
 		}
 
 		_vtol_vehicle_status.vtol_transition_failsafe = true;
@@ -309,6 +317,18 @@ VtolAttitudeControl::Run()
 		_airspeed_validated_sub.update(&_airspeed_validated);
 		_tecs_status_sub.update(&_tecs_status);
 		_land_detected_sub.update(&_land_detected);
+
+		if (_home_position_sub.updated()) {
+			home_position_s home_position;
+
+			if (_home_position_sub.copy(&home_position) && home_position.valid_alt) {
+				_home_position_z = home_position.z;
+
+			} else {
+				_home_position_z = NAN;
+			}
+		}
+
 		action_request_poll();
 		vehicle_cmd_poll();
 
@@ -331,7 +351,7 @@ VtolAttitudeControl::Run()
 			// vehicle is doing a transition to FW
 			_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW;
 
-			if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) {
+			if (mc_att_sp_updated || fw_att_sp_updated) {
 				_vtol_type->update_transition_state();
 				_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
 			}
@@ -342,7 +362,7 @@ VtolAttitudeControl::Run()
 			// vehicle is doing a transition to MC
 			_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC;
 
-			if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) {
+			if (mc_att_sp_updated || fw_att_sp_updated) {
 				_vtol_type->update_transition_state();
 				_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
 			}
@@ -353,6 +373,10 @@ VtolAttitudeControl::Run()
 			// vehicle is in rotary wing mode
 			_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
 
+			// VTOL types can implement code that is not in sync with attitude setpoint updates
+			// currently only used for tiltrotors
+			_vtol_type->update_mc_generic();
+
 			if (mc_att_sp_updated) {
 				_vtol_type->update_mc_state();
 				_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h
index aa41a9f1d0..2ff6fd0c32 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.h
+++ b/src/modules/vtol_att_control/vtol_att_control_main.h
@@ -68,6 +68,7 @@
 #include <uORB/topics/action_request.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/airspeed_validated.h>
+#include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_air_data.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/position_setpoint_triplet.h>
@@ -98,16 +99,6 @@ class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public Modul
 {
 public:
 
-	enum class QuadchuteReason {
-		TransitionTimeout = 0,
-		ExternalCommand,
-		MinimumAltBreached,
-		LossOfAlt,
-		LargeAltError,
-		MaximumPitchExceeded,
-		MaximumRollExceeded,
-	};
-
 	VtolAttitudeControl();
 	~VtolAttitudeControl() override;
 
@@ -154,6 +145,7 @@ public:
 	struct vehicle_thrust_setpoint_s 		*get_thrust_setpoint_0() {return &_thrust_setpoint_0;}
 	struct vehicle_thrust_setpoint_s 		*get_thrust_setpoint_1() {return &_thrust_setpoint_1;}
 	struct vtol_vehicle_status_s			*get_vtol_vehicle_status() {return &_vtol_vehicle_status;}
+	float get_home_position_z() { return _home_position_z; }
 
 private:
 	void Run() override;
@@ -166,6 +158,7 @@ private:
 	uORB::Subscription _action_request_sub{ORB_ID(action_request)};
 	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};
 	uORB::Subscription _fw_virtual_att_sp_sub{ORB_ID(fw_virtual_attitude_setpoint)};
+	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
 	uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)};
 	uORB::Subscription _local_pos_sp_sub{ORB_ID(vehicle_local_position_setpoint)};
 	uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};
@@ -212,6 +205,7 @@ private:
 	vehicle_local_position_s		_local_pos{};
 	vehicle_local_position_setpoint_s	_local_pos_sp{};
 	vtol_vehicle_status_s 			_vtol_vehicle_status{};
+	float _home_position_z{NAN};
 
 	float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C};	// [kg/m^3]
 
diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c
index 035426ac07..fe2b349b1d 100644
--- a/src/modules/vtol_att_control/vtol_att_control_params.c
+++ b/src/modules/vtol_att_control/vtol_att_control_params.c
@@ -59,7 +59,6 @@ PARAM_DEFINE_INT32(VT_IDLE_PWM_MC, 900);
  * @value 2 Standard
  * @min 0
  * @max 2
- * @decimal 0
  * @reboot_required true
  * @group VTOL Attitude Control
  */
@@ -203,6 +202,8 @@ PARAM_DEFINE_FLOAT(VT_TRANS_TIMEOUT, 15.0f);
  * @unit s
  * @min 0.0
  * @max 20.0
+ * @increment 0.1
+ * @decimal 1
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f);
@@ -212,8 +213,12 @@ PARAM_DEFINE_FLOAT(VT_TRANS_MIN_TM, 2.0f);
  *
  * Minimum altitude for fixed wing flight, when in fixed wing the altitude drops below this altitude
  * the vehicle will transition back to MC mode and enter failsafe RTL
+ *
+ * @unit m
  * @min 0.0
  * @max 200.0
+ * @increment 1
+ * @decimal 1
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
@@ -223,8 +228,12 @@ PARAM_DEFINE_FLOAT(VT_FW_MIN_ALT, 0.0f);
  *
  * Maximum negative altitude error for fixed wing flight. If the altitude drops below this value below the altitude setpoint
  * the vehicle will transition back to MC mode and enter failsafe RTL.
+ *
+ * @unit m
  * @min 0.0
  * @max 200.0
+ * @increment 1
+ * @decimal 1
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_FW_ALT_ERR, 0.0f);
@@ -251,6 +260,21 @@ PARAM_DEFINE_INT32(VT_FW_QC_P, 0);
  */
 PARAM_DEFINE_INT32(VT_FW_QC_R, 0);
 
+/**
+ * Quadchute maximum height.
+ *
+ * Maximum height above the ground (if available, otherwhise above home if available, otherwise above the local origin) where triggering a quadchute is possible.
+ * Triggering a quadchute always means transitioning the vehicle to hover flight in which generally a lot of energy is consumed.
+ * At high altitudes there is therefore a big risk to deplete the battery and therefore crash. Currently, there is no automated
+ * re-transition to fixed wing mode implemented and therefore this parameter serves and an intermediate measure to increase safety.
+ *
+ * @unit m
+ * @increment 1
+ * @decimal 1
+ * @group VTOL Attitude Control
+ */
+PARAM_DEFINE_FLOAT(VT_FW_QC_HMAX, 200.0f);
+
 /**
  * Airspeed less front transition time (open loop)
  *
@@ -259,6 +283,8 @@ PARAM_DEFINE_INT32(VT_FW_QC_R, 0);
  * @unit s
  * @min 1.0
  * @max 30.0
+ * @increment 0.5
+ * @decimal 1
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_F_TR_OL_TM, 6.0f);
@@ -317,7 +343,7 @@ PARAM_DEFINE_FLOAT(VT_FW_DIFTHR_SC, 0.1f);
  * @unit rad s^2/m
  * @min 0
  * @max 0.2
- * @decimal 1
+ * @decimal 2
  * @increment 0.01
  * @group VTOL Attitude Control
  */
@@ -330,7 +356,7 @@ PARAM_DEFINE_FLOAT(VT_B_DEC_FF, 0.f);
  * @unit rad s/m
  * @min 0
  * @max 0.3
- * @decimal 1
+ * @decimal 2
  * @increment 0.05
  * @group VTOL Attitude Control
  */
@@ -355,8 +381,11 @@ PARAM_DEFINE_INT32(VT_MC_ON_FMU, 0);
  * then the fixed-wing forward actuation can be used to compensate for the missing thrust in forward direction
  * (see VT_FW_TRHUST_EN)
  *
+ * @unit deg
  * @min -10.0
  * @max 45.0
+ * @increment 0.1
+ * @decimal 1
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_PITCH_MIN, -5.0f);
@@ -368,8 +397,11 @@ PARAM_DEFINE_FLOAT(VT_PITCH_MIN, -5.0f);
  * During landing it can be beneficial to allow lower minimum pitch angles as it can avoid the wings
  * generating too much lift and preventing the vehicle from sinking at the desired rate.
  *
+ * @unit deg
  * @min -10.0
  * @max 45.0
+ * @increment 0.1
+ * @decimal 1
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_LND_PITCH_MIN, -5.0f);
@@ -381,7 +413,7 @@ PARAM_DEFINE_FLOAT(VT_LND_PITCH_MIN, -5.0f);
  * @min -1
  * @max 1
  * @decimal 1
- * @increment 0.05
+ * @increment 0.1
  * @group VTOL Attitude Control
  */
 PARAM_DEFINE_FLOAT(VT_SPOILER_MC_LD, 0.f);
diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp
index 3d958e4867..82e49a938d 100644
--- a/src/modules/vtol_att_control/vtol_type.cpp
+++ b/src/modules/vtol_att_control/vtol_type.cpp
@@ -54,7 +54,7 @@ using namespace matrix;
 VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	ModuleParams(nullptr),
 	_attc(att_controller),
-	_vtol_mode(mode::ROTARY_WING)
+	_common_vtol_mode(mode::ROTARY_WING)
 {
 	_v_att = _attc->get_att();
 	_v_att_sp = _attc->get_att_sp();
@@ -241,6 +241,8 @@ void VtolType::update_transition_state()
 	_last_loop_ts = t_now;
 	_throttle_blend_start_ts = t_now;
 
+	_time_since_trans_start = (float)(t_now - _transition_start_timestamp) * 1e-6f;
+
 	check_quadchute_condition();
 }
 
@@ -280,76 +282,181 @@ bool VtolType::can_transition_on_ground()
 	return !_v_control_mode->flag_armed || _land_detected->landed;
 }
 
-void VtolType::check_quadchute_condition()
+bool VtolType::isQuadchuteEnabled()
+{
+	float dist_to_ground = 0.f;
+	const float home_position_z = _attc->get_home_position_z();
+
+	if (_local_pos->dist_bottom_valid) {
+		dist_to_ground = _local_pos->dist_bottom;
+
+	} else if (PX4_ISFINITE(home_position_z)) {
+		dist_to_ground = -(_local_pos->z - home_position_z);
+
+	} else {
+		dist_to_ground = -_local_pos->z;
+
+	}
+
+	return _v_control_mode->flag_armed && !_land_detected->landed && dist_to_ground < _param_quadchute_max_height.get();
+}
+
+bool VtolType::isMinAltBreached()
+{
+	// fixed-wing minimum altitude
+	if (_param_vt_fw_min_alt.get() > FLT_EPSILON) {
+
+		if (-(_local_pos->z) < _param_vt_fw_min_alt.get()) {
+			return true;
+
+		}
+	}
+
+	return false;
+}
+
+bool VtolType::largeAltitudeLoss()
+{
+	// adaptive quadchute
+	if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled && _tecs_running) {
+
+		// are we dropping while requesting significant ascend?
+		if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _param_vt_fw_alt_err.get()) &&
+		    (_ra_hrate < -1.0f) &&
+		    (_ra_hrate_sp > 1.0f)) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool VtolType::largeAltitudeError()
+{
+	// adaptive quadchute
+	if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled && !_tecs_running) {
+
+
+		const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _param_vt_fw_alt_err.get());
+		const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f);
+
+		if (height_error && height_rate_error) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool VtolType::isPitchExceeded()
+{
+	// fixed-wing maximum pitch angle
+	if (_param_vt_fw_qc_p.get() > 0) {
+		Eulerf euler = Quatf(_v_att->q);
+
+		if (fabsf(euler.theta()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_p.get())))) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool VtolType::isRollExceeded()
+{
+	// fixed-wing maximum roll angle
+	if (_param_vt_fw_qc_r.get() > 0) {
+		Eulerf euler = Quatf(_v_att->q);
+
+		if (fabsf(euler.phi()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_r.get())))) {
+			return true;
+		}
+	}
+
+	return false;
+}
+
+bool VtolType::isFrontTransitionTimeout()
+{
+	// check front transition timeout
+	if (_param_vt_trans_timeout.get()  > FLT_EPSILON && _common_vtol_mode == mode::TRANSITION_TO_FW) {
+
+		if (_time_since_trans_start > _param_vt_trans_timeout.get()) {
+			// transition timeout occured, abort transition
+			return true;
+		}
+	}
+
+	return false;
+}
+
+QuadchuteReason VtolType::getQuadchuteReason()
+{
+	if (isMinAltBreached()) {
+		return QuadchuteReason::MinimumAltBreached;
+	}
+
+	if (largeAltitudeLoss()) {
+		return QuadchuteReason::LossOfAlt;
+	}
+
+	if (largeAltitudeError()) {
+		return QuadchuteReason::LargeAltError;
+	}
+
+	if (isPitchExceeded()) {
+		return QuadchuteReason::MaximumPitchExceeded;
+	}
+
+	if (isRollExceeded()) {
+		return QuadchuteReason::MaximumRollExceeded;
+	}
+
+	if (isFrontTransitionTimeout()) {
+		return QuadchuteReason::TransitionTimeout;
+	}
+
+	return QuadchuteReason::None;
+}
+
+void VtolType::filterTecsHeightRates()
+{
+	if (_tecs_running) {
+		// 1 second rolling average
+		_ra_hrate = (49 * _ra_hrate + _tecs_status->height_rate) / 50;
+		_ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50;
+
+	} else {
+		// reset the filtered height rate and heigh rate setpoint if TECS is not running
+		_ra_hrate = 0.0f;
+		_ra_hrate_sp = 0.0f;
+	}
+}
+
+void VtolType::handleSpecialExternalCommandQuadchute()
 {
 	if (_attc->get_transition_command() == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC && _attc->get_immediate_transition()
 	    && !_quadchute_command_treated) {
-		_attc->quadchute(VtolAttitudeControl::QuadchuteReason::ExternalCommand);
+		_attc->quadchute(QuadchuteReason::ExternalCommand);
 		_quadchute_command_treated = true;
 		_attc->reset_immediate_transition();
 
 	} else {
 		_quadchute_command_treated = false;
 	}
+}
 
-	if (!_tecs_running) {
-		// reset the filtered height rate and heigh rate setpoint if TECS is not running
-		_ra_hrate = 0.0f;
-		_ra_hrate_sp = 0.0f;
-	}
+void VtolType::check_quadchute_condition()
+{
 
-	if (_v_control_mode->flag_armed && !_land_detected->landed) {
-		Eulerf euler = Quatf(_v_att->q);
+	filterTecsHeightRates();
+	handleSpecialExternalCommandQuadchute();
 
-		// fixed-wing minimum altitude
-		if (_param_vt_fw_min_alt.get() > FLT_EPSILON) {
+	if (isQuadchuteEnabled()) {
+		QuadchuteReason reason = getQuadchuteReason();
 
-			if (-(_local_pos->z) < _param_vt_fw_min_alt.get()) {
-				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MinimumAltBreached);
-			}
-		}
-
-		// adaptive quadchute
-		if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) {
-
-			// We use tecs for tracking in FW and local_pos_sp during transitions
-			if (_tecs_running) {
-				// 1 second rolling average
-				_ra_hrate = (49 * _ra_hrate + _tecs_status->height_rate) / 50;
-				_ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50;
-
-				// are we dropping while requesting significant ascend?
-				if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _param_vt_fw_alt_err.get()) &&
-				    (_ra_hrate < -1.0f) &&
-				    (_ra_hrate_sp > 1.0f)) {
-
-					_attc->quadchute(VtolAttitudeControl::QuadchuteReason::LossOfAlt);
-				}
-
-			} else {
-				const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _param_vt_fw_alt_err.get());
-				const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f);
-
-				if (height_error && height_rate_error) {
-					_attc->quadchute(VtolAttitudeControl::QuadchuteReason::LargeAltError);
-				}
-			}
-		}
-
-		// fixed-wing maximum pitch angle
-		if (_param_vt_fw_qc_p.get() > 0) {
-
-			if (fabsf(euler.theta()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_p.get())))) {
-				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumPitchExceeded);
-			}
-		}
-
-		// fixed-wing maximum roll angle
-		if (_param_vt_fw_qc_r.get() > 0) {
-
-			if (fabsf(euler.phi()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_r.get())))) {
-				_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumRollExceeded);
-			}
+		if (reason != QuadchuteReason::None) {
+			_attc->quadchute(reason);
 		}
 	}
 }
@@ -543,11 +650,19 @@ bool VtolType::is_channel_set(const int channel, const int bitmap)
 
 float VtolType::pusher_assist()
 {
-	// Altitude above ground is distance sensor altitude if available, otherwise local z-position
-	float dist_to_ground = -_local_pos->z;
+	// Altitude above ground is local z-position or altitude above home or distance sensor altitude depending on what's available
+	float dist_to_ground = 0.f;
+	const float home_position_z = _attc->get_home_position_z();
 
 	if (_local_pos->dist_bottom_valid) {
 		dist_to_ground = _local_pos->dist_bottom;
+
+	} else if (PX4_ISFINITE(home_position_z)) {
+		dist_to_ground = -(_local_pos->z - home_position_z);
+
+	} else {
+		dist_to_ground = -_local_pos->z;
+
 	}
 
 	// disable pusher assist depending on setting of forward_thrust_enable_mode:
@@ -678,6 +793,165 @@ float VtolType::pusher_assist()
 
 }
 
+bool VtolType::override_controls_for_test_mode()
+{
+	if (_v_control_mode->flag_armed) {
+		// if vehicle arms immediately deactivate test mode
+		return false;
+	}
+
+	if (_actuator_test_type == actuator_test_type::TYPE_CONTROL_SURFACES) {
+		/* Change actuator position every 1_s*/
+		if (hrt_elapsed_time(&_timestamp_new_state) >= 1_s) {
+			_timestamp_new_state = hrt_absolute_time();
+
+			switch (_actuator_test_position_state) {
+
+			case actuator_test_position_state::STATE_POSITION_WAIT:
+				_actuator_control_value_target = ACTUATOR_POSITIVE_DIRECTION;
+				_actuator_test_position_state = actuator_test_position_state::STATE_POSITION_POSITIVE;
+				break;
+
+			case actuator_test_position_state::STATE_POSITION_POSITIVE:
+				_actuator_test_position_state = actuator_test_position_state::STATE_POSITION_WAIT_1;
+				break;
+
+			case actuator_test_position_state::STATE_POSITION_NEGATIVE:
+				_actuator_test_position_state = actuator_test_position_state::STATE_POSITION_WAIT_2;
+				break;
+
+			case actuator_test_position_state::STATE_POSITION_WAIT_1:
+				_actuator_control_value_target = ACTUATOR_NEGATIVE_DIRECTION;
+				_actuator_test_position_state = actuator_test_position_state::STATE_POSITION_NEGATIVE;
+				break;
+
+			case actuator_test_position_state::STATE_POSITION_WAIT_2:
+				_actuator_test_position_state = actuator_test_position_state::STATE_POSITION_END;
+				_actuator_control_value_target = ACTUATOR_CENTRAL_DIRECTION;
+				break;
+
+			case actuator_test_position_state::STATE_POSITION_END:
+				break;
+
+			default:
+				break;
+			}
+		}
+
+	} else {
+		/* Change actuator position every 2_s for tilt check*/
+		if (hrt_elapsed_time(&_timestamp_new_state) >= 2_s) {
+			_timestamp_new_state = hrt_absolute_time();
+
+			switch (_actuator_test_position_state) {
+
+			case actuator_test_position_state::STATE_POSITION_WAIT:
+				_actuator_control_value_target = ACTUATOR_CENTRAL_DIRECTION;
+				_actuator_test_position_state = actuator_test_position_state::STATE_POSITION_CENTRAL;
+				break;
+
+			case actuator_test_position_state::STATE_POSITION_POSITIVE:
+				_actuator_test_position_state = actuator_test_position_state::STATE_POSITION_WAIT_2;
+				break;
+
+			case actuator_test_position_state::STATE_POSITION_CENTRAL:
+				_actuator_test_position_state = actuator_test_position_state::STATE_POSITION_WAIT_1;
+				break;
+
+			case actuator_test_position_state::STATE_POSITION_WAIT_1:
+				_actuator_control_value_target = ACTUATOR_POSITIVE_DIRECTION;
+				_actuator_test_position_state = actuator_test_position_state::STATE_POSITION_POSITIVE;
+				break;
+
+			case actuator_test_position_state::STATE_POSITION_WAIT_2:
+				_actuator_control_value_target = ACTUATOR_CENTRAL_DIRECTION;
+				_actuator_test_position_state = actuator_test_position_state::STATE_POSITION_END;
+				break;
+
+			default:
+				break;
+			}
+		}
+	}
+
+	if (_actuator_test_type == actuator_test_type::TYPE_CONTROL_SURFACES) {
+
+		switch (_control_surfaces_test_state) {
+
+		case control_surfaces_test_state::STATE_TEST_AILERON:
+			_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = _actuator_control_value_target;
+			_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = ACTUATOR_CENTRAL_DIRECTION;
+			_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = ACTUATOR_CENTRAL_DIRECTION;
+
+			if (_actuator_test_position_state == actuator_test_position_state::STATE_POSITION_END) {
+				_control_surfaces_test_state = control_surfaces_test_state::STATE_TEST_ELEVATOR;
+
+				/* After end reinitialize to STATE_POSITION_WAIT for new round of tests */
+				_actuator_test_position_state = actuator_test_position_state::STATE_POSITION_WAIT;
+			}
+
+			break;
+
+		case control_surfaces_test_state::STATE_TEST_ELEVATOR:
+			_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = ACTUATOR_CENTRAL_DIRECTION;
+			_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = _actuator_control_value_target;
+			_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = ACTUATOR_CENTRAL_DIRECTION;
+
+			if (_actuator_test_position_state == actuator_test_position_state::STATE_POSITION_END) {
+				_control_surfaces_test_state = control_surfaces_test_state::STATE_TEST_RUDDER;
+
+				/* After end reinitialize to STATE_POSITION_WAIT for new round of tests */
+				_actuator_test_position_state = actuator_test_position_state::STATE_POSITION_WAIT;
+			}
+
+			break;
+
+		case control_surfaces_test_state::STATE_TEST_RUDDER:
+			_actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = ACTUATOR_CENTRAL_DIRECTION;
+			_actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = ACTUATOR_CENTRAL_DIRECTION;
+			_actuators_out_1->control[actuator_controls_s::INDEX_YAW] = _actuator_control_value_target;
+
+			if (_actuator_test_position_state == actuator_test_position_state::STATE_POSITION_END) {
+				_control_surfaces_test_state = control_surfaces_test_state::STATE_TEST_END;
+			}
+
+			break;
+
+		default:
+			break;
+		}
+
+	} else if (_actuator_test_type == actuator_test_type::TYPE_TILT) {
+		_actuators_out_1->control[actuator_controls_s::INDEX_COLLECTIVE_TILT] = _actuator_control_value_target;
+	}
+
+	const bool checks_done = (_actuator_test_type == actuator_test_type::TYPE_CONTROL_SURFACES &&
+				  _control_surfaces_test_state == control_surfaces_test_state::STATE_TEST_END) ||
+				 (_actuator_test_type == actuator_test_type::TYPE_TILT &&
+				  _actuator_test_position_state == actuator_test_position_state::STATE_POSITION_END);
+
+	return !checks_done;
+}
+
+void VtolType::activate_actuator_test_mode(actuator_test_type test_type)
+{
+	if (test_type > actuator_test_type::TYPE_CONTROL_SURFACES || test_type <= actuator_test_type::TYPE_NONE
+	    || _v_control_mode->flag_armed) {
+		return;
+	}
+
+	if (test_type == actuator_test_type::TYPE_TILT && _param_vt_type.get() != (int32_t)vtol_type::TILTROTOR) {
+		return;
+	}
+
+	_in_actuator_test_mode = true;
+	_actuator_test_start_ts = hrt_absolute_time();
+	_actuator_test_type = test_type;
+	_control_surfaces_test_state = control_surfaces_test_state::STATE_TEST_AILERON;
+	_actuator_test_position_state = actuator_test_position_state::STATE_POSITION_WAIT;
+	_timestamp_new_state = hrt_absolute_time();
+}
+
 float VtolType::getFrontTransitionTimeFactor() const
 {
 	// assumptions: transition_time = transition_true_airspeed / average_acceleration (thrust)
diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h
index b8dbd554d7..dbd19fcf57 100644
--- a/src/modules/vtol_att_control/vtol_type.h
+++ b/src/modules/vtol_att_control/vtol_type.h
@@ -53,6 +53,11 @@
 static constexpr float kFlapSlewRateVtol = 1.f; // minimum time from none to full flap deflection [s]
 static constexpr float kSpoilerSlewRateVtol = 1.f; // minimum time from none to full spoiler deflection [s]
 
+using namespace time_literals;
+
+static constexpr float ACTUATOR_POSITIVE_DIRECTION{1.0f};
+static constexpr float ACTUATOR_CENTRAL_DIRECTION{0.0f};
+static constexpr float ACTUATOR_NEGATIVE_DIRECTION{-1.0f};
 
 // Has to match 1:1 msg/vtol_vehicle_status.msg
 enum class mode {
@@ -97,6 +102,41 @@ enum class pwm_limit_type {
 	TYPE_MAXIMUM
 };
 
+// defines the pre-flight control surface and tilting mechanism test types
+enum class actuator_test_type {
+	TYPE_NONE = 0,
+	TYPE_TILT,
+	TYPE_CONTROL_SURFACES
+};
+
+enum class control_surfaces_test_state {
+	STATE_TEST_AILERON = 0,
+	STATE_TEST_ELEVATOR,
+	STATE_TEST_RUDDER,
+	STATE_TEST_END
+};
+
+enum class actuator_test_position_state {
+	STATE_POSITION_WAIT = 0,		//!< Wait, do not output any actuator setpoint
+	STATE_POSITION_POSITIVE,		//!< The actuator is moving from central to positive position
+	STATE_POSITION_NEGATIVE,		//!< The actuator is moving from central to negative position
+	STATE_POSITION_CENTRAL,			//!< The actuator is moving from positive to central position
+	STATE_POSITION_WAIT_1,			//!< The actuator is waiting in positive position
+	STATE_POSITION_WAIT_2,			//!< The actuator is waiting in negative position
+	STATE_POSITION_END			//!< The actuator is moving to end position (central)
+};
+
+enum class QuadchuteReason {
+	None = 0,
+	TransitionTimeout,
+	ExternalCommand,
+	MinimumAltBreached,
+	LossOfAlt,
+	LargeAltError,
+	MaximumPitchExceeded,
+	MaximumRollExceeded,
+};
+
 class VtolAttitudeControl;
 
 class VtolType:  public ModuleParams
@@ -129,6 +169,13 @@ public:
 	 */
 	virtual void update_mc_state();
 
+
+	/**
+	 * Runs code wich will be executed at each loop iteration, does not need to be tied to
+	 * attitude setpoint updates.
+	 */
+	virtual void update_mc_generic() {};
+
 	/**
 	 * Update fixed wing state.
 	 */
@@ -145,6 +192,73 @@ public:
 	 */
 	virtual void waiting_on_tecs() {}
 
+	/**
+	 *  @brief Indicates if quadchute is enabled.
+	 *
+	 * @return     true if enabled
+	 */
+	bool isQuadchuteEnabled();
+
+	/**
+	 *  @brief Evaluates quadchute conditions and returns a reson for quadchute.
+	 *
+	 * @return     QuadchuteReason, can be None
+	 */
+	QuadchuteReason getQuadchuteReason();
+
+	/**
+	 *  @brief Indicates if the vehicle is lower than VT_FW_MIN_ALT above the local origin.
+	 *
+	 * @return     true if below threshold
+	 */
+	bool isMinAltBreached();
+
+	/**
+	 *  @brief Indicates if the vehicle has an altitude error larger than VT_FW_ALT_ERR and is losing altitude quickly.
+	 * 		This only applies when TECS is running.
+	 *
+	 * @return     true if error larger than threshold
+	 */
+	bool largeAltitudeLoss();
+
+	/**
+	 *  @brief Indicates if the vehicle has an altitude error larger than VT_FW_ALT_ERR. This only applied when TECS is not running.
+	 *
+	 * @return     true if error larger than threshold
+	 */
+	bool largeAltitudeError();
+
+	/**
+	 *  @brief Indicates if the absolute value of the vehicle pitch angle exceeds the threshold defined by VT_FW_QC_P
+	 *
+	 * @return     true if exeeded
+	 */
+	bool isPitchExceeded();
+
+	/**
+	 *  @brief Indicates if the absolute value of the vehicle roll angle exceeds the threshold defined by VT_FW_QC_R
+	 *
+	 * @return     true if exeeded
+	 */
+	bool isRollExceeded();
+
+	/**
+	 *  @brief Indicates if the front transition duration has exceeded the timeout definded by VT_TRANS_TIMEOUT
+	 *
+	 * @return     true if exeeded
+	 */
+	bool isFrontTransitionTimeout();
+
+	/**
+	 *  @brief Applied a first order low pass filte to TECS height rate and heigh rate setpoint.
+	 */
+	void filterTecsHeightRates();
+
+	/**
+	 *  @brief Special handling of QuadchuteReason::ReasonExternal
+	 */
+	void handleSpecialExternalCommandQuadchute();
+
 	/**
 	 * Checks for fixed-wing failsafe condition and issues abort request if needed.
 	 */
@@ -160,11 +274,19 @@ public:
 	 */
 	float pusher_assist();
 
+	/**
+	 * Activate a pre-flight control surface or tilt mechanism functionality check, according to test_type.
+	 */
+	void activate_actuator_test_mode(actuator_test_type test_type);
+
+	/**
+	 * Returns true if we are currently executing a control surface or tilt mechanism functionality check.
+	 */
+	bool in_actuator_test_mode() {return _in_actuator_test_mode;}
+
 	virtual void blendThrottleAfterFrontTransition(float scale) {};
 
-	mode get_mode() {return _vtol_mode;}
-
-	bool was_in_trans_mode() {return _flag_was_in_trans_mode;}
+	mode get_mode() {return _common_vtol_mode;}
 
 	/**
 	 * @return Minimum front transition time scaled for air density (if available) [s]
@@ -187,7 +309,7 @@ public:
 
 protected:
 	VtolAttitudeControl *_attc;
-	mode _vtol_mode;
+	mode _common_vtol_mode;
 
 	static constexpr const int num_outputs_max = 8;
 
@@ -203,7 +325,7 @@ protected:
 	struct actuator_controls_s			*_actuators_fw_in;			//actuator controls from fw_att_control
 	struct vehicle_local_position_s			*_local_pos;
 	struct vehicle_local_position_setpoint_s	*_local_pos_sp;
-	struct airspeed_validated_s 				*_airspeed_validated;					// airspeed
+	struct airspeed_validated_s 			*_airspeed_validated;					// airspeed
 	struct tecs_status_s				*_tecs_status;
 	struct vehicle_land_detected_s			*_land_detected;
 
@@ -229,6 +351,8 @@ protected:
 	bool _flag_was_in_trans_mode = false;	// true if mode has just switched to transition
 
 	hrt_abstime _trans_finished_ts = 0;
+	hrt_abstime _transition_start_timestamp{0};
+	float _time_since_trans_start{0};
 
 	bool _tecs_running = false;
 	hrt_abstime _tecs_running_ts = 0;
@@ -242,7 +366,13 @@ protected:
 	float _accel_to_pitch_integ = 0;
 
 	bool _quadchute_command_treated{false};
-
+	bool _in_actuator_test_mode{false};
+	hrt_abstime _actuator_test_start_ts{0};
+	hrt_abstime _timestamp_new_state{0};
+	float _actuator_control_value_target{ACTUATOR_CENTRAL_DIRECTION};
+	actuator_test_type _actuator_test_type{actuator_test_type::TYPE_NONE};
+	control_surfaces_test_state _control_surfaces_test_state{control_surfaces_test_state::STATE_TEST_AILERON};
+	actuator_test_position_state _actuator_test_position_state{actuator_test_position_state::STATE_POSITION_POSITIVE};
 
 	/**
 	 * @brief      Sets mc motor minimum pwm to VT_IDLE_PWM_MC which ensures
@@ -268,6 +398,13 @@ protected:
 
 	float update_and_get_backtransition_pitch_sp();
 
+	/**
+	 * @brief      Overrides controls for control surface or tilt mechanism in the context of preflight tests.
+	 *
+	 * @return     true while executing the test, false once done
+	 */
+	bool override_controls_for_test_mode();
+
 	SlewRate<float> _spoiler_setpoint_with_slewrate;
 	SlewRate<float> _flaps_setpoint_with_slewrate;
 
@@ -279,6 +416,7 @@ protected:
 					(ParamFloat<px4::params::VT_FW_ALT_ERR>) _param_vt_fw_alt_err,
 					(ParamInt<px4::params::VT_FW_QC_P>) _param_vt_fw_qc_p,
 					(ParamInt<px4::params::VT_FW_QC_R>) _param_vt_fw_qc_r,
+					(ParamFloat<px4::params::VT_FW_QC_HMAX>) _param_quadchute_max_height,
 					(ParamFloat<px4::params::VT_F_TR_OL_TM>) _param_vt_f_tr_ol_tm,
 					(ParamFloat<px4::params::VT_TRANS_MIN_TM>) _param_vt_trans_min_tm,
 
@@ -309,13 +447,14 @@ protected:
 					(ParamInt<px4::params::VT_MOT_ID>) _param_vt_mot_id,
 					(ParamBool<px4::params::VT_MC_ON_FMU>) _param_vt_mc_on_fmu,
 					(ParamInt<px4::params::VT_FW_MOT_OFFID>) _param_vt_fw_mot_offid,
-					(ParamFloat<px4::params::VT_SPOILER_MC_LD>) _param_vt_spoiler_mc_ld
+					(ParamFloat<px4::params::VT_SPOILER_MC_LD>) _param_vt_spoiler_mc_ld,
+
+					(ParamInt<px4::params::VT_TYPE>) _param_vt_type // APX4 custom, needed for actuator test
 
 				       )
 
 private:
 
-
 	hrt_abstime _throttle_blend_start_ts{0};	// time at which we start blending between transition throttle and fixed wing throttle
 
 	/**
diff --git a/src/systemcmds/actuator_endurance_test/CMakeLists.txt b/src/systemcmds/actuator_endurance_test/CMakeLists.txt
new file mode 100644
index 0000000000..db5bdcb9f4
--- /dev/null
+++ b/src/systemcmds/actuator_endurance_test/CMakeLists.txt
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+px4_add_module(
+	MODULE systemcmds__actuator_endurance_test
+	MAIN actuator_endurance_test
+	STACK_MAIN 4096
+	COMPILE_FLAGS
+	SRCS
+	actuator_endurance_test.cpp
+	DEPENDS
+	px4_work_queue
+)
diff --git a/src/systemcmds/actuator_endurance_test/Kconfig b/src/systemcmds/actuator_endurance_test/Kconfig
new file mode 100644
index 0000000000..e682a382b8
--- /dev/null
+++ b/src/systemcmds/actuator_endurance_test/Kconfig
@@ -0,0 +1,5 @@
+menuconfig SYSTEMCMDS_ACTUATOR_ENDURANCE_TEST
+	bool "actuator_endurance_test"
+	default n
+	---help---
+		Enable support for actuator_endurance_test
diff --git a/src/systemcmds/actuator_endurance_test/actuator_endurance_test.cpp b/src/systemcmds/actuator_endurance_test/actuator_endurance_test.cpp
new file mode 100644
index 0000000000..e1557851c3
--- /dev/null
+++ b/src/systemcmds/actuator_endurance_test/actuator_endurance_test.cpp
@@ -0,0 +1,394 @@
+/****************************************************************************
+ *
+ *   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 "actuator_endurance_test.hpp"
+
+using namespace time_literals;
+
+int ActuatorEnduranceTest::print_status()
+{
+	int time_remaining = (int)((_time_finished_abs - hrt_absolute_time()) / 1_s);
+
+	if (_execute.load()) {
+		PX4_INFO("running: %i seconds remaining", time_remaining);
+
+	} else {
+		PX4_INFO("running: timeout in %i seconds", time_remaining);
+	}
+
+	print_params();
+
+	return 0;
+}
+
+int ActuatorEnduranceTest::task_spawn(int argc, char *argv[])
+{
+	_task_id = px4_task_spawn_cmd(
+			   "actuator_endurance_test",
+			   SCHED_DEFAULT,
+			   SCHED_PRIORITY_DEFAULT,
+			   4096,
+			   (px4_main_t)&run_trampoline,
+			   (char *const *)argv);
+
+	if (_task_id < 0) {
+		_task_id = -1;
+		return -errno;
+	}
+
+	return 0;
+}
+
+ActuatorEnduranceTest *ActuatorEnduranceTest::instantiate(int argc, char *argv[])
+{
+	ActuatorEnduranceTest *instance = new ActuatorEnduranceTest();
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+	}
+
+	return instance;
+}
+
+int ActuatorEnduranceTest::custom_command(int argc, char *argv[])
+{
+	if (!is_running()) {
+		PX4_INFO("not running");
+		return PX4_ERROR;
+	}
+
+	ActuatorEnduranceTest *instance = get_instance();
+
+	// bump timeout
+	instance->_time_finished_abs = hrt_absolute_time() + _module_timeout;
+
+	// default values unset
+	int motor = -1;
+	int profile = -1;
+	float frequency = -1.0f;
+	float min = -1.0f;
+	float max = -1.0f;
+	int duration = -1;
+	int interval = -1;
+	int driver = -1;
+
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = nullptr;
+
+	while ((ch = px4_getopt(argc, argv, "d:t:i:m:p:f:l:h:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'd':
+			driver = (int)strtol(myoptarg, nullptr, 0);
+			break;
+
+		case 't':
+			duration = (int)strtol(myoptarg, nullptr, 0);
+			break;
+
+		case 'i':
+			interval = (int)strtol(myoptarg, nullptr, 0);
+			break;
+
+		case 'm':
+			motor = (int)strtol(myoptarg, nullptr, 0);
+			break;
+
+		case 'p':
+			profile = (int)strtol(myoptarg, nullptr, 0);
+			break;
+
+		case 'f':
+			frequency = strtof(myoptarg, nullptr);
+			break;
+
+		case 'l':
+			min = strtof(myoptarg, nullptr);
+			break;
+
+		case 'h':
+			max = strtof(myoptarg, nullptr);
+			break;
+
+		default:
+			print_usage(nullptr);
+			return 1;
+		}
+	}
+
+	if (argc == 1 && !strcmp(argv[0], "execute")) {
+		instance->execute();
+		return PX4_OK;
+
+	} else if (!strcmp(argv[0], "set")) {
+
+		TestParams new_params = instance->get_params();
+
+		// check global settings
+		if (driver >= 0) {
+			new_params.driver_instance = (uint8_t)driver;
+		}
+
+		if (interval >= 0) {
+			new_params.cmd_interval_ms = (uint8_t)interval;
+		}
+
+		if (duration >= 0) {
+			new_params.test_duration_s = (uint32_t)duration;
+		}
+
+		// check motor settings
+		if (motor >= 0 && motor <= instance->_num_channels) {
+			// motor valid
+			if (profile >= 0 && profile <= 2) {
+				new_params.profile[motor] = (Profile)profile;
+			}
+
+			if (frequency >= 0.0f) {
+				new_params.frequency[motor] = frequency;
+			}
+
+			if (min >= 0.0f) {
+				new_params.min[motor] = min;
+			}
+
+			if (max >= 0.0f) {
+				new_params.max[motor] = max;
+			}
+		}
+
+		if (instance->set_params(new_params)) {
+			PX4_INFO("params accepted");
+			return PX4_OK;
+
+		} else {
+			print_usage("params rejected");
+			return PX4_ERROR;
+		}
+
+	}
+
+	return print_usage("unknown command");
+}
+
+int ActuatorEnduranceTest::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(R"DESCR_STR(
+Utility to test actuators.
+WARNING: remove all props before using this command.
+)DESCR_STR");
+	PRINT_MODULE_USAGE_NAME("actuator_endurance_test", "command");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "start the module (no actuator outputs yet)");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("set", "define the test parameters using the flags below");
+	PRINT_MODULE_USAGE_PARAM_COMMENT("global settings, define these first");
+	PRINT_MODULE_USAGE_PARAM_INT('d', -1, 0, 4, "driver instance", false);
+	PRINT_MODULE_USAGE_PARAM_INT('t', 0, 0, UINT32_MAX, "test duration [s]", false);
+	PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, UINT8_MAX, "min interval [ms] between commands per channel", false);
+	PRINT_MODULE_USAGE_PARAM_COMMENT("motor settings");
+	PRINT_MODULE_USAGE_PARAM_INT('m', 0, 0, _num_channels, "driver instance", true);
+	PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 3, "profile: 0 = sine, 1 = triangle, 2 = square", true);
+	PRINT_MODULE_USAGE_PARAM_FLOAT('f', 0.0, 0.0, 1.0, "frequency [Hz]", true);
+	PRINT_MODULE_USAGE_PARAM_FLOAT('l', 0.0, 0.0, 1.0, "normalized min value", true);
+	PRINT_MODULE_USAGE_PARAM_FLOAT('h', 1.0, 0.0, 1.0, "normalized max value", true);
+	PRINT_MODULE_USAGE_COMMAND_DESCR("status", "prints the time remaining and the current settings");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("execute", "execute the actuator test with the current settings");
+	PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "send actuator disarm value and stop the module");
+	return 0;
+}
+
+void ActuatorEnduranceTest::run()
+{
+	PX4_INFO("started");
+	_time_finished_abs = hrt_absolute_time() + _module_timeout; // timeout if test not started
+
+	while (true) {
+		if (should_exit()) {
+			PX4_INFO("stopped");
+			break;
+
+		} else if (hrt_absolute_time() > _time_finished_abs) {
+			PX4_INFO("timed out");
+			break;
+
+		} else if (_execute.load()) {
+			PX4_INFO("execute test");
+			run_test();
+			PX4_INFO("done");
+			break;
+		}
+
+		px4_usleep(100_ms);
+	}
+
+	exit_and_cleanup();
+}
+
+int ActuatorEnduranceTest::print_params()
+{
+	PX4_INFO("*** Test Parameters ***");
+	PX4_INFO("driver instance: %i ", _test_params.driver_instance);
+	PX4_INFO("act cmd interval: %i milliseconds ", (int)_test_params.cmd_interval_ms);
+	PX4_INFO("test duration: %i seconds", (int)_test_params.test_duration_s);
+
+	for (unsigned ch = 0; ch < _num_channels; ch++) {
+		PX4_INFO("channel %i: profile: %i, frequency: %.2f, min: %.3f, max: %.3f",
+			 ch,
+			 (int)_test_params.profile[ch],
+			 (double)_test_params.frequency[ch],
+			 (double)_test_params.min[ch],
+			 (double)_test_params.max[ch]);
+	}
+
+	return 0;
+}
+
+const ActuatorEnduranceTest::TestParams& ActuatorEnduranceTest::get_params()
+{
+	return _test_params;
+}
+
+bool ActuatorEnduranceTest::set_params(const TestParams &test_params)
+{
+	if (!_execute.load() && params_feasible(test_params)) {
+		_test_params = test_params;
+		return true;
+
+	}
+
+	return false;
+}
+
+void ActuatorEnduranceTest::execute()
+{
+	if (params_feasible(_test_params)) {
+		_execute.store(true);
+
+	} else {
+		PX4_INFO("execute rejected: parameters infeasible");
+	}
+}
+
+bool ActuatorEnduranceTest::params_feasible(const TestParams &test_params)
+{
+	for (unsigned ch = 0; ch < _num_channels; ch++) {
+		if (test_params.driver_instance < 0 ||
+		    test_params.driver_instance > 4 ||
+		    test_params.frequency[ch] < 0.0f ||
+		    test_params.frequency[ch] > (float)_max_act_frequency_hz ||
+		    test_params.min[ch] < 0.0f ||
+		    test_params.min[ch] > 1.0f ||
+		    test_params.max[ch] < 0.0f ||
+		    test_params.max[ch] > 1.0f ||
+		    test_params.min[ch] > test_params.max[ch]) {
+			return false;
+		}
+	}
+
+	return true;
+}
+
+void ActuatorEnduranceTest::run_test()
+{
+	hrt_abstime start_time = hrt_absolute_time();
+	_time_finished_abs = start_time + _test_params.test_duration_s * 1_s;
+
+	while (hrt_absolute_time() < _time_finished_abs) {
+
+		if (should_exit()) {
+			stop_motors();
+			exit_and_cleanup();
+			return;
+		}
+
+		for (size_t ch = 0; ch < _num_channels; ch++) {
+
+			if (_test_params.frequency[ch] > 0.0f) {
+
+				float val = -1.0f;
+				float tf = (float)hrt_elapsed_time(&start_time) / 1e6f * _test_params.frequency[ch];
+
+				if (_test_params.profile[ch] == Profile::Sine) {
+					val = (0.5f * sinf(2 * M_PI_F * tf) + 0.5f);
+
+				} else if (_test_params.profile[ch] == Profile::Triangle) {
+					val = 2.0f * fabsf(tf - floorf(tf + 0.5f));
+
+				} else if (_test_params.profile[ch] == Profile::Square) {
+					val = (2.0f * floorf(tf) - floorf(2.0f * tf)) + 1.0f;
+				}
+
+				val = _test_params.min[ch] + (_test_params.max[ch] - _test_params.min[ch]) * val;
+
+				if (hrt_elapsed_time(&_last_command_time[ch]) > _test_params.cmd_interval_ms * 1_ms) {
+					test_motor_pub(ch, val, _test_params.driver_instance, 0);
+					_last_command_time[ch] = hrt_absolute_time();
+				}
+			}
+		}
+
+		px4_usleep(_control_period_us);
+	}
+
+	stop_motors();
+}
+
+void ActuatorEnduranceTest::test_motor_pub(unsigned channel, float value, uint8_t driver_instance, int timeout_ms)
+{
+	test_motor_s test_motor{};
+	test_motor.timestamp = hrt_absolute_time();
+	test_motor.motor_number = channel;
+	test_motor.value = value;
+	test_motor.action = value >= 0.f ? test_motor_s::ACTION_RUN : test_motor_s::ACTION_STOP;
+	test_motor.driver_instance = driver_instance;
+	test_motor.timeout_ms = timeout_ms;
+
+	_test_motor_pub.publish(test_motor);
+
+	if (test_motor.action == test_motor_s::ACTION_STOP) {
+		PX4_INFO("motors stop command sent");
+	}
+}
+
+void ActuatorEnduranceTest::stop_motors()
+{
+	test_motor_pub(-1, -1.0f, _test_params.driver_instance, 0);
+}
+
+extern "C" __EXPORT int actuator_endurance_test_main(int argc, char *argv[])
+{
+	return ActuatorEnduranceTest::main(argc, argv);
+}
diff --git a/src/systemcmds/actuator_endurance_test/actuator_endurance_test.hpp b/src/systemcmds/actuator_endurance_test/actuator_endurance_test.hpp
new file mode 100644
index 0000000000..b652e731c8
--- /dev/null
+++ b/src/systemcmds/actuator_endurance_test/actuator_endurance_test.hpp
@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ *   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 <math.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/posix.h>
+#include <px4_platform_common/getopt.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
+
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/test_motor.h>
+#include <uORB/topics/vehicle_status_flags.h>
+
+using namespace time_literals;
+
+class ActuatorEnduranceTest : public ModuleBase<ActuatorEnduranceTest>
+{
+public:
+	static constexpr uint8_t _num_channels{8};
+	static constexpr uint32_t _control_frequency_hz{100};
+	static constexpr uint32_t _control_period_us{1_s / _control_frequency_hz};
+	static constexpr uint8_t _max_act_frequency_hz{1};
+	static constexpr hrt_abstime _module_timeout{60_s};
+
+	enum class Profile {
+		Sine = 0,
+		Triangle = 1,
+		Square = 2,
+	};
+
+	struct TestParams {
+		int8_t driver_instance = -1;
+		uint8_t cmd_interval_ms = 0;
+		uint32_t test_duration_s = 0;
+		Profile profile[_num_channels] {};
+		float frequency[_num_channels] {};
+		float min[_num_channels] {};
+		float max[_num_channels] {};
+	};
+
+	ActuatorEnduranceTest() = default;
+
+	~ActuatorEnduranceTest() = default;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static ActuatorEnduranceTest *instantiate(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::run() */
+	void run() override;
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	int print_params();
+
+	const TestParams &get_params();
+
+	bool set_params(const TestParams &test_params);
+
+	void execute();
+
+private:
+	hrt_abstime _time_finished_abs{0};
+	hrt_abstime _last_command_time[_num_channels] {};
+	TestParams _test_params;
+	px4::atomic_bool _execute{false};
+
+	bool params_feasible(const TestParams &test_params);
+
+	void run_test();
+
+	void test_motor_pub(unsigned channel, float value, uint8_t driver_instance, int act_timeout_ms);
+
+	void stop_motors();
+
+	uORB::Publication<test_motor_s> _test_motor_pub{ORB_ID(test_motor)};
+};
diff --git a/src/systemcmds/bl_update/bl_update.cpp b/src/systemcmds/bl_update/bl_update.cpp
index 535402a468..bf4d0ea903 100644
--- a/src/systemcmds/bl_update/bl_update.cpp
+++ b/src/systemcmds/bl_update/bl_update.cpp
@@ -62,7 +62,7 @@
 #  define BL_FILE_SIZE_LIMIT	32*1024
 #  define STM_RAM_BASE        STM32_SRAM_BASE
 #else
-#  define BL_FILE_SIZE_LIMIT  16384
+#  define BL_FILE_SIZE_LIMIT  32768
 #  define STM_RAM_BASE        STM32_SRAM_BASE
 #endif
 
diff --git a/src/systemcmds/failure/failure.cpp b/src/systemcmds/failure/failure.cpp
index 527608f59d..2b28aefde8 100644
--- a/src/systemcmds/failure/failure.cpp
+++ b/src/systemcmds/failure/failure.cpp
@@ -139,7 +139,7 @@ int inject_failure(const FailureUnit& unit, const FailureType& type, uint8_t ins
 	while (hrt_elapsed_time(&command.timestamp) < 1_s) {
 		if (command_ack_sub.update(&ack)) {
 			if (ack.command == command.command) {
-				if (ack.result != vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED) {
+				if (ack.result != vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED) {
 					PX4_ERR("Result: %d", ack.result);
 					return 1;
 
diff --git a/src/systemcmds/gpio/gpio.cpp b/src/systemcmds/gpio/gpio.cpp
index 0b1458f9b6..343c1bbd18 100644
--- a/src/systemcmds/gpio/gpio.cpp
+++ b/src/systemcmds/gpio/gpio.cpp
@@ -42,6 +42,8 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/px4_config.h>
 
+#include <stdlib.h>
+
 #include <nuttx/ioexpander/gpio.h>
 #include <fcntl.h>
 
diff --git a/src/systemcmds/i2cdetect/i2cdetect.cpp b/src/systemcmds/i2cdetect/i2cdetect.cpp
index f02f3570e3..cf2ce1290a 100644
--- a/src/systemcmds/i2cdetect/i2cdetect.cpp
+++ b/src/systemcmds/i2cdetect/i2cdetect.cpp
@@ -43,6 +43,8 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/getopt.h>
 
+#include <stdlib.h>
+
 namespace i2cdetect
 {
 
diff --git a/src/systemcmds/mtd/mtd.cpp b/src/systemcmds/mtd/mtd.cpp
index 609c7ada29..1aee24b958 100644
--- a/src/systemcmds/mtd/mtd.cpp
+++ b/src/systemcmds/mtd/mtd.cpp
@@ -80,11 +80,11 @@ static int mtd_status(void)
 	bool running = false;
 	unsigned int num_instances;
 
-	const mtd_instance_s *instances = px4_mtd_get_instances(&num_instances);
+	mtd_instance_s **instances = px4_mtd_get_instances(&num_instances);
 
 	if (instances) {
 		for (unsigned int i = 0; i < num_instances; ++i) {
-			if (instances[i].mtd_dev) {
+			if (instances[i]->mtd_dev) {
 
 				unsigned long blocksize;
 				unsigned long erasesize;
@@ -93,7 +93,7 @@ static int mtd_status(void)
 				unsigned int  nblocks;
 				unsigned int  partsize;
 
-				ret = px4_mtd_get_geometry(&instances[i], &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize);
+				ret = px4_mtd_get_geometry(instances[i], &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize);
 
 				if (ret == 0) {
 
@@ -102,17 +102,17 @@ static int mtd_status(void)
 					printf("  blocksize:      %lu\n", blocksize);
 					printf("  erasesize:      %lu\n", erasesize);
 					printf("  neraseblocks:   %lu\n", neraseblocks);
-					printf("  No. partitions: %u\n", instances[i].n_partitions_current);
+					printf("  No. partitions: %u\n", instances[i]->n_partitions_current);
 
 
 					unsigned int  totalnblocks = 0;
 					unsigned int  totalpartsize = 0;
 
-					for (unsigned int p = 0; p < instances[i].n_partitions_current; p++) {
+					for (unsigned int p = 0; p < instances[i]->n_partitions_current; p++) {
 						FAR struct mtd_geometry_s geo;
-						ret = instances[i].part_dev[p]->ioctl(instances[i].part_dev[p], MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo));
+						ret = instances[i]->part_dev[p]->ioctl(instances[i]->part_dev[p], MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo));
 						printf("    partition: %u:\n", p);
-						printf("     name:   %s\n", instances[i].partition_names[p]);
+						printf("     name:   %s\n", instances[i]->partition_names[p]);
 						printf("     blocks: %" PRIu32 " (%lu bytes)\n", geo.neraseblocks, erasesize * geo.neraseblocks);
 						totalnblocks += geo.neraseblocks;
 						totalpartsize += erasesize * geo.neraseblocks;
@@ -323,7 +323,7 @@ int mtd_main(int argc, char *argv[])
 
 	if (myoptind < argc) {
 		unsigned int num_instances;
-		mtd_instance_s *instances = px4_mtd_get_instances(&num_instances);
+		mtd_instance_s **instances = px4_mtd_get_instances(&num_instances);
 
 		if (instances == nullptr) {
 			PX4_ERR("Driver not running");
@@ -338,11 +338,11 @@ int mtd_main(int argc, char *argv[])
 #if !defined(CONSTRAINED_FLASH)
 
 		if (!strcmp(argv[myoptind], "readtest")) {
-			return mtd_readtest(instances[instance]);
+			return mtd_readtest(*instances[instance]);
 		}
 
 		if (!strcmp(argv[myoptind], "rwtest")) {
-			return mtd_rwtest(instances[instance]);
+			return mtd_rwtest(*instances[instance]);
 		}
 
 #endif
@@ -352,7 +352,7 @@ int mtd_main(int argc, char *argv[])
 		}
 
 		if (!strcmp(argv[myoptind],  "erase")) {
-			return mtd_erase(instances[instance]);
+			return mtd_erase(*instances[instance]);
 		}
 	}
 
diff --git a/src/systemcmds/pairing_button_sim/CMakeLists.txt b/src/systemcmds/pairing_button_sim/CMakeLists.txt
new file mode 100644
index 0000000000..9b452994bc
--- /dev/null
+++ b/src/systemcmds/pairing_button_sim/CMakeLists.txt
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2020 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 systemcmds__pairing_button_sim
+	MAIN pairing_button_sim
+	COMPILE_FLAGS
+	SRCS
+		pairing_button_sim.cpp
+	DEPENDS
+	)
diff --git a/src/systemcmds/pairing_button_sim/Kconfig b/src/systemcmds/pairing_button_sim/Kconfig
new file mode 100644
index 0000000000..2d68e4e803
--- /dev/null
+++ b/src/systemcmds/pairing_button_sim/Kconfig
@@ -0,0 +1,5 @@
+menuconfig SYSTEMCMDS_PAIRING_BUTTON_SIM
+	bool "pairing_button_sim"
+	default n
+	---help---
+		Enable support for pairing_button_sim
diff --git a/src/systemcmds/pairing_button_sim/pairing_button_sim.cpp b/src/systemcmds/pairing_button_sim/pairing_button_sim.cpp
new file mode 100644
index 0000000000..4615bfe866
--- /dev/null
+++ b/src/systemcmds/pairing_button_sim/pairing_button_sim.cpp
@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2020 PX4 Development Team. All rights reserved.
+ *   Author: Daniele Pettenuzzo <daniele@auterion.com>
+ *
+ * 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 pairing_button_sim.cpp
+ *
+ * Tool to simulate pairing button
+ */
+
+#include <drivers/drv_hrt.h>
+#include <px4_platform_common/px4_config.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/vehicle_command.h>
+
+extern "C" __EXPORT int pairing_button_sim_main(int argc, char *argv[]);
+
+static void send_pairing_command();
+static void usage();
+
+void send_pairing_command()
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	vehicle_command_s vcmd{};
+	vcmd.command = vehicle_command_s::VEHICLE_CMD_START_RX_PAIR;
+	vcmd.timestamp = now;
+	vcmd.param1 = 10.f; // GCS pairing request handled by a companion.
+
+	uORB::Publication<vehicle_command_s> pairing_command{ORB_ID(vehicle_command)};
+	pairing_command.publish(vcmd);
+	PX4_INFO("Sending GCS pairing request");
+}
+
+static void usage()
+{
+	PX4_INFO("usage: pairing_button_sim {send}\n");
+}
+
+int pairing_button_sim_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		usage();
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "send")) {
+
+		send_pairing_command();
+	}
+
+	return 0;
+}
diff --git a/src/systemcmds/serial_test/serial_test.c b/src/systemcmds/serial_test/serial_test.c
index c5d4d14c9f..1faf45ad77 100644
--- a/src/systemcmds/serial_test/serial_test.c
+++ b/src/systemcmds/serial_test/serial_test.c
@@ -574,7 +574,6 @@ static void process_read_data(struct g_mod_t *g_mod, struct cli_args_t *g_cl)
 static void process_write_data(struct g_mod_t *g_mod, struct cli_args_t *g_cl)
 {
 	ssize_t count = 0;
-	int loops = 10;
 	int repeat = (g_cl->_tx_bytes == 0);
 
 	do {
@@ -594,14 +593,19 @@ static void process_write_data(struct g_mod_t *g_mod, struct cli_args_t *g_cl)
 
 			c = 0;
 
-		} else {
-			loops--;
 		}
 
 		count += c;
 
-		if (loops == 0 || c < g_mod->_write_size) {
-			g_mod->_write_count_value = g_mod->_write_data[c];
+		if (c <= g_mod->_write_size) {
+
+			if (c == 0) {
+				g_mod->_write_count_value = g_mod->_write_data[0];
+
+			} else {
+				g_mod->_write_count_value = next_count_value(g_mod->_write_data[c - 1], g_cl->_ascii_range);
+			}
+
 			repeat = 0;
 		}
 	} while (repeat);
diff --git a/src/systemcmds/system_time/system_time.cpp b/src/systemcmds/system_time/system_time.cpp
index 4d4b2d8feb..361e757861 100644
--- a/src/systemcmds/system_time/system_time.cpp
+++ b/src/systemcmds/system_time/system_time.cpp
@@ -34,6 +34,8 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/time.h>
 
+#include <stdlib.h>
+
 using namespace time_literals;
 
 static void	usage();
@@ -124,4 +126,4 @@ $ system_time get
 	PRINT_MODULE_USAGE_COMMAND_DESCR("set", "Set the system time, provide time in unix epoch time format");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("get", "Get the system time");
 
-}
\ No newline at end of file
+}
diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c
index 44fb747540..3df496dbe8 100644
--- a/src/systemcmds/tests/test_dataman.c
+++ b/src/systemcmds/tests/test_dataman.c
@@ -79,7 +79,7 @@ task_main(int argc, char *argv[])
 	}
 
 	/* try to read an invalid index */
-	if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINTS_MAX, buffer, sizeof(buffer)) >= 0) {
+	if (dm_read(DM_KEY_SAFE_POINTS, DM_KEY_SAFE_POINT_ITEMS_MAX, buffer, sizeof(buffer)) >= 0) {
 		PX4_ERR("%d read an invalid index failed", my_id);
 		goto fail;
 	}
diff --git a/src/systemcmds/ver/ver.cpp b/src/systemcmds/ver/ver.cpp
index 5028adff93..212bd9a285 100644
--- a/src/systemcmds/ver/ver.cpp
+++ b/src/systemcmds/ver/ver.cpp
@@ -141,11 +141,11 @@ extern "C" __EXPORT int ver_main(int argc, char *argv[])
 				int  r = px4_board_hw_revision();
 
 				if (v >= 0) {
-					snprintf(vb, sizeof(vb), "0x%08X", v);
+					snprintf(vb, sizeof(vb), "0x%0" STRINGIFY(HW_INFO_VER_DIGITS) "X", v);
 				}
 
 				if (r >= 0) {
-					snprintf(rb, sizeof(rb), "0x%08X", r);
+					snprintf(rb, sizeof(rb), "0x%0" STRINGIFY(HW_INFO_REV_DIGITS) "X", r);
 				}
 
 				printf("HW type: %s\n", strlen(px4_board_sub_type()) ? px4_board_sub_type() : "NA");
@@ -258,6 +258,22 @@ extern "C" __EXPORT int ver_main(int argc, char *argv[])
 				ret = 0;
 			}
 
+#ifdef PX4_RESTRICTED_BUILD
+
+			if (show_all) {
+				printf("Restricted build: on\n");
+			}
+
+#endif /* PX4_RESTRICTED_BUILD */
+
+#ifdef PX4_EXPORT_CONTROLLED_BUILD
+
+			if (show_all) {
+				printf("Export controlled build (flight time / wind restrictions): on\n");
+			}
+
+#endif /* PX4_EXPORT_CONTROLLED_BUILD */
+
 			if (ret == 1) {
 				PX4_ERR("unknown command");
 				return 1;
diff --git a/test/mavsdk_tests/CMakeLists.txt b/test/mavsdk_tests/CMakeLists.txt
index 01bdf58727..f1eca60043 100644
--- a/test/mavsdk_tests/CMakeLists.txt
+++ b/test/mavsdk_tests/CMakeLists.txt
@@ -17,6 +17,7 @@ if(MAVSDK_FOUND)
         test_main.cpp
         autopilot_tester.cpp
         autopilot_tester_failure.cpp
+        autopilot_tester_follow_me.cpp
         test_multicopter_control_allocation.cpp
         test_multicopter_failure_injection.cpp
         test_multicopter_failsafe.cpp
@@ -24,6 +25,7 @@ if(MAVSDK_FOUND)
         test_multicopter_offboard.cpp
         test_multicopter_manual.cpp
         test_vtol_mission.cpp
+        test_multicopter_follow_me.cpp
     )
 
     target_link_libraries(mavsdk_tests
diff --git a/test/mavsdk_tests/autopilot_tester.cpp b/test/mavsdk_tests/autopilot_tester.cpp
index bf82c0a4b2..ac2f5bf09c 100644
--- a/test/mavsdk_tests/autopilot_tester.cpp
+++ b/test/mavsdk_tests/autopilot_tester.cpp
@@ -37,6 +37,7 @@
 #include <future>
 #include <thread>
 #include <unistd.h>
+#include <cmath>
 
 std::string connection_url {"udp://"};
 std::optional<float> speed_factor {std::nullopt};
@@ -79,27 +80,24 @@ void AutopilotTester::connect(const std::string uri)
 
 void AutopilotTester::wait_until_ready()
 {
-	std::cout << time_str() << "Waiting for system to be ready" << std::endl;
+	std::cout << time_str() << "Waiting for system to be ready (system health ok & able to arm)" << std::endl;
+
+	// Wiat until the system is healthy
 	CHECK(poll_condition_with_timeout(
 	[this]() { return _telemetry->health_all_ok(); }, std::chrono::seconds(30)));
 
-	// FIXME: workaround to prevent race between PX4 switching to Hold mode
-	// and us trying to arm and take off. If PX4 is not in Hold mode yet,
-	// our arming presumably triggers a failsafe in manual mode.
-	std::this_thread::sleep_for(std::chrono::seconds(1));
-}
+	// Note: There is a known bug in MAVSDK (https://github.com/mavlink/MAVSDK/issues/1852),
+	// where `health_all_ok()` returning true doesn't actually mean vehicle is ready to accept
+	// global position estimate as valid (due to hysteresis). This needs to be fixed properly.
 
-void AutopilotTester::wait_until_ready_local_position_only()
-{
-	std::cout << time_str() << "Waiting for system to be ready" << std::endl;
+	// However, this is mitigated by the `is_armable` check below as a side effect, since
+	// when the vehicle considers global position to be valid, it will then allow arming
+	// if the COM_ARM_WO_GPS parameter is set to 0. For this case, switching to any mode
+	// relying on GPS after arming could be denied, and the test may fail.
+
+	// Wait until we can arm
 	CHECK(poll_condition_with_timeout(
-	[this]() {
-		return
-			(_telemetry->health().is_gyrometer_calibration_ok &&
-			 _telemetry->health().is_accelerometer_calibration_ok &&
-			 _telemetry->health().is_magnetometer_calibration_ok &&
-			 _telemetry->health().is_local_position_ok);
-	}, std::chrono::seconds(20)));
+	[this]() {	return _telemetry->health().is_armable;	}, std::chrono::seconds(20)));
 }
 
 void AutopilotTester::store_home()
@@ -534,6 +532,11 @@ void AutopilotTester::check_tracks_mission(float corridor_radius_m)
 	});
 }
 
+void AutopilotTester::check_current_altitude(float target_rel_altitude_m, float max_distance_m)
+{
+	CHECK(std::abs(_telemetry->position().relative_altitude_m - target_rel_altitude_m) <= max_distance_m);
+}
+
 std::array<float, 3> AutopilotTester::get_current_position_ned()
 {
 	mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned();
diff --git a/test/mavsdk_tests/autopilot_tester.h b/test/mavsdk_tests/autopilot_tester.h
index 941e09cbab..9d6f9e1ede 100644
--- a/test/mavsdk_tests/autopilot_tester.h
+++ b/test/mavsdk_tests/autopilot_tester.h
@@ -95,8 +95,12 @@ public:
 	~AutopilotTester();
 
 	void connect(const std::string uri);
+
+	/**
+	 * @brief Wait until vehicle's system status is healthy & is able to arm
+	 */
 	void wait_until_ready();
-	void wait_until_ready_local_position_only();
+
 	void store_home();
 	void check_home_within(float acceptance_radius_m);
 	void check_home_not_within(float min_distance_m);
@@ -133,6 +137,7 @@ public:
 	void request_ground_truth();
 	void check_mission_item_speed_above(int item_index, float min_speed_m_s);
 	void check_tracks_mission(float corridor_radius_m = 1.5f);
+	void check_current_altitude(float target_rel_altitude_m, float max_distance_m = 1.5f);
 	void start_checking_altitude(const float max_deviation_m);
 	void stop_checking_altitude();
 
@@ -147,7 +152,41 @@ public:
 protected:
 	mavsdk::Param *getParams() const { return _param.get();}
 	mavsdk::Telemetry *getTelemetry() const { return _telemetry.get();}
+	mavsdk::ManualControl *getManualControl() const { return _manual_control.get();}
 	std::shared_ptr<System> get_system() { return _mavsdk.systems().at(0);}
+	Telemetry::GroundTruth getHome()
+	{
+		// Check if home was stored before it is accessed
+		CHECK(_home.absolute_altitude_m != NAN);
+		CHECK(_home.latitude_deg != NAN);
+		CHECK(_home.longitude_deg != NAN);
+		return _home;
+	}
+
+	template<typename Rep, typename Period>
+	void sleep_for(std::chrono::duration<Rep, Period> duration)
+	{
+		const std::chrono::microseconds duration_us(duration);
+
+		if (_telemetry && _telemetry->attitude_quaternion().timestamp_us != 0) {
+
+			const int64_t start_time_us = _telemetry->attitude_quaternion().timestamp_us;
+
+			while (true) {
+				// Hopefully this is often enough not to have PX4 time out on us.
+				std::this_thread::sleep_for(std::chrono::milliseconds(1));
+
+				const int64_t elapsed_time_us = _telemetry->attitude_quaternion().timestamp_us - start_time_us;
+
+				if (elapsed_time_us > duration_us.count()) {
+					return;
+				}
+			}
+
+		} else {
+			std::this_thread::sleep_for(duration);
+		}
+	}
 
 private:
 	mavsdk::geometry::CoordinateTransformation get_coordinate_transformation();
@@ -169,6 +208,12 @@ private:
 
 	void report_speed_factor();
 
+	/**
+	 * @brief Continue polling until condition returns true or we have a timeout
+	 *
+	 * @param fun Boolean returning function. When true, the polling terminates.
+	 * @param duration Timeout for polling in `std::chrono::` time unit
+	 */
 	template<typename Rep, typename Period>
 	bool poll_condition_with_timeout(
 		std::function<bool()> fun, std::chrono::duration<Rep, Period> duration)
@@ -218,30 +263,7 @@ private:
 		return true;
 	}
 
-	template<typename Rep, typename Period>
-	void sleep_for(std::chrono::duration<Rep, Period> duration)
-	{
-		const std::chrono::microseconds duration_us(duration);
 
-		if (_telemetry && _telemetry->attitude_quaternion().timestamp_us != 0) {
-
-			const int64_t start_time_us = _telemetry->attitude_quaternion().timestamp_us;
-
-			while (true) {
-				// Hopefully this is often enough not to have PX4 time out on us.
-				std::this_thread::sleep_for(std::chrono::milliseconds(1));
-
-				const int64_t elapsed_time_us = _telemetry->attitude_quaternion().timestamp_us - start_time_us;
-
-				if (elapsed_time_us > duration_us.count()) {
-					return;
-				}
-			}
-
-		} else {
-			std::this_thread::sleep_for(duration);
-		}
-	}
 
 	mavsdk::Mavsdk _mavsdk{};
 	std::unique_ptr<mavsdk::Action> _action{};
diff --git a/test/mavsdk_tests/autopilot_tester_follow_me.cpp b/test/mavsdk_tests/autopilot_tester_follow_me.cpp
new file mode 100644
index 0000000000..98f40c872c
--- /dev/null
+++ b/test/mavsdk_tests/autopilot_tester_follow_me.cpp
@@ -0,0 +1,453 @@
+/****************************************************************************
+ *
+ *   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 "autopilot_tester_follow_me.h"
+
+// #include <mavsdk/plugins/follow_me/follow_me.h>
+
+#include "math_helpers.h"
+#include <iostream>
+#include <future>
+#include <thread>
+#include <unistd.h>
+#include <cmath>
+#include <random>
+
+
+FollowTargetSimulator::FollowTargetSimulator(std::array<float, 3> initial_position_ned,
+		mavsdk::Telemetry::GroundTruth home) :
+	_position_ned(initial_position_ned), _home(home)
+{
+	_velocity_ned[0] = 0.0f;
+	_velocity_ned[1] = 0.0f;
+	_velocity_ned[2] = 0.0f;
+}
+
+FollowTargetSimulator::~FollowTargetSimulator()
+{
+
+}
+
+void FollowTargetSimulator::update(float delta_t_s)
+{
+	const float velocity_m_s = 2.0;
+
+	_velocity_ned[0] = velocity_m_s;
+	_velocity_ned[1] = 0.0;
+
+	_position_ned[0] += _velocity_ned[0] * delta_t_s;
+	_position_ned[1] += _velocity_ned[1] * delta_t_s;
+	_position_ned[2] += _velocity_ned[2] * delta_t_s;
+
+	_udpate_count++;
+}
+
+std::array<double, 3> FollowTargetSimulator::get_position_global(bool add_noise)
+{
+	std::array<float, 3> pos_ned = _position_ned;
+
+	if (add_noise) {
+		unsigned seed = _udpate_count;
+		std::default_random_engine generator(seed);
+		std::normal_distribution<double> distribution(0.0, 1.0);
+		pos_ned[0] += distribution(generator);
+		pos_ned[1] += distribution(generator);
+		pos_ned[2] += distribution(generator);
+	}
+
+	CHECK(std::isfinite(_home.latitude_deg));
+	CHECK(std::isfinite(_home.longitude_deg));
+	const auto ct = CoordinateTransformation({_home.latitude_deg, _home.longitude_deg});
+
+	mavsdk::geometry::CoordinateTransformation::LocalCoordinate local_coordinate{pos_ned[0], pos_ned[1]};
+	mavsdk::geometry::CoordinateTransformation::GlobalCoordinate global_coordinate = ct.global_from_local(local_coordinate);
+	std::array<double, 3> global_pos{global_coordinate.latitude_deg, global_coordinate.longitude_deg, pos_ned[2] + _home.absolute_altitude_m};
+	return global_pos;
+}
+
+std::array<float, 3> FollowTargetSimulator::get_position_ned(bool add_noise)
+{
+
+	std::array<float, 3> pos_ned = _position_ned;
+
+	if (add_noise) {
+		unsigned seed = _udpate_count;
+		std::default_random_engine generator(seed);
+		std::normal_distribution<double> distribution(0.0, pos_noise_std);
+		pos_ned[0] += distribution(generator);
+		pos_ned[1] += distribution(generator);
+		pos_ned[2] += distribution(generator);
+	}
+
+	return pos_ned;
+}
+
+std::array<float, 3> FollowTargetSimulator::get_velocity_ned_noisy()
+{
+	return get_velocity_ned(true);
+}
+
+std::array<float, 3> FollowTargetSimulator::get_velocity_ned_ground_truth()
+{
+	return get_velocity_ned(false);
+}
+
+std::array<float, 3> FollowTargetSimulator::get_velocity_ned(bool add_noise)
+{
+	std::array<float, 3> vel_ned = _velocity_ned;
+
+	if (add_noise) {
+		unsigned seed = _udpate_count;
+		std::default_random_engine generator(seed);
+		std::normal_distribution<double> distribution(0.0, vel_noise_std);
+
+		vel_ned[0] += distribution(generator);
+		vel_ned[1] += distribution(generator);
+		vel_ned[2] += distribution(generator);
+	}
+
+	return vel_ned;
+}
+
+std::array<float, 3> FollowTargetSimulator::get_position_ned_noisy()
+{
+	return get_position_ned(true);
+}
+
+std::array<float, 3> FollowTargetSimulator::get_position_ground_truth_ned()
+{
+	return get_position_ned(false);
+}
+
+std::array<double, 3> FollowTargetSimulator::get_position_global_noisy()
+{
+	return get_position_global(true);
+}
+
+std::array<double, 3> FollowTargetSimulator::get_position_global_ground_truth()
+{
+	return get_position_global(false);
+}
+
+void FollowTargetSimulator::check_follow_angle(FollowMe::Config config, std::array<float, 3> drone_pos_ned,
+		std::array<float, 3> target_pos_ned, float tolerance)
+{
+	// This check assumes that the target is travelling straight on the x-axis
+	const float x_dist_to_target = target_pos_ned[0] - drone_pos_ned[0];
+	const float y_dist_to_target = target_pos_ned[1] - drone_pos_ned[1];
+
+	switch (config.follow_direction) {
+	case FollowMe::Config::FollowDirection::None:
+		CHECK(x_dist_to_target < tolerance);
+		CHECK(x_dist_to_target > -tolerance);
+		CHECK(y_dist_to_target < tolerance);
+		CHECK(y_dist_to_target > -tolerance);
+		break;
+
+	case FollowMe::Config::FollowDirection::Behind:
+		CHECK(drone_pos_ned[0] < target_pos_ned[0]);
+		CHECK(y_dist_to_target < tolerance);
+		CHECK(y_dist_to_target > -tolerance);
+		break;
+
+	case FollowMe::Config::FollowDirection::Front:
+		CHECK(drone_pos_ned[0] > target_pos_ned[0]);
+		CHECK(y_dist_to_target < tolerance);
+		CHECK(y_dist_to_target > -tolerance);
+		break;
+
+	case FollowMe::Config::FollowDirection::FrontRight:
+		CHECK(drone_pos_ned[0] > target_pos_ned[0]);
+		CHECK(drone_pos_ned[1] > target_pos_ned[1]);
+		break;
+
+	case FollowMe::Config::FollowDirection::FrontLeft:
+		CHECK(drone_pos_ned[0] > target_pos_ned[0]);
+		CHECK(drone_pos_ned[1] < target_pos_ned[1]);
+		break;
+
+	default:
+		break;
+	}
+}
+
+void AutopilotTesterFollowMe::connect(const std::string uri)
+{
+	AutopilotTester::connect(uri);
+
+	auto system = get_system();
+	_follow_me.reset(new FollowMe(system));
+}
+
+
+void AutopilotTesterFollowMe::straight_line_test(const float altitude_m, const bool stream_velocity)
+{
+	const unsigned location_update_rate = 1;
+	const float position_tolerance = 6.0f;
+
+	// Start with simulated target on the same plane as drone's home position
+	std::array<float, 3> start_location_ned = get_current_position_ned();
+	FollowTargetSimulator target_simulator(start_location_ned, getHome());
+
+	// Configure Min height of the drone to be "20 meters" above home & Follow direction as "Front
+	// right".
+	FollowMe::Config config;
+	config.min_height_m = altitude_m;
+	config.follow_distance_m = 8.0f;
+
+	// Allow some time for mode switch
+	sleep_for(std::chrono::milliseconds(1000));
+
+	// Start Follow Me
+	CHECK(FollowMe::Result::Success ==  _follow_me->start());
+
+	// Allow some time for mode switch
+	sleep_for(std::chrono::milliseconds(1000));
+
+	// task loop
+	bool target_moving = false;
+	bool perform_checks = false;
+
+	for (unsigned i = 0; i < 75 * location_update_rate; ++i) {
+		std::array<float, 3> target_pos_ned_ground_truth = target_simulator.get_position_ground_truth_ned();
+		std::array<float, 3> position_ned = get_current_position_ned();
+		const float distance_to_target = norm(diff(target_pos_ned_ground_truth, position_ned));
+
+		// poor-man's state machine
+		if (i < 5) {
+			// Stream target location without moving
+
+		} else if (i == 5) {
+			// Change config
+			perform_checks = false;
+			config.follow_direction = FollowMe::Config::FollowDirection::Behind;
+			CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
+
+		} else if (i > 5 && i < 15) {
+			// Move target and wait for steady state of drone
+			target_moving = true;
+
+		} else if (i >= 15 && i < 20) {
+			// Perform positional checks in steady state
+			perform_checks = true;
+
+		} else if (i == 20) {
+			// Change config
+			perform_checks = false;
+			config.follow_direction = FollowMe::Config::FollowDirection::Front;
+			CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
+
+		} else if (i > 20 && i < 30) {
+			// Move target and wait for steady state of drone
+
+		} else if (i >= 30 && i < 35) {
+			// Perform positional checks in steady state
+			perform_checks = true;
+
+		} else if (i == 35) {
+			// Change config
+			perform_checks = false;
+			config.follow_direction = FollowMe::Config::FollowDirection::FrontRight;
+			CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
+
+		} else if (i > 35 && i < 45) {
+			// Move target and wait for steady state of drone
+
+
+		} else if (i >= 45 && i < 55) {
+			// Perform positional checks in steady state
+			perform_checks = true;
+
+		} else if (i == 55) {
+			// Change config
+			perform_checks = false;
+			config.follow_direction = FollowMe::Config::FollowDirection::FrontLeft;
+			CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
+
+		} else if (i > 55 && i < 65) {
+			// Move target and wait for steady state of drone
+
+		} else if (i >= 65 && i < 75) {
+			// Perform positional checks in steady state
+			perform_checks = true;
+		}
+
+		if (target_moving) {
+			target_simulator.update(1.0f / location_update_rate);
+		}
+
+		if (perform_checks) {
+			check_current_altitude(altitude_m);
+			CHECK(distance_to_target <= config.follow_distance_m + position_tolerance);
+			CHECK(distance_to_target >= config.follow_distance_m - position_tolerance);
+			target_simulator.check_follow_angle(config, position_ned, target_pos_ned_ground_truth, position_tolerance);
+		}
+
+		// Construct follow-me message
+		std::array<double, 3> global_coordinate = target_simulator.get_position_global_noisy();
+
+		FollowMe::TargetLocation target_location{};
+		target_location.latitude_deg = global_coordinate[0];
+		target_location.longitude_deg = global_coordinate[1];
+		target_location.absolute_altitude_m = global_coordinate[2];
+
+		if (stream_velocity) {
+			std::array<float, 3> target_vel_ned = target_simulator.get_velocity_ned_noisy();
+			target_location.velocity_x_m_s = target_vel_ned[0];
+			target_location.velocity_y_m_s = target_vel_ned[1];
+			target_location.velocity_z_m_s = target_vel_ned[2];
+
+		} else {
+			target_location.velocity_x_m_s = NAN;
+			target_location.velocity_y_m_s = NAN;
+			target_location.velocity_z_m_s = NAN;
+		}
+
+
+		// Send message and check result
+		CHECK(FollowMe::Result::Success == _follow_me->set_target_location(target_location));
+
+		sleep_for(std::chrono::milliseconds(1000 / location_update_rate));
+	}
+
+	CHECK(FollowMe::Result::Success ==  _follow_me->stop());
+}
+
+void AutopilotTesterFollowMe::stream_velocity_only()
+{
+	const unsigned loop_update_rate = 1;
+	const float position_tolerance = 4.0f;
+
+	// Configure follow-me
+	FollowMe::Config config;
+	config.follow_direction = FollowMe::Config::FollowDirection::Behind;
+	CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
+
+	// Allow some time for mode switch
+	sleep_for(std::chrono::milliseconds(1000));
+
+	// Start Follow Me
+	CHECK(FollowMe::Result::Success ==  _follow_me->start());
+
+	// Allow some time for mode switch
+	sleep_for(std::chrono::milliseconds(1000));
+
+	std::array<float, 3> drone_initial_pos = get_current_position_ned();
+
+	// Start streaming velocity only. The drone should not move.
+	for (unsigned i = 0; i < 15 * loop_update_rate; i++) {
+		FollowMe::TargetLocation target_location{};
+		target_location.latitude_deg = NAN;
+		target_location.longitude_deg = NAN;
+		target_location.absolute_altitude_m = NAN;
+		target_location.velocity_x_m_s = 2.0f;
+		target_location.velocity_y_m_s = 1.0f;
+		target_location.velocity_z_m_s = 0.5f;
+
+		// Send message and check result
+		CHECK(FollowMe::Result::Success == _follow_me->set_target_location(target_location));
+
+		sleep_for(std::chrono::milliseconds(1000 / loop_update_rate));
+	}
+
+	// Check that drone is still close to initial position and has not moved much
+	std::array<float, 3> drone_final_pos = get_current_position_ned();
+	const float distance_travelled = norm(diff(drone_initial_pos, drone_final_pos));
+	CHECK(distance_travelled < position_tolerance);
+}
+
+void AutopilotTesterFollowMe::rc_override_test(const float altitude_m)
+{
+	const unsigned loop_update_rate = 50;
+	const float position_tolerance = 4.0f;
+
+	// Start with simulated target on the same plane as drone's home position
+	std::array<float, 3> start_location_ned = get_current_position_ned();
+	FollowTargetSimulator target_simulator(start_location_ned, getHome());
+
+	// Configure Min height of the drone to be "20 meters" above home & Follow direction as "Front
+	// right".
+	FollowMe::Config config;
+	config.min_height_m = altitude_m;
+	config.follow_distance_m = 8.0f;
+	config.follow_direction = FollowMe::Config::FollowDirection::Behind;
+	CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
+
+	// task loop
+	std::array<float, 3> drone_initial_pos;
+
+	for (unsigned i = 0; i <= 30 * loop_update_rate; ++i) {
+		// Start streaming target data after x seconds to provide RC before switching to the flight task
+		bool stream_follow_me_data = (i > 7 * loop_update_rate);
+
+		// Deflect a stick for a short time only
+		bool deflect_rc_sticks = (i > 10 * loop_update_rate && i <= 11 * loop_update_rate);
+
+		// Switch to follow-me at this instant
+		if (i == 5 * loop_update_rate) {
+			// Start Follow Me
+			CHECK(FollowMe::Result::Success ==  _follow_me->start());
+		}
+
+		// After approximately 10 seconds we would expect the drone to have stopped because of the RC stick input
+		if (i == 20 * loop_update_rate) {
+			drone_initial_pos = get_current_position_ned();
+		}
+
+		if (stream_follow_me_data) {
+			target_simulator.update(1.0f / loop_update_rate);
+			std::array<double, 3> global_coordinate = target_simulator.get_position_global_noisy();
+			FollowMe::TargetLocation target_location{};
+			target_location.latitude_deg = global_coordinate[0];
+			target_location.longitude_deg = global_coordinate[1];
+			target_location.absolute_altitude_m = global_coordinate[2];
+			target_location.velocity_x_m_s = NAN;
+			target_location.velocity_y_m_s = NAN;
+			target_location.velocity_z_m_s = NAN;
+			_follow_me->set_target_location(target_location);
+		}
+
+		if (deflect_rc_sticks) {
+			CHECK(getManualControl()->set_manual_control_input(1.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
+
+		} else {
+			CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
+		}
+
+		sleep_for(std::chrono::milliseconds(1000 / loop_update_rate));
+	}
+
+	std::array<float, 3> drone_final_pos = get_current_position_ned();
+	const float distance_travelled = norm(diff(drone_initial_pos, drone_final_pos));
+	CHECK(distance_travelled < position_tolerance);
+}
diff --git a/test/mavsdk_tests/autopilot_tester_follow_me.h b/test/mavsdk_tests/autopilot_tester_follow_me.h
new file mode 100644
index 0000000000..7a11e9770b
--- /dev/null
+++ b/test/mavsdk_tests/autopilot_tester_follow_me.h
@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   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 "autopilot_tester.h"
+
+#include <mavsdk/mavsdk.h>
+#include <mavsdk/plugins/action/action.h>
+#include <mavsdk/plugins/follow_me/follow_me.h>
+
+
+// Simulated a target moving on a straight line
+class FollowTargetSimulator
+{
+public:
+	FollowTargetSimulator(std::array<float, 3> initial_position_ned, mavsdk::Telemetry::GroundTruth home);
+	~FollowTargetSimulator();
+
+	// Integrate simulator by one time step
+	// This moves the target on a line
+	void update(float delta_t_s);
+
+	// Retrieve noisy version of position state in NED coordinate frame
+	// The noise is deterministic and changes whenever the update() function is called
+	std::array<float, 3> get_position_ned_noisy();
+
+	// Retrieve ground truth of position state in NED coordinate frame
+	std::array<float, 3> get_position_ground_truth_ned();
+
+	// Retrieve noisy version of velocity state in NED coordinate frame
+	// The noise is deterministic and changes whenever the update() function is called
+	std::array<float, 3> get_velocity_ned_noisy();
+
+	// Retrieve ground truth of velocity state in NED coordinate frame
+	std::array<float, 3> get_velocity_ned_ground_truth();
+
+	// Retrieve noisy version of position state in global coordinate frame (lat, lon, alt)
+	// The noise is deterministic and changes whenever the update() function is called
+	std::array<double, 3> get_position_global_noisy();
+
+	// Retrieve ground truth of position state in global coordinate frame (lat, lon, alt)
+	std::array<double, 3> get_position_global_ground_truth();
+
+	// Run checks whether the drone has the correct angle towards the target, specified by the follow-me configuration
+	void check_follow_angle(FollowMe::Config config, std::array<float, 3> drone_pos_ned,
+				std::array<float, 3> target_pos_ned, float tolerance);
+
+private:
+	// Retrieve estimate with the option to add deterministic gaussian noise
+	//
+	// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
+	std::array<double, 3> get_position_global(bool add_noise);
+
+	// Retrieve estimate with the option to add deterministic gaussian noise
+	//
+	// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
+	std::array<float, 3> get_position_ned(bool add_noise);
+
+	// Retrieve estimate with the option to add deterministic gaussian noise
+	//
+	// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
+	std::array<float, 3> get_velocity_ned(bool add_noise);
+
+	std::array<float, 3> _position_ned;
+	std::array<float, 3> _velocity_ned;
+	mavsdk::Telemetry::GroundTruth _home;
+	size_t _udpate_count = 0;
+
+	const double pos_noise_std = 0.3;
+	const double vel_noise_std = 0.1;
+
+	std::unique_ptr<mavsdk::Telemetry> _telemetry{};
+};
+
+class AutopilotTesterFollowMe : public AutopilotTester
+{
+public:
+	AutopilotTesterFollowMe() = default;
+	~AutopilotTesterFollowMe() = default;
+	void connect(const std::string uri);
+
+	void straight_line_test(const float altitude_m, const bool stream_velocity);
+
+	void stream_velocity_only();
+
+	void rc_override_test(const float altitude_m);
+
+private:
+	std::unique_ptr<mavsdk::FollowMe> _follow_me{};
+};
diff --git a/test/mavsdk_tests/test_multicopter_follow_me.cpp b/test/mavsdk_tests/test_multicopter_follow_me.cpp
new file mode 100644
index 0000000000..84c2d40fa7
--- /dev/null
+++ b/test/mavsdk_tests/test_multicopter_follow_me.cpp
@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   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 <chrono>
+
+#include "autopilot_tester_follow_me.h"
+
+
+
+TEST_CASE("Follow target - Position streaming", "[multicopter]")
+{
+	const bool stream_velocity = false;
+	AutopilotTesterFollowMe tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.store_home();
+	tester.arm();
+
+	const float takeoff_altitude = 10.0;
+	tester.set_takeoff_altitude(takeoff_altitude);
+	tester.takeoff();
+	tester.wait_until_hovering();
+
+	tester.straight_line_test(takeoff_altitude, stream_velocity);
+
+	tester.land();
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+}
+
+TEST_CASE("Follow target - Position and velocity streaming", "[multicopter]")
+{
+	const bool stream_velocity = true;
+	AutopilotTesterFollowMe tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.store_home();
+	tester.arm();
+
+	const float takeoff_altitude = 10.0;
+	tester.set_takeoff_altitude(takeoff_altitude);
+	tester.takeoff();
+	tester.wait_until_hovering();
+
+	tester.straight_line_test(takeoff_altitude, stream_velocity);
+
+	tester.land();
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+}
+
+TEST_CASE("Follow target - Velocity streaming only", "[multicopter]")
+{
+	// Streaming only velocity should not work, the drone should not move.
+	// There needs to be at least one position message for follow-me
+	// to be able to integrate velocity.
+	AutopilotTesterFollowMe tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.store_home();
+	tester.arm();
+
+	const float takeoff_altitude = 10.0;
+	tester.set_takeoff_altitude(takeoff_altitude);
+	tester.takeoff();
+	tester.wait_until_hovering();
+
+	tester.stream_velocity_only();
+
+	tester.land();
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+	tester.check_home_within(1.0f);
+}
+
+TEST_CASE("Follow target - Manual takeover", "[multicopter]")
+{
+	AutopilotTesterFollowMe tester;
+	tester.connect(connection_url);
+	tester.wait_until_ready();
+	tester.store_home();
+	tester.arm();
+
+	const float takeoff_altitude = 10.0;
+	tester.set_takeoff_altitude(takeoff_altitude);
+	tester.takeoff();
+	tester.wait_until_hovering();
+
+	tester.rc_override_test(takeoff_altitude);
+
+	tester.land();
+	std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
+	tester.wait_until_disarmed(until_disarmed_timeout);
+}
+
+TEST_CASE("Follow target - Spamming duplicate messages", "[multicopter]")
+{
+
+}
diff --git a/test/mavsdk_tests/test_vtol_mission.cpp b/test/mavsdk_tests/test_vtol_mission.cpp
index 5417254fce..85a022174c 100644
--- a/test/mavsdk_tests/test_vtol_mission.cpp
+++ b/test/mavsdk_tests/test_vtol_mission.cpp
@@ -39,7 +39,7 @@ TEST_CASE("Fly VTOL mission", "[vtol]")
 	AutopilotTester tester;
 	tester.connect(connection_url);
 	tester.wait_until_ready();
-	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission.plan");
+	tester.load_qgc_mission_raw_and_move_here("test/mavsdk_tests/vtol_mission_allmend.plan");
 	tester.arm();
 	tester.execute_mission_raw();
 	tester.wait_until_disarmed();
diff --git a/test/mavsdk_tests/vtol_mission_allmend.plan b/test/mavsdk_tests/vtol_mission_allmend.plan
new file mode 100644
index 0000000000..2aad670f43
--- /dev/null
+++ b/test/mavsdk_tests/vtol_mission_allmend.plan
@@ -0,0 +1,200 @@
+{
+    "fileType": "Plan",
+    "geoFence": {
+        "circles": [
+        ],
+        "polygons": [
+        ],
+        "version": 2
+    },
+    "groundStation": "QGroundControl",
+    "mission": {
+        "cruiseSpeed": 15,
+        "firmwareType": 12,
+        "globalPlanAltitudeMode": 1,
+        "hoverSpeed": 5,
+        "items": [
+            {
+                "AMSLAltAboveTerrain": 20,
+                "Altitude": 20,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 84,
+                "doJumpId": 1,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.35575503646861,
+                    8.52080660767436,
+                    20
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 20,
+                "Altitude": 20,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 2,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.35537212,
+                    8.52207767,
+                    20
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 3,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.35349688,
+                    8.52355825,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 4,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.3528936,
+                    8.52294671,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 5,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.35230484,
+                    8.52030742,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 6,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.35294448,
+                    8.51882684,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 30,
+                "Altitude": 30,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 16,
+                "doJumpId": 7,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    null,
+                    47.35375855,
+                    8.52002847,
+                    30
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "autoContinue": true,
+                "command": 189,
+                "doJumpId": 8,
+                "frame": 2,
+                "params": [
+                    0,
+                    0,
+                    0,
+                    0,
+                    0,
+                    0,
+                    0
+                ],
+                "type": "SimpleItem"
+            },
+            {
+                "AMSLAltAboveTerrain": 0,
+                "Altitude": 0,
+                "AltitudeMode": 1,
+                "autoContinue": true,
+                "command": 85,
+                "doJumpId": 9,
+                "frame": 3,
+                "params": [
+                    0,
+                    0,
+                    20,
+                    null,
+                    47.3557210047974,
+                    8.518762469830165,
+                    0
+                ],
+                "type": "SimpleItem"
+            }
+        ],
+        "plannedHomePosition": [
+            47.3557317,
+            8.5187574,
+            418
+        ],
+        "vehicleType": 20,
+        "version": 2
+    },
+    "rallyPoints": {
+        "points": [
+        ],
+        "version": 2
+    },
+    "version": 1
+}