Fix thrust scaling - working with tuning

Fix issues with nans
Switch origin to SITL position
2026-05-31 16:50:06 +08:00 · 2025-08-04 12:01:32 +09:00 · 2025-08-02 11:12:27 +02:00 · 2025-07-08 16:50:30 +09:00 · 2025-07-08 16:12:07 +09:00 · 2025-07-08 15:40:05 +09:00
2140 changed files with 44476 additions and 36376 deletions
@@ -0,0 +1,10 @@
+# Labeler configuration file for GitHub Actions
+# The action is ./.github/workflows/label.yml
+# Docs for the syntax in this file can be found at
+# https://github.com/actions/labeler
+
+
+# Add 'Documentation' label to any changes within 'docs' folder or any subfolders
+"Documentation+📑":
+- changed-files:
+  - any-glob-to-any-file: docs/**
@@ -18,7 +18,7 @@ on:
      - 'docs/**'
  pull_request:
    branches:
-      - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'

@@ -54,7 +54,13 @@ jobs:
        run: echo "::set-output name=timestamp::$(date +"%Y%m%d%H%M%S")"

      - id: set-branch
-        run: echo "::set-output name=branchname::${GITHUB_HEAD_REF:-${GITHUB_REF#refs/heads/}}"
+        run: |
+          echo "branchname=${{
+            github.event_name == 'pull_request' &&
+            format('pr-{0}', github.event.pull_request.number) ||
+            github.head_ref ||
+            github.ref_name
+          }}" >> $GITHUB_OUTPUT

      - name: Debug Matrix Output
        if: runner.debug == '1'
@@ -6,13 +6,15 @@ on:
    - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-    - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true

 jobs:
  build:
@@ -41,7 +43,7 @@ jobs:
      - name: Building [${{ matrix.check }}]
        uses: addnab/docker-run-action@v3
        with:
-          image: px4io/px4-dev-nuttx-focal:2022-08-12
+          image: px4io/px4-dev:v1.16.0-rc1-258-g0369abd556
          options: -v ${{ github.workspace }}:/workspace
          run: |
            cd /workspace
@@ -6,13 +6,11 @@ on:
    - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-    - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
 jobs:
  build:
    runs-on: ubuntu-latest
@@ -6,13 +6,15 @@ on:
    - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-    - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true

 jobs:
  build:
@@ -9,17 +9,19 @@ on:
      - 'release/**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-      - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'

 env:
  RUNS_IN_DOCKER: true

+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true
+
 jobs:
  build_and_test:
    name: Build and Test
@@ -11,7 +11,7 @@ on:
      - 'v*'
  pull_request:
    branches:
-      - '*'
+      - '**'
    paths:
      - '.github/workflows/dev_container.yml'
      - 'Tools/setup/ubuntu.sh'
@@ -30,6 +30,10 @@ on:
        type: boolean
        default: false

+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true
+
 jobs:
  setup:
    name: Set Tags and Variables
@@ -9,7 +9,7 @@ on:
      - 'docs/en/**'
  pull_request:
    branches:
-      - '*'
+      - '**'
    paths:
      - 'docs/en/**'

@@ -80,7 +80,7 @@ jobs:
      - name: Deploy
        run: |
          git clone --single-branch --branch main --depth 1 https://${{ secrets.PX4BUILTBOT_PERSONAL_ACCESS_TOKEN }}@github.com/PX4/docs.px4.io.git
-          # make it an orphan branch      
+          # make it an orphan branch
          cd docs.px4.io
          CURRENT_DATETIME=$(date +'%Y%m%d_%H_%M')
          git checkout --orphan "${CURRENT_DATETIME}_main"
@@ -3,10 +3,9 @@ name: EKF Change Indicator
 on:
  pull_request:
    branches:
-    - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'

 jobs:
  unit_tests:
@@ -4,7 +4,6 @@ on:
  push:
    paths-ignore:
      - 'docs/**'
-      - '.github/**'

 jobs:
  unit_tests:
@@ -6,13 +6,15 @@ on:
    - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-    - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true

 jobs:
  build:
@@ -27,7 +29,7 @@ jobs:
          "failsafe_web",
        ]
    container:
-      image: px4io/px4-dev:v1.16.0-ondemand
+      image: px4io/px4-dev:v1.16.0-rc1-258-g0369abd556
      options: --privileged --ulimit core=-1 --security-opt seccomp=unconfined
    steps:
      - name: Install Node v20.18.0
@@ -11,13 +11,11 @@ on:
      - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-      - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'

 env:
  MIN_FLASH_POS_DIFF_FOR_COMMENT: 50
@@ -28,7 +26,7 @@ jobs:
    name: Analyzing ${{ matrix.target }}
    runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
    container:
-      image: px4io/px4-dev:v1.16.0-ondemand
+      image: px4io/px4-dev:v1.16.0-rc1-258-g0369abd556
    strategy:
      matrix:
        target: [px4_fmu-v5x, px4_fmu-v6x]
@@ -9,20 +9,22 @@ on:
      - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-      - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true

 jobs:
  check_itcm:
    name: Checking ${{ matrix.target }}
    runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
    container:
-      image: px4io/px4-dev:v1.16.0-ondemand
+      image: px4io/px4-dev:v1.16.0-rc1-258-g0369abd556
    strategy:
      fail-fast: false
      matrix:
@@ -0,0 +1,21 @@
+# This workflow will triage pull requests and apply a label based on the
+# paths that are modified in the pull request.
+# The paths are set up in .github/labeler.yml
+#
+# See: https://github.com/actions/labeler
+
+name: Labeler
+on: [pull_request_target]
+
+jobs:
+  label:
+
+    runs-on: ubuntu-latest
+    permissions:
+      contents: read
+      pull-requests: write
+
+    steps:
+    - uses: actions/labeler@v5
+      with:
+        repo-token: "${{ secrets.GITHUB_TOKEN }}"
@@ -6,13 +6,15 @@ on:
    - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-    - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true

 jobs:
  build:
@@ -6,13 +6,15 @@ on:
    - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-    - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true

 jobs:
  build:
@@ -6,13 +6,15 @@ on:
    - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-    - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true

 jobs:
  build:
@@ -31,7 +33,7 @@ jobs:
    - name: Build PX4 and Run Test [${{ matrix.config }}]
      uses: addnab/docker-run-action@v3
      with:
-        image: px4io/px4-dev:v1.16.0-ondemand
+        image: px4io/px4-dev:v1.16.0-rc1-258-g0369abd556
        options: -v ${{ github.workspace }}:/workspace
        run: |
          cd /workspace
@@ -6,13 +6,11 @@ on:
    - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-    - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'

 jobs:
  build:
@@ -11,13 +11,15 @@ on:
    - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-    - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true

 jobs:
  build:
@@ -33,6 +35,12 @@ jobs:
    - name: Git Ownership Workaround
      run: git config --system --add safe.directory '*'

+    - name: Update ROS Keys
+      run: |
+        sudo rm /etc/apt/sources.list.d/ros2.list && \
+        sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \
+        echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
+
    - name: Install gazebo
      run: |
        apt update && apt install -y gazebo11 libgazebo11-dev gstreamer1.0-plugins-bad gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-ugly libgstreamer-plugins-base1.0-dev
@@ -5,16 +5,19 @@ on:
    - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-    - '*'
+      - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
 defaults:
  run:
    shell: bash
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true
+
 jobs:
  build_and_test:
    name: Build and test
@@ -11,13 +11,15 @@ on:
    - 'main'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
  pull_request:
    branches:
-    - '*'
+    - '**'
    paths-ignore:
      - 'docs/**'
-      - '.github/**'
+
+concurrency:
+  group: ${{ github.workflow }}-${{ github.ref }}
+  cancel-in-progress: true

 jobs:
  build:
@@ -6,6 +6,11 @@ CONFIG:
      buildType: RelWithDebInfo
      settings:
        CONFIG: px4_sitl_default
+    px4_sitl_spacecraft:
+      short: px4_sitl_spacecraft
+      buildType: RelWithDebInfo
+      settings:
+        CONFIG: px4_sitl_spacecraft
    px4_sitl_nolockstep:
      short: px4_sitl_nolockstep
      buildType: RelWithDebInfo
@@ -301,6 +306,11 @@ CONFIG:
      buildType: MinSizeRel
      settings:
        CONFIG: holybro_durandal-v1_default
+    holybro_kakuteh7-wing_default:
+      short: holybro_kakuteh7-wing
+      buildType: MinSizeRel
+      settings:
+        CONFIG: holybro_kakuteh7-wing_default
    holybro_kakuteh7dualimu_default:
      short: holybro_kakuteh7dualimu
      buildType: MinSizeRel
@@ -426,7 +426,6 @@ if(BUILD_TESTING)
 	set_target_properties(test_results PROPERTIES EXCLUDE_FROM_ALL TRUE)
 endif()

-
 #=============================================================================
 # subdirectories
 #
@@ -496,7 +495,6 @@ add_custom_target(uorb_graph DEPENDS ${uorb_graph_config})
 include(bloaty)


-include(doxygen)
 include(metadata)
 include(package)

@@ -7,33 +7,34 @@ See [the documentation on Maintainers](https://docs.px4.io/main/en/contribute/ma

 | Name                    | Sector | GitHub | Chat | email
 |-------------------------|--------|--------|------|----------------
-| Lorenz Meier | Founder | [LorenzMeier][LorenzMeier] |  | <lorenz@px4.io>
-| Daniel Agar | Architecture | [dagar][dagar] | daniel_agar | <daniel@agar.ca>
-| Beat Küng | Architecture | [bkueng][bkueng] | beatkueng | <beat-kueng@gmx.net>
-| Ramón Roche | CI / Testing | [mrpollo][mrpollo] | rroche | <rroche@linuxfoundation.org>
-| Mathieu Bresciani | State Estimation | [bresch][bresch] | mbresch |
-| Paul Riseborough | State Estimation | [priseborough][priseborough] |  |
-| David Sidrane | RTOS / NuttX | [davids5][davids5] | david_s5 | <David.Sidrane@Nscdg.com>
-| Jayoung Lim | Simulation | [Jaeyoung-Lim][Jaeyoung-Lim] | jaeyounglim. | <jalim@ethz.ch>
-| Beniamino Pozzan | ROS 2 | [beniaminopozzan][beniaminopozzan] | beniaminopozzan | <beniamino.pozzan@gmail.com>
-| Matthias Grob | Multirotor | [MaEtUgR][MaEtUgR] | maetugr |
-| Silvan Fuhrer | Fixed-Wing / VTOL | [sfuhrer][sfuhrer] | sfuhrer |
-| Christian Friedrich | Rover | [chfriedrich98][chfriedrich98] | christian982564 |
-| Pedro Roque | Spacecraft | [Pedro-Roque][Pedro-Roque] | .pedroroque | <padr@kth.se>
+| Lorenz Meier | Founder | [@LorenzMeier](https://github.com/LorenzMeier) |  | <lorenz@px4.io>
+| Daniel Agar | Architecture | [@dagar](https://github.com/dagar) | daniel_agar | <daniel@agar.ca>
+| Beat Küng | Architecture | [@bkueng](https://github.com/bkueng) | beatkueng | <beat-kueng@gmx.net>
+| Ramón Roche | CI / Testing | [@mrpollo](https://github.com/mrpollo) | rroche | <rroche@linuxfoundation.org>
+| Mathieu Bresciani | State Estimation | [@bresch](https://github.com/bresch) | mbresch |
+| Paul Riseborough | State Estimation | [@priseborough](https://github.com/priseborough) |  |
+| David Sidrane | RTOS / NuttX | [@davids5](https://github.com/davids5) | david_s5 | <David.Sidrane@Nscdg.com>
+| Jayoung Lim | Simulation | [@Jaeyoung-Lim](https://github.com/Jaeyoung-Lim) | jaeyounglim. | <jalim@ethz.ch>
+| Beniamino Pozzan | ROS 2 | [@beniaminopozzan](https://github.com/beniaminopozzan) | beniaminopozzan | <beniamino.pozzan@gmail.com>
+| Matthias Grob | Multirotor | [@MaEtUgR](https://github.com/MaEtUgR) | maetugr |
+| Silvan Fuhrer | Fixed-Wing / VTOL | [@sfuhrer](https://github.com/sfuhrer) | sfuhrer |
+| Christian Friedrich | Rover | [@chfriedrich98](https://github.com/chfriedrich98) | christian982564 |
+| Pedro Roque | Spacecraft | [@Pedro-Roque](https://github.com/Pedro-Roque) | .pedroroque | <padr@kth.se>
+| Jacob Dahl | Simulation | [@dakejahl](https://github.com/dakejahl) | dakejahl | <dahl.jakejacob@gmail.com>


 **Documentation Maintainers**

 | Name | GitHub | Chat | email
 |------|--------|------|----------------------
-| Hamish Willee  | [hamishwillee][hamishwillee] | hamishwillee |
+| Hamish Willee  | [@hamishwillee](https://github.com/hamishwillee) | hamishwillee |

 **Release Managers**

 | Name | GitHub | Chat | email
 |------|--------|------|----------------------
-| Ramón Roche | [mrpollo][mrpollo] | rroche | <rroche@linuxfoundation.org>
-| Daniel Agar | [dagar][dagar] | daniel_agar | <daniel@agar.ca>
+| Ramón Roche | [@mrpollo](https://github.com/mrpollo) | rroche | <rroche@linuxfoundation.org>
+| Daniel Agar | [@dagar](https://github.com/dagar) | daniel_agar | <daniel@agar.ca>

 **Retired Maintainers**

@@ -365,7 +365,7 @@ coverity_scan: px4_sitl_default

 # Documentation
 # --------------------------------------------------------------------
-.PHONY: parameters_metadata airframe_metadata module_documentation extract_events px4_metadata doxygen
+.PHONY: parameters_metadata airframe_metadata module_documentation extract_events px4_metadata

 parameters_metadata:
 	@$(MAKE) --no-print-directory px4_sitl_default metadata_parameters ver_gen
@@ -381,12 +381,6 @@ extract_events:

 px4_metadata: parameters_metadata airframe_metadata module_documentation extract_events

-doxygen:
-	@mkdir -p "$(SRC_DIR)"/build/doxygen
-	@cd "$(SRC_DIR)"/build/doxygen && cmake "$(SRC_DIR)" $(CMAKE_ARGS) -G"$(PX4_CMAKE_GENERATOR)" -DCONFIG=px4_sitl_default -DBUILD_DOXYGEN=ON
-	@$(PX4_MAKE) -C "$(SRC_DIR)"/build/doxygen
-	@touch "$(SRC_DIR)"/build/doxygen/Documentation/.nojekyll
-
 # Style
 # --------------------------------------------------------------------
 .PHONY: check_format format check_newlines
@@ -416,11 +410,18 @@ tests:
 	$(eval UBSAN_OPTIONS += color=always)
 	$(call cmake-build,px4_sitl_test)

+# work around lcov bug #316; remove once lcov is fixed (see https://github.com/linux-test-project/lcov/issues/316)
+LCOBUG = --ignore-errors mismatch
 tests_coverage:
 	@$(MAKE) clean
 	@$(MAKE) --no-print-directory tests PX4_CMAKE_BUILD_TYPE=Coverage
 	@mkdir -p coverage
-	@lcov --directory build/px4_sitl_test --base-directory build/px4_sitl_test --gcov-tool gcov --capture -o coverage/lcov.info
+	@lcov --directory build/px4_sitl_test \
+		--base-directory build/px4_sitl_test \
+		--gcov-tool gcov \
+		--capture \
+		$(LCOBUG) \
+		-o coverage/lcov.info


 rostest: px4_sitl_default
@@ -216,12 +216,33 @@ foreach(board_extra_file ${OPTIONAL_BOARD_EXTRAS})
 		if(CONFIG_SYSTEMCMDS_BL_UPDATE)
 			# generate rc.board_bootloader_upgrade
 			set(BOARD_FIRMWARE_BIN "${PX4_BOARD_VENDOR}_${PX4_BOARD_MODEL}_bootloader.bin")
-			configure_file(${PX4_SOURCE_DIR}/platforms/nuttx/init/rc.board_bootloader_upgrade.in ${romfs_gen_root_dir}/init.d/rc.board_bootloader_upgrade @ONLY)
+			message(STATUS "ROMFS:  Adding platforms/nuttx/init/rc.board_bootloader_upgrade -> /etc/init.d/rc.board_bootloader_upgrade")
+
+			# Generate the file using configure_file at configure time to a temporary location
+			set(bootloader_upgrade_tmp ${CMAKE_CURRENT_BINARY_DIR}/rc.board_bootloader_upgrade.tmp)
+			configure_file(${PX4_SOURCE_DIR}/platforms/nuttx/init/rc.board_bootloader_upgrade.in ${bootloader_upgrade_tmp} @ONLY)
+
+			# Then copy it at build time with proper dependencies
+			add_custom_command(
+				OUTPUT
+					${romfs_gen_root_dir}/init.d/rc.board_bootloader_upgrade
+					rc.board_bootloader_upgrade.stamp
+				COMMAND ${CMAKE_COMMAND} -E copy_if_different ${bootloader_upgrade_tmp} ${romfs_gen_root_dir}/init.d/rc.board_bootloader_upgrade
+				COMMAND ${CMAKE_COMMAND} -E touch rc.board_bootloader_upgrade.stamp
+				DEPENDS
+					${bootloader_upgrade_tmp}
+					${PX4_SOURCE_DIR}/platforms/nuttx/init/rc.board_bootloader_upgrade.in
+					romfs_copy.stamp
+				COMMENT "ROMFS: copying rc.board_bootloader_upgrade"
+			)
+
+			list(APPEND extras_dependencies
+				rc.board_bootloader_upgrade.stamp
+			)
 		else()
 			# remove bootloader from extras
 			list(REMOVE_ITEM OPTIONAL_BOARD_EXTRAS ${board_extra_file})
 		endif()
-
 	endif()
 endforeach()

@@ -46,3 +46,22 @@ param set-default PWM_MAIN_FUNC3 203
 param set-default PWM_MAIN_FUNC4 101

 param set-default EKF2_GPS_DELAY 0
+
+# Rate controllers
+param set-default FW_RR_P 0.0500
+param set-default FW_RR_I 2.0000
+param set-default FW_RR_D 0.0000
+param set-default FW_RR_FF 0.0000
+param set-default FW_RR_IMAX 1.0000
+
+param set-default FW_PR_P 0.0800
+param set-default FW_PR_I 2.5000
+param set-default FW_PR_D 0.0000
+param set-default FW_PR_FF 0.0000
+param set-default FW_PR_IMAX 1.0000
+
+param set-default FW_YR_P 0.0500
+param set-default FW_YR_I 3.0000
+param set-default FW_YR_D 0.0000
+param set-default FW_YR_FF 0.0000
+param set-default FW_YR_IMAX 1.0000
@@ -0,0 +1,47 @@
+#!/bin/sh
+#
+# @name HexarotorX SITL for SIH
+#
+# @type Hexarotor x
+# @class Copter
+#
+# @maintainer Matthias Grob <maetugr@gmail.com>
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=hex}
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+
+param set SIH_VEHICLE_TYPE 4
+
+# Symmetric hexacopter X clockwise motor numbering
+param set-default CA_ROTOR_COUNT 6
+param set-default CA_ROTOR0_PX 0.866
+param set-default CA_ROTOR0_PY 0.5
+param set-default CA_ROTOR1_PX 0
+param set-default CA_ROTOR1_PY 1
+param set-default CA_ROTOR1_KM -0.05
+param set-default CA_ROTOR2_PX -0.866
+param set-default CA_ROTOR2_PY 0.5
+param set-default CA_ROTOR3_PX -0.866
+param set-default CA_ROTOR3_PY -0.5
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_PX 0
+param set-default CA_ROTOR4_PY -1
+param set-default CA_ROTOR5_PX 0.866
+param set-default CA_ROTOR5_PY -0.5
+param set-default CA_ROTOR5_KM -0.05
+
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_MAIN_FUNC5 105
+param set-default PWM_MAIN_FUNC6 106
+
+param set-default EKF2_GPS_DELAY 0
@@ -15,7 +15,6 @@ param set-default NAV_ACC_RAD 0.5

 # Differential Parameters
 param set-default RD_WHEEL_TRACK 0.3
-param set-default RD_MAX_THR_YAW_R 1.5
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533

@@ -31,14 +30,16 @@ param set-default RO_YAW_RATE_P 0.25
 param set-default RO_YAW_RATE_LIM 180
 param set-default RO_YAW_ACCEL_LIM 120
 param set-default RO_YAW_DECEL_LIM 1000
+param set-default RO_YAW_RATE_CORR 1.43

 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 5

-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 2
 param set-default RO_SPEED_I 0.01
 param set-default RO_SPEED_P 0.1
+param set-default RO_SPEED_RED 1

 # Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1
@@ -1,51 +1,68 @@
 #!/bin/sh
-# @name Gazebo lawnmower
+# @name Gazebo - Zero Turn Lawnmower
 # @type Rover
 # @class Rover

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

 PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
-PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=lawn}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower}

 param set-default SIM_GZ_EN 1 # Gazebo bridge

+param set-default NAV_ACC_RAD 0.5
+
 # We can arm and drive in manual mode when it slides and GPS check fails:
 param set-default COM_ARM_WO_GPS 1

-# Rover parameters
+# Differential Parameters
 param set-default RD_WHEEL_TRACK 0.9
-param set-default RD_YAW_RATE_I 0.1
-param set-default RD_YAW_RATE_P 5
-param set-default RD_MAX_ACCEL 1
-param set-default RD_MAX_JERK 3
-param set-default RD_MAX_SPEED 8
-param set-default RD_YAW_P 5
-param set-default RD_YAW_I 0.1
-param set-default RD_MAX_YAW_RATE 30
 param set-default RD_TRANS_DRV_TRN 0.349066
 param set-default RD_TRANS_TRN_DRV 0.174533

+# Rover Control Parameters
+param set-default RO_ACCEL_LIM 1.5
+param set-default RO_DECEL_LIM 5
+param set-default RO_JERK_LIM 15
+param set-default RO_MAX_THR_SPEED 2.7
+
+# Rover Rate Control Parameters
+param set-default RO_YAW_RATE_I 0.2
+param set-default RO_YAW_RATE_P 1.0
+param set-default RO_YAW_RATE_LIM 60
+param set-default RO_YAW_ACCEL_LIM 50
+param set-default RO_YAW_DECEL_LIM 1000
+param set-default RO_YAW_RATE_CORR 1.0
+
+# Rover Attitude Control Parameters
+param set-default RO_YAW_P 5
+
+# Rover Velocity Control Parameters
+param set-default RO_SPEED_LIM 2.1
+param set-default RO_SPEED_I 0.01
+param set-default RO_SPEED_P 0.1
+param set-defatul RO_SPEED_RED 1.0
+
 # Pure pursuit parameters
-param set-default PP_LOOKAHD_MAX 30
-param set-default PP_LOOKAHD_MIN 2
 param set-default PP_LOOKAHD_GAIN 1
+param set-default PP_LOOKAHD_MAX 10
+param set-default PP_LOOKAHD_MIN 1

 # Actuator mapping - set SITL motors/servos output parameters:

 # "Motors" - motor channels 0 (Right) and 1 (Left) - via Wheels GZ bridge:
 param set-default SIM_GZ_WH_FUNC1 101 # right wheel
-#param set-default SIM_GZ_WH_MIN1 0
-#param set-default SIM_GZ_WH_MAX1 200
-#param set-default SIM_GZ_WH_DIS1 100
-#param set-default SIM_GZ_WH_FAIL1 100
+param set-default SIM_GZ_WH_MIN1 87
+param set-default SIM_GZ_WH_MAX1 113
+param set-default SIM_GZ_WH_DIS1 100
+param set-default SIM_GZ_WH_FAIL1 100

 param set-default SIM_GZ_WH_FUNC2 102 # left wheel
-#param set-default SIM_GZ_WH_MIN2 0
-#param set-default SIM_GZ_WH_MAX2 200
-#param set-default SIM_GZ_WH_DIS2 100
-#param set-default SIM_GZ_WH_FAIL2 100
+param set-default SIM_GZ_WH_MIN2 87
+param set-default SIM_GZ_WH_MAX2 113
+param set-default SIM_GZ_WH_DIS2 100
+param set-default SIM_GZ_WH_FAIL2 100

 param set-default SIM_GZ_WH_REV 0 # no need to reverse any wheels

@@ -30,14 +30,16 @@ param set-default RO_MAX_THR_SPEED 3.1
 param set-default RO_YAW_RATE_I 0.1
 param set-default RO_YAW_RATE_P 1
 param set-default RO_YAW_RATE_LIM 180
+param set-default RO_YAW_RATE_CORR 1

 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 3

-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 3
 param set-default RO_SPEED_I 0.1
 param set-default RO_SPEED_P 1
+param set-default RO_SPEED_RED 1

 # Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1
@@ -15,8 +15,6 @@ param set-default NAV_ACC_RAD 0.5

 # Mecanum Parameters
 param set-default RM_WHEEL_TRACK 0.3
-param set-default RM_MAX_THR_YAW_R 1.2
-param set-default RM_MISS_SPD_GAIN 1

 # Rover Control Parameters
 param set-default RO_ACCEL_LIM 3
@@ -30,14 +28,16 @@ param set-default RO_YAW_RATE_P 0.1
 param set-default RO_YAW_RATE_LIM 120
 param set-default RO_YAW_ACCEL_LIM 240
 param set-default RO_YAW_DECEL_LIM 1000
+param set-default RO_YAW_RATE_CORR 1.75

 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 5

-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 2
 param set-default RO_SPEED_I 0.5
 param set-default RO_SPEED_P 1
+param set-default RO_SPEED_RED 1

 # Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 0.5
@@ -0,0 +1,56 @@
+#!/bin/sh
+# @name Generic Differential Rover
+# @type Rover
+# @class Rover
+
+. ${R}etc/init.d/rc.rover_differential_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=rover}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover_differential}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+param set-default NAV_ACC_RAD 0.5
+
+# Differential Parameters
+param set-default RD_WHEEL_TRACK 0.6
+param set-default RD_TRANS_DRV_TRN 0.785398
+param set-default RD_TRANS_TRN_DRV 0.174533
+
+# Rate Control Parameters
+param set-default RO_YAW_RATE_I 0.01
+param set-default RO_YAW_RATE_P 0.25
+param set-default RO_YAW_RATE_LIM 250
+param set-default RO_YAW_ACCEL_LIM 400
+param set-default RO_YAW_DECEL_LIM 800
+param set-default RO_YAW_RATE_CORR 1
+
+# Attitude Control Parameters
+param set-default RO_YAW_P 5
+
+# Velocity Control Parameters
+param set-default RO_ACCEL_LIM 4
+param set-default RO_DECEL_LIM 6
+param set-default RO_JERK_LIM 10
+param set-default RO_MAX_THR_SPEED 2
+param set-default RO_SPEED_LIM 1.8
+param set-default RO_SPEED_I 0.01
+param set-default RO_SPEED_P 0.01
+param set-default RO_SPEED_RED 1
+
+# Pure Pursuit parameters
+param set-default PP_LOOKAHD_GAIN 1
+param set-default PP_LOOKAHD_MAX 10
+param set-default PP_LOOKAHD_MIN 1
+
+# Actuator mapping
+param set-default SIM_GZ_WH_FUNC1 101 # right wheel
+param set-default SIM_GZ_WH_MIN1 80
+param set-default SIM_GZ_WH_MAX1 120
+param set-default SIM_GZ_WH_DIS1 100
+
+param set-default SIM_GZ_WH_FUNC2 102 # left wheel
+param set-default SIM_GZ_WH_MIN2 80
+param set-default SIM_GZ_WH_MAX2 120
+param set-default SIM_GZ_WH_DIS2 100
@@ -1,155 +0,0 @@
-#!/bin/sh
-#
-# @name 6DoF Spacecraft Model
-#
-# @type Freeflyer with 8 thrusters
-#
-# @maintainer Pedro Roque <padr@kth.se>
-#
-
-. ${R}etc/init.d/rc.sc_defaults
-
-param set-default CA_AIRFRAME 15
-param set-default MAV_TYPE 99
-
-param set-default CA_THRUSTER_CNT 12
-param set-default CA_R_REV 0
-
-# param set-default FW_ARSP_MODE 1
-
-# Auto to be provided by Custom Airframe
-param set-default CA_METHOD 0
-
-# disable attitude failure detection
-param set-default FD_FAIL_P 0
-param set-default FD_FAIL_R 0
-
-# Set proper failsafes
-param set-default COM_ACT_FAIL_ACT 0
-param set-default COM_LOW_BAT_ACT 0
-param set-default NAV_DLL_ACT 0
-param set-default GF_ACTION 1
-param set-default NAV_RCL_ACT 1
-param set-default COM_POSCTL_NAVL 2
-
-# Set thrusters
-param set-default CA_THRUSTER0_PX -0.50
-param set-default CA_THRUSTER0_PY 0.50
-param set-default CA_THRUSTER0_PZ 0.0
-param set-default CA_THRUSTER0_CT 0.237
-param set-default CA_THRUSTER0_AX 0.0
-param set-default CA_THRUSTER0_AY -1.0
-param set-default CA_THRUSTER0_AZ 0.0
-
-param set-default CA_THRUSTER1_PX 0.50
-param set-default CA_THRUSTER1_PY 0.50
-param set-default CA_THRUSTER1_PZ 0.0
-param set-default CA_THRUSTER1_CT 0.237
-param set-default CA_THRUSTER1_AX 0.0
-param set-default CA_THRUSTER1_AY -1.0
-param set-default CA_THRUSTER1_AZ 0.0
-
-param set-default CA_THRUSTER2_PX 0.50
-param set-default CA_THRUSTER2_PY -0.50
-param set-default CA_THRUSTER2_PZ 0.0
-param set-default CA_THRUSTER2_CT 0.237
-param set-default CA_THRUSTER2_AX 0.0
-param set-default CA_THRUSTER2_AY 1.0
-param set-default CA_THRUSTER2_AZ 0.0
-
-param set-default CA_THRUSTER3_PX -0.50
-param set-default CA_THRUSTER3_PY -0.50
-param set-default CA_THRUSTER3_PZ 0.0
-param set-default CA_THRUSTER3_CT 0.237
-param set-default CA_THRUSTER3_AX 0.0
-param set-default CA_THRUSTER3_AY 1.0
-param set-default CA_THRUSTER3_AZ 0.0
-
-param set-default CA_THRUSTER4_PX -0.50
-param set-default CA_THRUSTER4_PY 0.0
-param set-default CA_THRUSTER4_PZ -0.50
-param set-default CA_THRUSTER4_CT 0.237
-param set-default CA_THRUSTER4_AX 1.0
-param set-default CA_THRUSTER4_AY 0.0
-param set-default CA_THRUSTER4_AZ 0.0
-
-param set-default CA_THRUSTER5_PX 0.50
-param set-default CA_THRUSTER5_PY 0.0
-param set-default CA_THRUSTER5_PZ -0.50
-param set-default CA_THRUSTER5_CT 0.237
-param set-default CA_THRUSTER5_AX -1.0
-param set-default CA_THRUSTER5_AY 0.0
-param set-default CA_THRUSTER5_AZ 0.0
-
-param set-default CA_THRUSTER6_PX 0.50
-param set-default CA_THRUSTER6_PY 0.0
-param set-default CA_THRUSTER6_PZ 0.50
-param set-default CA_THRUSTER6_CT 0.237
-param set-default CA_THRUSTER6_AX -1.0
-param set-default CA_THRUSTER6_AY 0.0
-param set-default CA_THRUSTER6_AZ 0.0
-
-param set-default CA_THRUSTER7_PX -0.50
-param set-default CA_THRUSTER7_PY 0.0
-param set-default CA_THRUSTER7_PZ 0.50
-param set-default CA_THRUSTER7_CT 0.237
-param set-default CA_THRUSTER7_AX 1.0
-param set-default CA_THRUSTER7_AY 0.0
-param set-default CA_THRUSTER7_AZ 0.0
-
-param set-default CA_THRUSTER8_PX 0.0
-param set-default CA_THRUSTER8_PY -0.50
-param set-default CA_THRUSTER8_PZ -0.50
-param set-default CA_THRUSTER8_CT 0.237
-param set-default CA_THRUSTER8_AX 0.0
-param set-default CA_THRUSTER8_AY 0.0
-param set-default CA_THRUSTER8_AZ 1.0
-
-param set-default CA_THRUSTER9_PX 0.0
-param set-default CA_THRUSTER9_PY 0.50
-param set-default CA_THRUSTER9_PZ -0.50
-param set-default CA_THRUSTER9_CT 0.237
-param set-default CA_THRUSTER9_AX 0.0
-param set-default CA_THRUSTER9_AY 0.0
-param set-default CA_THRUSTER9_AZ 1.0
-
-param set-default CA_THRUSTER10_PX 0.0
-param set-default CA_THRUSTER10_PY 0.50
-param set-default CA_THRUSTER10_PZ 0.50
-param set-default CA_THRUSTER10_CT 0.237
-param set-default CA_THRUSTER10_AX 0.0
-param set-default CA_THRUSTER10_AY 0.0
-param set-default CA_THRUSTER10_AZ -1.0
-
-param set-default CA_THRUSTER11_PX 0.0
-param set-default CA_THRUSTER11_PY -0.50
-param set-default CA_THRUSTER11_PZ 0.50
-param set-default CA_THRUSTER11_CT 0.237
-param set-default CA_THRUSTER11_AX 0.0
-param set-default CA_THRUSTER11_AY 0.0
-param set-default CA_THRUSTER11_AZ -1.0
-
-param set-default PWM_MAIN_FUNC1 101
-param set-default PWM_MAIN_FUNC2 102
-param set-default PWM_MAIN_FUNC3 103
-param set-default PWM_MAIN_FUNC4 104
-param set-default PWM_MAIN_FUNC5 105
-param set-default PWM_MAIN_FUNC6 106
-param set-default PWM_MAIN_FUNC7 107
-param set-default PWM_MAIN_FUNC8 108
-param set-default PWM_MAIN_FUNC9 109
-param set-default PWM_MAIN_FUNC10 110
-param set-default PWM_MAIN_FUNC11 111
-param set-default PWM_MAIN_FUNC12 112
-
-# PWM Simulation
-param set PWM_SIM_PWM_MAX 10000
-param set PWM_SIM_PWM_MIN 0
-
-# Controller Tunings
-param set-default SC_ROLLRATE_P 0.14
-param set-default SC_PITCHRATE_P 0.14
-param set-default SC_ROLLRATE_I 0.3
-param set-default SC_PITCHRATE_I 0.3
-param set-default SC_ROLLRATE_D 0.004
-param set-default SC_PITCHRATE_D 0.004
@@ -20,7 +20,7 @@ param set-default COM_ARM_CHK_ESCS 0  # We don't have ESCs
 param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later?

 param set-default CA_AIRFRAME 14
-param set-default MAV_TYPE 99
+param set-default MAV_TYPE 45

 param set-default CA_THRUSTER_CNT 8
 param set-default CA_R_REV 0
@@ -109,11 +109,13 @@ px4_add_romfs_files(
 	10041_sihsim_airplane
 	10042_sihsim_xvert
 	10043_sihsim_standard_vtol
+	10044_sihsim_hex

 	17001_flightgear_tf-g1
 	17002_flightgear_tf-g2

-	71001_gazebo-classic_spacecraft_dart
+	50000_gz_rover_differential
+
 	71002_gz_spacecraft_2d

 	# [22000, 22999] Reserve for custom models
@@ -110,25 +110,36 @@ if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
 	MODEL_NAME="${PX4_SIM_MODEL#*gz_}"
 	MODEL_NAME_INSTANCE="${MODEL_NAME}_${px4_instance}"

-	POSE_ARG=""
-	if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
-		pos_x=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $1}')
-		pos_y=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $2}')
-		pos_z=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $3}')
-		pos_x=${pos_x:-0}
-		pos_y=${pos_y:-0}
-		pos_z=${pos_z:-0}
+	sdf_pose_str=""

-		POSE_ARG=", pose: { position: { x: ${pos_x}, y: ${pos_y}, z: ${pos_z} } }"
-		echo "INFO  [init] Spawning model at position: ${pos_x} ${pos_y} ${pos_z}"
+	if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
+		pose_x=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $1}')
+		pose_y=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $2}')
+		pose_z=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $3}')
+		pose_roll=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $4}')
+		pose_pitch=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $5}')
+		pose_yaw=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $6}')
+
+		pose_x=${pose_x:-0}
+		pose_y=${pose_y:-0}
+		pose_z=${pose_z:-0}
+		pose_roll=${pose_roll:-0}
+		pose_pitch=${pose_pitch:-0}
+		pose_yaw=${pose_yaw:-0}
+
+		sdf_pose_str="<pose> ${pose_x} ${pose_y} ${pose_z} ${pose_roll} ${pose_pitch} ${pose_yaw} </pose>"
+		echo "INFO  [init] Gazebo model pose: ${pose_x} ${pose_y} ${pose_z} ${pose_roll} ${pose_pitch} ${pose_yaw}"
 	fi

-	echo "INFO  [init] Spawning model"
+	echo "INFO  [init] Spawning Gazebo model"
+
+	# include the actual SDF in this one, containing the pose if given
+	sdf_str="<sdf version=\"1.6\"> <include> <uri>file://${PX4_GZ_MODELS}/${MODEL_NAME}/model.sdf</uri> ${sdf_pose_str} </include> </sdf>"

 	# Spawn model
 	${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \
 			--reptype gz.msgs.Boolean --timeout 5000 \
-			--req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}/model.sdf\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1
+			--req "name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false, sdf: '${sdf_str}'" > /dev/null 2>&1

 	# Wait for model to spawn
 	sleep 1
@@ -295,10 +295,15 @@ then
 	# for multi intances setup, add namespace prefix
 	uxrce_dds_ns="-n px4_$px4_instance"
 fi
-if [ -n "$PX4_UXRCE_DDS_NS" ]
-then
-	# Override namespace if environment variable is defined
-	uxrce_dds_ns="-n $PX4_UXRCE_DDS_NS"
+if [ "${PX4_UXRCE_DDS_NS+x}" ]; then
+	# Override, as variable is set (empty or not)
+	if [ -n "$PX4_UXRCE_DDS_NS" ]; then
+		# Override namespace if environment variable is non-empty
+		uxrce_dds_ns="-n $PX4_UXRCE_DDS_NS"
+	else
+		# Clear namespace if variable is empty
+		uxrce_dds_ns=""
+	fi
 fi
 if [ -n "$ROS_DOMAIN_ID" ]
 then
@@ -5,8 +5,5 @@
 # @type Rover
 # @class Rover
 #
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#

 . ${R}etc/init.d/rc.rover_differential_defaults
@@ -7,9 +7,6 @@
 # @type Rover
 # @class Rover
 #
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#

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

@@ -26,7 +23,6 @@ param set-default NAV_ACC_RAD 0.5

 # Differential Parameters
 param set-default RD_WHEEL_TRACK 0.3
-param set-default RD_MAX_THR_YAW_R 0.7
 param set-default RD_TRANS_DRV_TRN 0.785398
 param set-default RD_TRANS_TRN_DRV 0.139626

@@ -42,14 +38,16 @@ param set-default RO_YAW_RATE_P 0.1
 param set-default RO_YAW_RATE_LIM 250
 param set-default RO_YAW_ACCEL_LIM 600
 param set-default RO_YAW_DECEL_LIM 600
+param set-default RO_YAW_RATE_CORR 2.7

 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 2.5

-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 1.6
 param set-default RO_SPEED_I 0.01
 param set-default RO_SPEED_P 0.1
+param set-default RO_SPEED_RED 1

 # Pure Pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1
@@ -5,8 +5,5 @@
 # @type Rover
 # @class Rover
 #
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#

 . ${R}etc/init.d/rc.rover_ackermann_defaults
@@ -7,9 +7,6 @@
 # @type Rover
 # @class Rover
 #
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#

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

@@ -33,14 +30,16 @@ param set-default RO_MAX_THR_SPEED 2.8
 param set-default RO_YAW_RATE_I 0.1
 param set-default RO_YAW_RATE_P 0.1
 param set-default RO_YAW_RATE_LIM 120
+param set-default RO_YAW_RATE_CORR 1

 # Rover Attitude Control Parameters
 param set-default RO_YAW_P 2.5

-# Rover Position Control Parameters
+# Rover Velocity Control Parameters
 param set-default RO_SPEED_LIM 2.5
 param set-default RO_SPEED_I 0.01
 param set-default RO_SPEED_P 0.1
+param set-default RO_SPEED_RED 1

 # Pure pursuit parameters
 param set-default PP_LOOKAHD_GAIN 1
@@ -5,8 +5,5 @@
 # @type Rover
 # @class Rover
 #
-# @board px4_fmu-v2 exclude
-# @board bitcraze_crazyflie exclude
-#

 . ${R}etc/init.d/rc.rover_mecanum_defaults
@@ -11,7 +11,7 @@
 . ${R}etc/init.d/rc.sc_defaults

 param set-default CA_AIRFRAME 14
-param set-default MAV_TYPE 99
+param set-default MAV_TYPE 45

 param set-default CA_THRUSTER_CNT 8
 param set-default CA_R_REV 0
@@ -13,9 +13,11 @@ control_allocator start
 #
 # Start attitude controller.
 #
+fw_dyn_soar_control start
 fw_rate_control start
 fw_att_control start
-fw_pos_control start
+fw_mode_manager start
+fw_lat_lon_control start
 airspeed_selector start

 #
@@ -13,8 +13,6 @@ param set-default MAV_TYPE 1
 #
 # Default parameters for fixed wing UAVs.
 #
-# there is a 2.5 factor applied on the _FS thresholds if for invalidation
-param set-default COM_POS_FS_EPH 50
 param set-default COM_VEL_FS_EVH 3

 param set-default COM_POS_LOW_EPH 50
@@ -20,7 +20,5 @@ param set-default NAV_ACC_RAD 2
 param set-default RTL_RETURN_ALT 30
 param set-default RTL_DESCEND_ALT 10

-param set-default GPS_UBX_DYNMODEL 6
-
 # lower RNG_FOG since MC are expected to fly closer over obstacles
 param set-default EKF2_RNG_FOG 1.0
@@ -3,10 +3,10 @@
 # NOTE: Script variables are declared/initialized/unset in the rcS script.
 #

-set VEHICLE_TYPE sc
+set VEHICLE_TYPE spacecraft

-# MAV_TYPE_QUADROTOR 2
-#param set-default MAV_TYPE 12
+# MAV_TYPE_SPACECRAFT_ORBITTER
+param set-default MAV_TYPE 45

 # Set micro-dds-client to use ethernet and IP-address 192.168.0.1
 param set-default UXRCE_DDS_AG_IP -1062731775
@@ -234,6 +234,7 @@ then
 		lis3mdl -X -q -a 0x1c start
 	fi
 	qmc5883l -X -q start
+	qmc5883p -X -q start
 	rm3100 -X -q start

 	# start last (wait for possible icm20948 passthrough mode)
@@ -68,6 +68,15 @@ then
 	. ${R}etc/init.d/rc.vtol_apps
 fi

+#
+# Spapcecraft setup.
+#
+if [ $VEHICLE_TYPE = spacecraft ]
+then
+	# Start standard multicopter apps.
+	. ${R}etc/init.d/rc.sc_apps
+fi
+
 #
 # Airship setup.
 #
@@ -27,7 +27,8 @@ fi

 fw_rate_control start vtol
 fw_att_control start vtol
-fw_pos_control start vtol
+fw_mode_manager start
+fw_lat_lon_control start vtol
 fw_autotune_attitude_control start vtol

 # Start Land Detector
@@ -10,9 +10,6 @@ set VEHICLE_TYPE vtol
 # MAV_TYPE_VTOL_FIXEDROTOR 22
 param set-default MAV_TYPE 22

-# there is a 2.5 factor applied on COM_POS_FS_EPH if for invalidation
-param set-default COM_POS_FS_EPH 50
-
 param set-default COM_POS_LOW_EPH 50

 param set-default MIS_TAKEOFF_ALT 20
@@ -39,7 +39,7 @@ set VEHICLE_TYPE none
 # Airframe parameter versioning
 # Value set to 1 by default but can optionally be overridden in the airframe configuration startup script.
 # Airframe maintainers can ensure a reset to the airframe defaults during an update by increasing by one.
-# e.g. add line "set PARAM_DEFAULTS_VER 2" in your airframe file to build the first update that enfoces a reset.
+# e.g. add line "set PARAM_DEFAULTS_VER 2" in your airframe file to build the first update that enforces a reset.
 set PARAM_DEFAULTS_VER 1

 #
@@ -224,7 +224,7 @@ else
 		# Look for airframe in ROMFS
 		. ${R}etc/init.d/rc.autostart

-		if [ ${VEHICLE_TYPE} == none ]
+		if [ ${VEHICLE_TYPE} = none ]
 		then
 			# Use external startup file
 			if [ $STORAGE_AVAILABLE = yes ]
@@ -235,7 +235,7 @@ else
 			fi
 		fi

-		if [ ${VEHICLE_TYPE} == none ]
+		if [ ${VEHICLE_TYPE} = none ]
 		then
 			echo "ERROR [init] No airframe file found for SYS_AUTOSTART value"
 			param set SYS_AUTOSTART 0
@@ -280,6 +280,14 @@ else
 	#
 	send_event start

+	#
+	# Start the hardfault streamer.
+	#
+	if param compare -s SYS_HF_MAV 1
+	then
+		hardfault_stream start
+	fi
+
 	#
 	# Start the resource load monitor.
 	#
@@ -3,14 +3,14 @@
 return_value=0

 # Check if there are files checked in that don't end in a newline (POSIX requirement)
-git grep --cached -Il '' -- ':!docs/*' | xargs -L1 bash -c 'if test "$(tail -c 1 "$0")"; then echo "No new line at end of $0"; exit 1; fi'
+git grep --cached -Il '' -- ':!docs/public/' ':!docs/ko/' ':!docs/uk/' ':!docs/zh/' | xargs -L1 bash -c 'if test "$(tail -c 1 "$0")"; then echo "No new line at end of $0"; exit 1; fi'

 if [ $? -ne 0 ]; then
 	return_value=1
 fi

 # Check if there are files checked in that have duplicate newlines at the end (fail trailing whitespace checks)
-git grep --cached -Il '' -- ':!docs/*' | xargs -L1 bash -c 'if tail -c 2 "$0" | ( read x && read y && [ x"$x" = x ] && [ x"$y" = x ]); then echo "Multiple newlines at the end of $0"; exit 1; fi'
+git grep --cached -Il '' -- ':!docs/public/' ':!docs/ko/' ':!docs/uk/' ':!docs/zh/' | xargs -L1 bash -c 'if tail -c 2 "$0" | ( read x && read y && [ x"$x" = x ] && [ x"$y" = x ]); then echo "Multiple newlines at the end of $0"; exit 1; fi'

 if [ $? -ne 0 ]; then
 	return_value=1
@@ -27,9 +27,9 @@ do
    # - An old .msg version exists
    # - A translation header exists and is included

-    # Ignore changes to comments or constants
-    content_a=$(git show "${BASE_COMMIT}:${file}" | grep -o '^[^#]*' | grep -v =)
-    content_b=$(git show "${HEAD_COMMIT}:${file}" | grep -o '^[^#]*' | grep -v =)
+    # Ignore changes to comments or constants and trim whitespace
+    content_a=$(git show "${BASE_COMMIT}:${file}" | grep -o '^[^#]*' | grep -v = | sed 's/^ *//;s/[ \t]*$//')
+    content_b=$(git show "${HEAD_COMMIT}:${file}" | grep -o '^[^#]*' | grep -v = | sed 's/^ *//;s/[ \t]*$//')
    if [ "${content_a}" == "${content_b}" ]; then
      echo "No version update required for ${file}"
      continue
@@ -35,6 +35,7 @@ if args.filter:
    for board in args.filter.split(','):
        board_filter.append(board)

+default_container = 'ghcr.io/px4/px4-dev:v1.16.0-rc1-258-g0369abd556'
 build_configs = []
 grouped_targets = {}
 excluded_boards = ['modalai_voxl2', 'px4_ros2']  # TODO: fix and enable
@@ -86,7 +87,7 @@ def process_target(px4board_file, target_name):
    assert platform, f"PLATFORM not found in {px4board_file}"

    if platform not in excluded_platforms:
-        container = 'ghcr.io/px4/px4-dev:v1.16.0-ondemand'
+        container = default_container
        if platform == 'posix':
            group = 'base'
            if toolchain:
@@ -120,7 +121,7 @@ if(verbose):
 # - Events
 metadata_targets = ['airframe_metadata', 'parameters_metadata', 'extract_events']
 grouped_targets['base'] = {}
-grouped_targets['base']['container'] = 'ghcr.io/px4/px4-dev:v1.16.0-ondemand'
+grouped_targets['base']['container'] = default_container
 grouped_targets['base']['manufacturers'] = {}
 grouped_targets['base']['manufacturers']['px4'] = []
 grouped_targets['base']['manufacturers']['px4'] += metadata_targets
@@ -2,19 +2,7 @@

 if [ -z ${PX4_DOCKER_REPO+x} ]; then
 	echo "guessing PX4_DOCKER_REPO based on input";
-	if [[ $@ =~ .*px4_fmu.* ]]; then
-		# nuttx-px4fmu-v{1,2,3,4,5}
-		PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2022-08-12"
-	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*beaglebone.* ]] || [[ $@ =~ .*pilotpi.default ]] || [[ $@ =~ .*navigator.* ]]; then
-		# beaglebone_blue_default, emlid_navio2_default, px4_raspberrypi_default, scumaker_pilotpi_default, bluerobotics_navigator_default
-		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2023-06-26"
-	elif [[ $@ =~ .*pilotpi.arm64 ]]; then
-		# scumaker_pilotpi_arm64
-		PX4_DOCKER_REPO="px4io/px4-dev-aarch64:2022-08-12"
-	elif [[ $@ =~ .*navio2.* ]] || [[ $@ =~ .*raspberry.* ]] || [[ $@ =~ .*bebop.* ]]; then
-		# posix_rpi_cross, posix_bebop_default
-		PX4_DOCKER_REPO="px4io/px4-dev-armhf:2023-06-26"
-	elif [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then
+	if [[ $@ =~ .*clang.* ]] || [[ $@ =~ .*scan-build.* ]]; then
 		# clang tools
 		PX4_DOCKER_REPO="px4io/px4-dev-clang:2021-02-04"
 	elif [[ $@ =~ .*tests* ]]; then
@@ -27,17 +15,9 @@ fi

 # otherwise default to nuttx
 if [ -z ${PX4_DOCKER_REPO+x} ]; then
-	PX4_DOCKER_REPO="px4io/px4-dev-nuttx-focal:2022-08-12"
+	PX4_DOCKER_REPO="px4io/px4-dev:v1.16.0-rc1-258-g0369abd556"
 fi

-# docker hygiene
-
-#Delete all stopped containers (including data-only containers)
-# docker container prune
-
-#Delete all 'untagged/dangling' (<none>) images
-# docker image prune
-
 echo "PX4_DOCKER_REPO: $PX4_DOCKER_REPO";

 PWD=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
@@ -2,6 +2,7 @@

 """
 Generate docs from .msg files
+Also generates docs/en/middleware/dds_topics.md from dds_topics.yaml
 """

 import os
@@ -9,6 +10,99 @@ import argparse
 import sys


+import yaml
+
+def generate_dds_yaml_doc(allMessageFiles, output_file = 'dds_topics.md'):
+    """
+    Generates human readable version of dds_topics.yaml.
+    Default output is to docs/en/middleware/dds_topics.md
+    """
+
+    dds_file_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),"../../src/modules/uxrce_dds_client/dds_topics.yaml")
+    output_file_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),f"../../docs/en/middleware/{output_file}")
+
+    try:
+        with open(dds_file_path, 'r') as file:
+            data = yaml.safe_load(file)
+
+        # Get messages and topics that are not published by default
+        # Start by getting all that are published.
+        all_messages_in_source = set()
+        all_message_types =set()
+        all_topics =set()
+        for message in data["publications"]:
+            all_message_types.add(message['type'].split("::")[-1])
+            all_topics.add(message['topic'].split('/')[-1])
+        for message in data["subscriptions"]:
+            all_message_types.add(message['type'].split("::")[-1])
+            all_topics.add(message['topic'].split('/')[-1])
+        if data["subscriptions_multi"]: # There is none now
+            dds_markdown += "None\n"
+            for message in data["subscriptions_multi"]:
+                all_message_types.add(message['type'].split("::")[-1])
+                all_topics.add(message['topic'].split('/')[-1])
+        for message in allMessageFiles:
+            all_messages_in_source.add(message.split('/')[-1].split('.')[0])
+        messagesNotExported = all_messages_in_source - all_message_types
+
+        # write out the dds file
+        dds_markdown="""# dds_topics.yaml — PX4 Topics Exposed to ROS 2
+
+::: info
+This document is [auto-generated](https://github.com/PX4/PX4-Autopilot/blob/main/Tools/msg/generate_msg_docs.py) from the source code.
+:::
+
+
+The [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml) file specifies which uORB message definitions are compiled into the [uxrce_dds_client](../modules/modules_system.md#uxrce-dds-client) module when [PX4 is built](../middleware/uxrce_dds.md#code-generation), and hence which topics are available for ROS 2 applications to subscribe or publish (by default).
+
+This document shows a markdown-rendered version of [dds_topics.yaml](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/uxrce_dds_client/dds_topics.yaml), listing the publications, subscriptions, and so on.
+
+## Publications
+
+Topic | Type| Rate Limit
+--- | --- | ---
+"""
+
+        for message in data["publications"]:
+            type = message['type']
+            px4Type=type.split("::")[-1]
+            dds_markdown += f"`{message['topic']}` | [{type}](../msg_docs/{px4Type}.md) | {message.get('rate_limit','')}\n"
+
+        dds_markdown += "\n## Subscriptions\n\nTopic | Type\n--- | ---\n"
+
+        for message in data["subscriptions"]:
+            type = message['type']
+            px4Type=type.split("::")[-1]
+            dds_markdown += f"{message['topic']} | [{type}](../msg_docs/{px4Type}.md)\n"
+
+        dds_markdown += "\n## Subscriptions Multi\n\n"
+
+        if not data["subscriptions_multi"]: # There is none now
+            dds_markdown += "None\n"
+        else:
+            print("Warning - we now have subscription_multi data - check format")
+            dds_markdown += "Topic | Type\n--- | ---\n"
+            for message in data["subscriptions_multi"]:
+                dds_markdown += f"{message['topic']} | {message['type']}\n"
+
+        if messagesNotExported:
+            # Print the topics that are not exported to DDS
+            dds_markdown += "\n## Not Exported\n\nThese messages are not listed in the yaml file.\nThey are not build into the module, and hence are neither published or subscribed."
+            dds_markdown += "\n\n::: details See messages\n"
+            for item in  messagesNotExported:
+                dds_markdown += f"\n- [{item}](../msg_docs/{item}.md)"
+            dds_markdown += "\n:::\n" # End of details block
+
+        #print(dds_markdown)
+        with open(output_file_path, 'w') as content_file:
+                content_file.write(dds_markdown)
+
+    except yaml.YAMLError as exc:
+        print(f"Error parsing YAML: {exc}")
+    except FileNotFoundError:
+        print(f"Error: {dds_file_path} not found.")
+
+
 def get_msgs_list(msgdir):
    """
    Makes a list of relative paths of .msg files in the given directory
@@ -30,6 +124,7 @@ def get_msgs_list(msgdir):


 if __name__ == "__main__":
+
    parser = argparse.ArgumentParser(description='Generate docs from .msg files')
    parser.add_argument('-d', dest='dir', help='output directory', required=True)
    args = parser.parse_args()
@@ -132,3 +227,5 @@ Graphs showing how these are used [can be found here](../middleware/uorb_graph.m
    index_file = os.path.join(output_dir, 'index.md')
    with open(index_file, 'w') as content_file:
            content_file.write(index_text)
+
+    generate_dds_yaml_doc(msg_files)
@@ -40,25 +40,25 @@ The generated files will be written to the `modules` directory.
 ## Categories
 """
        for category in sorted(module_groups):
-            result += "- [%s](modules_%s.md)\n" % (category.capitalize(), category)
+            result += f"- [{category.capitalize()}](modules_{category}.md)\n"

        self._outputs['main'] = result

        for category in sorted(module_groups):
-            result = "# Modules Reference: %s\n" % category.capitalize()
+            result = f"# Modules Reference: {category.capitalize()}\n\n"
            subcategories = module_groups[category]
+
            if len(subcategories) > 1:
-                result += 'Subcategories:\n'
-                for subcategory in subcategories:
+                result += 'Subcategories:\n\n'
+                for subcategory in sorted(subcategories): 
                    if subcategory == '':
                        continue
                    subcategory_label = subcategory.replace('_', ' ').title()
                    subcategory_file_name = category+'_'+subcategory
-                    result += '- [%s](modules_%s.md)\n' % (subcategory_label, subcategory_file_name)
+                    result += f'- [{subcategory_label}](modules_{subcategory_file_name}.md)\n'

                    # add a sub-page for the subcategory
-                    result_subpage = '# Modules Reference: %s (%s)\n' % \
-                        (subcategory_label, category.capitalize())
+                    result_subpage = f'# Modules Reference: {subcategory_label} ({category.capitalize()})\n'
                    result_subpage += self._ProcessModules(subcategories[subcategory])
                    self._outputs[subcategory_file_name] = result_subpage

@@ -68,14 +68,14 @@ The generated files will be written to the `modules` directory.
    def _ProcessModules(self, module_list):
        result = ''
        for module in module_list:
-            result += "## %s\n" % module.name()
-            result += "Source: [%s](https://github.com/PX4/PX4-Autopilot/tree/main/src/%s)\n\n" % (module.scope(), module.scope())
+            result += f"\n## {module.name()}\n\n"
+            result += f"Source: [{module.scope()}](https://github.com/PX4/PX4-Autopilot/tree/main/src/{module.scope()})\n\n"
            doc = module.documentation()
            if len(doc) > 0:
-                result += "%s\n" % doc
+                result += f"{doc}\n"
            usage_string = module.usage_string()
            if len(usage_string) > 0:
-                result += '<a id="%s_usage"></a>\n### Usage\n```\n%s\n```\n' % (module.name(), usage_string)
+                result += f'### Usage {{#{module.name()}_usage}}\n\n```\n{usage_string}\n```\n'
        return result

    def Save(self, dirname):
@@ -12,11 +12,11 @@ class ModuleDocumentation(object):
    """

    # If you add categories or subcategories, they also need to be added to the
-    # TOC in https://github.com/PX4/PX4-user_guide/blob/main/en/SUMMARY.md
+    # TOC in https://github.com/PX4/PX4-Autopilot/blob/main/docs/en/SUMMARY.md
    valid_categories = ['driver', 'estimator', 'controller', 'system',
                        'communication', 'command', 'template', 'simulation', 'autotune']
    valid_subcategories = ['', 'camera', 'distance_sensor', 'imu', 'ins', 'airspeed_sensor',
-                           'magnetometer', 'baro', 'optical_flow', 'rpm_sensor', 'transponder']
+                           'magnetometer', 'baro', 'optical_flow', 'radio_control','rpm_sensor', 'transponder']

    max_line_length = 80 # wrap lines that are longer than this

@@ -5,7 +5,7 @@
 set SERIAL_DEV none
 {% for serial_device in serial_devices -%}
 if param compare "$PRT" {{ serial_device.index }}; then
-	if [ "x$PRT_{{ serial_device.tag }}_" = "x" ]; then
+	if [ "$PRT_{{ serial_device.tag }}_" = "" ]; then
 		set SERIAL_DEV {{ serial_device.device }}
 		set BAUD_PARAM SER_{{ serial_device.tag }}_BAUD
 		set PRT_{{ serial_device.tag }}_ 1
@@ -27,3 +27,4 @@ six>=1.12.0
 toml>=0.9
 sympy>=1.10.1
 pycryptodome
+lark
@@ -10,6 +10,7 @@ CONFIG_DRIVERS_BAROMETER_DPS310=y
 CONFIG_DRIVERS_BATT_SMBUS=y
 CONFIG_DRIVERS_CAMERA_CAPTURE=y
 CONFIG_DRIVERS_CAMERA_TRIGGER=y
+CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y
 CONFIG_DRIVERS_DSHOT=y
@@ -41,7 +42,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -132,6 +132,7 @@ ENTRY(_stext)
 */
 EXTERN(abort)
 EXTERN(_bootdelay_signature)
+EXTERN(board_get_manifest)

 SECTIONS
 {
@@ -48,6 +48,7 @@ else()
 		i2c.cpp
 		init.c
 		led.c
+		mtd.cpp
 		spi.cpp
 		timer_config.cpp
 		usb.c
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2025 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 <nuttx/spi/spi.h>
+#include <px4_platform_common/px4_manifest.h>
+//                                                              KiB BS    nB
+static const px4_mft_device_t spi2 = {             // FM25V01A on FMUM native: 32K X 8, emulated as (1024 Blocks of 32)
+	.bus_type = px4_mft_device_t::SPI,
+	.devid    = SPIDEV_FLASH(0)
+};
+
+static const px4_mtd_entry_t fmum_fram = {
+	.device = &spi2,
+	.npart = 1,
+	.partd = {
+		{
+			.type = MTD_PARAMETERS,
+			.path = "/fs/mtd_params",
+			.nblocks = (32768 / (1 << CONFIG_RAMTRON_EMULATE_PAGE_SHIFT))
+		},
+	},
+};
+
+static const px4_mtd_manifest_t board_mtd_config = {
+	.nconfigs = 1,
+	.entries  = {
+		&fmum_fram
+	}
+};
+
+static const px4_mft_entry_s mtd_mft = {
+	.type = MTD,
+	.pmft = (void *) &board_mtd_config,
+};
+
+static const px4_mft_s mft = {
+	.nmft = 1,
+	.mfts = {
+		&mtd_mft
+	}
+};
+
+const px4_mft_s *board_get_manifest(void)
+{
+	return &mft;
+}
@@ -46,7 +46,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -56,11 +56,13 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
+CONFIG_MODULES_HARDFAULT_STREAM=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -37,7 +37,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -56,9 +57,11 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
+CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_UXRCE_DDS_CLIENT=y
+CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_BSONDUMP=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -34,75 +34,25 @@
 #pragma once

 // DMAMUX1 Using at most 8 Channels on DMA1 --------   Assigned
-//                                                     V
-
-// Timer 4 Channel 1                               /* DMA1:29 TIM4CH1 */
-
-#define DMAMAP_SPI1_RX    DMAMAP_DMA12_SPI1RX_0   /* 1 DMA1:37 IIM-42653 */
-#define DMAMAP_SPI1_TX    DMAMAP_DMA12_SPI1TX_0   /* 2 DMA1:38 IIM-42653 */
-
-//#define DMAMAP_SPI2_RX    DMAMAP_DMA12_SPI2RX_0  /* 3 DMA1:39 ICM-42688-P */
-//#define DMAMAP_SPI2_TX    DMAMAP_DMA12_SPI2TX_0  /* 4 DMA1:40 ICM-42688-P */
-
-#define DMAMAP_USART1_RX  DMAMAP_DMA12_USART1RX_0 /*  DMA1:41  GPS1 */
-#define DMAMAP_USART1_TX  DMAMAP_DMA12_USART1TX_0 /*  DMA1:42  GPS1 */
-
-//#define DMAMAP_USART3_RX  DMAMAP_DMA12_USART3RX_0 /*  DMA1:45 DEBUG */
-//#define DMAMAP_USART3_TX  DMAMAP_DMA12_USART3TX_0 /*  DMA1:46 DEBUG */
-
-// Timer 8 Channel 1                               /* DMA1:47 TIM8CH1 */
-// Timer 8 Channel 2                               /* DMA1:48 TIM8CH2 */
-// Timer 8 Channel 3                               /* DMA1:49 TIM8CH3 */
-// Timer 8 Channel 4                               /* DMA1:50 TIM8CH4 */
-
-// Timer 5 Channel 1                               /* DMA1:55 TIM5CH1 */
-// Timer 5 Channel 2                               /* DMA1:56 TIM5CH2 */
-// Timer 5 Channel 3                               /* DMA1:57 TIM5CH3 */
-// Timer 5 Channel 4                               /* DMA1:58 TIM5CH4 */
-
-// #define DMAMAP_UART4_RX   DMAMAP_DMA12_UART4RX_0  /*  DMA1:63 UART4 */
-// #define DMAMAP_UART4_TX   DMAMAP_DMA12_UART4TX_0  /*  DMA1:64 UART4 */
-
-#define DMAMAP_USART6_RX  DMAMAP_DMA12_USART6RX_0 /* 5 DMA1:71 RC */
-// #define DMAMAP_USART6_TX  DMAMAP_DMA12_USART6TX_0 /* 6 DMA1:72 RC */
-
-// Assigned in timer_config.cpp
-
-// Timer 4                                          /* 7 DMA1:32 TIM4UP */
-// Timer 5                                          /* 8 DMA1:50 TIM5UP */
+#define DMAMAP_SPI1_RX    DMAMAP_DMA12_SPI1RX_0     // 1 DMA1:37 IIM-42653
+#define DMAMAP_SPI1_TX    DMAMAP_DMA12_SPI1TX_0     // 2 DMA1:38 IIM-42653
+#define DMAMAP_USART1_RX  DMAMAP_DMA12_USART1RX_0   // 3 DMA1:41 GPS1
+#define DMAMAP_USART1_TX  DMAMAP_DMA12_USART1TX_0   // 4 DMA1:42 GPS1
+#define DMAMAP_USART6_RX  DMAMAP_DMA12_USART6RX_0   // 5 DMA1:71 RC
+#define DMAMAP_USART6_TX  DMAMAP_DMA12_USART6TX_0   // 6 DMA1:72 RC
+// Timer 4 (DMAMAP_DMA12_TIM4UP_0)                  // 7 DMA1:32 TIM4UP/TIM4CH1-4
+// Timer 5 (DMAMAP_DMA12_TIM5UP_0)                  // 8 DMA1:50 TIM5UP/TIM5CH1-4

 // DMAMUX2 Using at most 8 Channels on DMA2 --------   Assigned
-//                                                     V
-
-// Timer 4 Channel 1                               /* DMA2:29 TIM4CH1 */
-
-#define DMAMAP_USART2_RX  DMAMAP_DMA12_USART2RX_1   /* 3 DMA2:43 TELEM3 */
-#define DMAMAP_USART2_TX  DMAMAP_DMA12_USART2TX_1   /* 4 DMA2:44 TELEM3 */
-
-#define DMAMAP_USART3_RX  DMAMAP_DMA12_USART3RX_1   /* 3 DMA2:45 DEBUG */
-#define DMAMAP_USART3_TX  DMAMAP_DMA12_USART3TX_1   /* 4 DMA2:46 DEBUG */
-
-// Timer 8 Channel 1                                 /* DMA2:47 TIM8CH1 */
-// Timer 8 Channel 2                                 /* DMA2:48 TIM8CH2 */
-// Timer 8 Channel 3                                 /* DMA2:49 TIM8CH3 */
-// Timer 8 Channel 4                                 /* DMA2:50 TIM8CH4 */
-
-// Timer 5 Channel 1                                 /* DMA2:55 TIM5CH1 */
-// Timer 5 Channel 2                                 /* DMA2:56 TIM5CH2 */
-// Timer 5 Channel 3                                 /* DMA2:57 TIM5CH3 */
-// Timer 5 Channel 4                                 /* DMA2:58 TIM5CH4 */
-
-//#define DMAMAP_SPI3_RX    DMAMAP_DMA12_SPI3RX_1     /* 1 DMA2:61 BMI088 */
-//#define DMAMAP_SPI3_TX    DMAMAP_DMA12_SPI3TX_1     /* 2 DMA2:62 BMI088 */
-
-#define DMAMAP_UART5_RX   DMAMAP_DMA12_UART5RX_1    /* 5 DMA2:65 TELEM2 */
-#define DMAMAP_UART5_TX   DMAMAP_DMA12_UART5TX_1    /* 6 DMA2:66 TELEM2 */
-
-#define DMAMAP_UART7_RX   DMAMAP_DMA12_UART7RX_1    /* 7 DMA1:79 TELEM1 */
-#define DMAMAP_UART7_TX   DMAMAP_DMA12_UART7TX_1    /* 8 DMA1:80 TELEM1 */
+#define DMAMAP_USART2_RX  DMAMAP_DMA12_USART2RX_1   // 1 DMA2:43 VTX
+#define DMAMAP_UART5_RX   DMAMAP_DMA12_UART5RX_1    // 2 DMA2:65 VTX
+#define DMAMAP_UART5_TX   DMAMAP_DMA12_UART5TX_1    // 3 DMA2:66 VTX
+#define DMAMAP_UART7_RX   DMAMAP_DMA12_UART7RX_1    // 4 DMA2:79 TELEM1
+#define DMAMAP_UART7_TX   DMAMAP_DMA12_UART7TX_1    // 5 DMA2:80 TELEM1
+#define DMAMAP_USART3_RX  DMAMAP_DMA12_USART3RX_1   // 6 DMA2:45 DEBUG
+#define DMAMAP_USART3_TX  DMAMAP_DMA12_USART3TX_1   // 7 DMA2:46 DEBUG
+// available

 // DMAMUX2 Using at most 8 Channels on BDMA --------   Assigned
-//                                                     V
-
-#define DMAMAP_SPI6_RX    DMAMAP_BDMA_SPI6_RX       /* 1 BDMA:11 SPI J11 */
-#define DMAMAP_SPI6_TX    DMAMAP_BDMA_SPI6_TX       /* 2 BDMA:12 SPI J11 */
+#define DMAMAP_SPI6_RX    DMAMAP_BDMA_SPI6_RX       // 1 BDMA:11 SPI J11
+#define DMAMAP_SPI6_TX    DMAMAP_BDMA_SPI6_TX       // 2 BDMA:12 SPI J11
@@ -260,8 +260,6 @@ CONFIG_USART1_TXDMA=y
 CONFIG_USART2_BAUD=57600
 CONFIG_USART2_RXBUFSIZE=600
 CONFIG_USART2_RXDMA=y
-CONFIG_USART2_TXBUFSIZE=1500
-CONFIG_USART2_TXDMA=y
 CONFIG_USART3_BAUD=57600
 CONFIG_USART3_RXBUFSIZE=180
 CONFIG_USART3_RXDMA=y
@@ -272,6 +270,7 @@ CONFIG_USART6_BAUD=57600
 CONFIG_USART6_RXBUFSIZE=600
 CONFIG_USART6_RXDMA=y
 CONFIG_USART6_TXBUFSIZE=1500
+CONFIG_USART6_TXDMA=y
 CONFIG_USBDEV=y
 CONFIG_USBDEV_BUSPOWERED=y
 CONFIG_USBDEV_MAXPOWER=500
@@ -37,11 +37,13 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_FIGURE_OF_EIGHT=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
+CONFIG_MODULES_HARDFAULT_STREAM=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
 CONFIG_MODULES_LOAD_MON=y
@@ -26,3 +26,6 @@ nshterm /dev/ttyS3 &

 # Start the time_persistor to cyclically store the RTC in FRAM
 time_persistor start
+
+# Start the ESC telemetry
+dshot telemetry -d /dev/ttyS5 -x
@@ -3,7 +3,8 @@ CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=n
 CONFIG_MODULES_AIRSPEED_SELECTOR=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_MODE_MANAGER=n
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_VTOL_ATT_CONTROL=n
 CONFIG_COMMON_RC=y
@@ -2,7 +2,8 @@ CONFIG_MODULES_AIRSPEED_SELECTOR=n
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
 CONFIG_MODULES_FW_ATT_CONTROL=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
-CONFIG_MODULES_FW_POS_CONTROL=n
+CONFIG_MODULES_FW_MODE_MANAGER=n
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
 CONFIG_MODULES_FW_RATE_CONTROL=n
 CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
 CONFIG_MODULES_MC_ATT_CONTROL=n
@@ -42,7 +42,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -30,7 +30,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -49,7 +49,8 @@ CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
@@ -38,7 +38,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
@@ -20,6 +20,7 @@ CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_BOSCH_BMI088=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y
 CONFIG_DRIVERS_IMU_INVENSENSE_IIM42652=y
+CONFIG_DRIVERS_IMU_INVENSENSE_IIM42653=y
 CONFIG_COMMON_INS=y
 CONFIG_COMMON_LIGHT=y
 CONFIG_COMMON_MAGNETOMETER=y
@@ -42,7 +43,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -12,10 +12,17 @@ else
 fi

 iim42652 -s -R 22 start
-bmi088 -A -R 29 -s start
-bmi088 -G -R 29 -s start

-ist8310 -I -R 18 start
+bmi088 -A -R 29 -s start
+if ! bmi088 -G -R 29 -s start
+then
+	iim42653 -s -b 2 -R 22 start
+fi
+
+if ! ist8310 -I -R 18 start
+then
+	iis2mdc -I -R 37 start
+fi

 bmp581 -s start
 icp201xx -I start
@@ -42,6 +42,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI2, {
 		initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortG, GPIO::Pin2}, SPI::DRDY{GPIO::PortG, GPIO::Pin3}),
 		initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortH, GPIO::Pin5}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
+		initSPIDevice(DRV_IMU_DEVTYPE_IIM42653, SPI::CS{GPIO::PortD, GPIO::Pin12}, SPI::DRDY{GPIO::PortA, GPIO::Pin10}),
 	}, {GPIO::PortI, GPIO::Pin11}),
 	// initSPIBus(SPI::Bus::SPI3,{
 	// 	// no devices
@@ -50,7 +50,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -50,7 +50,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -49,7 +49,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -50,7 +50,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -46,7 +46,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -14,6 +14,7 @@ CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_IMU_INVENSENSE_ICM20602=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU6000=y
 CONFIG_DRIVERS_IMU_INVENSENSE_MPU9250=y
+CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y
 CONFIG_DRIVERS_MAGNETOMETER_AKM_AK8963=y
 CONFIG_DRIVERS_MAGNETOMETER_HMC5883=y
 CONFIG_DRIVERS_PWM_OUT=y
@@ -5,14 +5,16 @@

 board_adc start

-if ! mpu6000 -R 6 -s start
-then
+if ! mpu6000 -R 6 -s start; then
 	# some boards such as the Hobbywing XRotor F4 G2 use the ICM-20602
-	icm20602 -s -R 6 start
+	if ! icm20602 -s -R 6 start; then
+		# The ICM-42688-P is used on the Diatone Mamba F405 MK2 v2
+		icm42688p -s -R 4 start
+	fi
 fi

 if ! hmc5883 -T -X start
-then	
+then
 	if mpu9250_i2c -X -b 2 -a 0x68 -R 28 start; then
 		sleep 1 # wait for mpu9250 to be configured with bypass enabled
 		ak8963 -X -b 2 -R 30 start # -R 34
@@ -39,6 +39,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
 		initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortA, GPIO::Pin4}),
 		initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortA, GPIO::Pin4}),
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::PortA, GPIO::Pin4}),
 	}),
 	initSPIBus(SPI::Bus::SPI2, {
 		initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}),
@@ -32,7 +32,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -39,7 +39,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -10,4 +10,4 @@
 # fi

 # DShot telemetry is always on UART7
-# dshot telemetry /dev/ttyS5
+# dshot telemetry -d /dev/ttyS5
@@ -41,7 +41,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -10,4 +10,4 @@
 # fi

 # DShot telemetry is always on UART7
-# dshot telemetry /dev/ttyS5
+# dshot telemetry -d /dev/ttyS5
@@ -44,7 +44,8 @@ CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
-CONFIG_MODULES_FW_POS_CONTROL=y
+CONFIG_MODULES_FW_MODE_MANAGER=y
+CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=y
 CONFIG_MODULES_FW_RATE_CONTROL=y
 CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
@@ -9,4 +9,4 @@ then
 fi

 # DShot telemetry is always on UART7
-dshot telemetry /dev/ttyS5
+dshot telemetry -d /dev/ttyS5
--- a/Show More
+++ b/Show More