ActuatorEffectivenessHelicopter: clarfification suggestions for servo linearization feature

reduce CA_MAX_SVO_THROW short description length to stop test failure
fix formatting
2026-06-13 05:50:05 +08:00 · 2025-02-20 19:50:08 +01:00 · 2025-02-20 14:50:46 +01:00 · 2025-02-20 14:50:46 +01:00 · 2025-02-20 14:50:46 +01:00 · 2025-02-20 14:50:46 +01:00
54 changed files with 1612 additions and 2346 deletions
@@ -94,7 +94,7 @@ jobs:

      - name: Building [${{ matrix.group }}]
        run: |
-            ./Tools/ci/build_all_runner.sh ${{matrix.targets}} ${{matrix.arch}}
+            ./Tools/ci/build_all_runner.sh ${{matrix.targets}}

      - name: Arrange Build Artifacts
        run: |
@@ -140,12 +140,10 @@ jobs:

  release:
    name: Create Release and Upload Artifacts
-    permissions:
-      contents: write
    # runs-on: ubuntu-latest
    runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu22-full-x64,"run-id=${{ github.run_id }}",spot=false]
    needs: [setup, group_targets]
-    if: startsWith(github.ref, 'refs/tags/')
+    if: startsWith(github.ref, 'refs/tags/v')
    steps:
      - name: Download Artifacts
        uses: actions/download-artifact@v4
@@ -16,13 +16,9 @@ on:

 jobs:
  build:
-    name: Build and Push Container (${{ matrix.arch }})
-    strategy:
-      matrix:
-        arch: [x64, arm64]
-    runs-on: [runs-on,"runner=8cpu-linux-${{ matrix.arch }}","image=ubuntu24-full-${{ matrix.arch }}","run-id=${{ github.run_id }}",spot=false,extras=s3-cache]
+    name: Build and Push Container
+    runs-on: [runs-on,runner=8cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
    steps:
-      - uses: runs-on/action@v1
      - uses: actions/checkout@v4
        with:
          fetch-tags: true
@@ -36,14 +32,13 @@ jobs:

      - name: Login to Docker Hub
        uses: docker/login-action@v3
-        # if: ${{ startsWith(github.ref, 'refs/tags/') }}
+        if: github.event_name != 'pull_request'
        with:
          username: ${{ secrets.DOCKERHUB_USERNAME }}
          password: ${{ secrets.DOCKERHUB_TOKEN }}

      - name: Login to GitHub Container Registry
        uses: docker/login-action@v3
-        # if: ${{ startsWith(github.ref, 'refs/tags/') }}
        with:
          registry: ghcr.io
          username: ${{ github.actor }}
@@ -69,11 +64,12 @@ jobs:
          context: Tools/setup
          tags: ${{ steps.meta.outputs.tags }}
          labels: ${{ steps.meta.outputs.labels }}
-          platforms: linux/${{ matrix.arch == 'x64' && 'amd64' || 'arm64' }}
+          platforms: |
+            linux/amd64
          load: true
          push: false
-          cache-from: type=gha
-          cache-to: type=gha,mode=max
+          cache-from: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }}
+          cache-to: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }},mode=max

      - name: Get Tag Name
        id: tag_name
@@ -93,13 +89,13 @@ jobs:

      - name: Push container image
        uses: docker/build-push-action@v6
-        # if: ${{ startsWith(github.ref, 'refs/tags/') }}
        with:
          context: Tools/setup
          tags: ${{ steps.meta.outputs.tags }}
          labels: ${{ steps.meta.outputs.labels }}
-          platforms: linux/${{ matrix.arch == 'x64' && 'amd64' || 'arm64' }}
+          platforms: |
+            linux/amd64
          provenance: mode=max
-          push: true
-          cache-from: type=gha
-          cache-to: type=gha,mode=max
+          push: ${{ github.event_name != 'pull_request' }}
+          cache-from: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }}
+          cache-to: type=s3,blobs_prefix=cache/${{ github.repository }}/,manifests_prefix=cache/${{ github.repository }}/,region=${{ env.RUNS_ON_AWS_REGION }},bucket=${{ env.RUNS_ON_S3_BUCKET_CACHE }},mode=max
@@ -1,10 +1,5 @@
 name: FLASH usage analysis

-permissions:
-  contents: read
-  pull-requests: write
-  issues: write
-
 on:
  push:
    branches:
@@ -88,63 +83,59 @@ jobs:
          echo '${{ steps.bloaty-step.outputs.bloaty-summary-map }}' >> $GITHUB_OUTPUT
          echo "$EOF" >> $GITHUB_OUTPUT

-  # TODO:
-  # This part of the workflow is causing errors, we should find a way to fix this and enable this test again
-  # Track this issue https://github.com/PX4/PX4-Autopilot/issues/24408
-  #
-  #post_pr_comment:
-    #name: Publish Results
-    #runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
-    #needs: [analyze_flash]
-    #env:
-      #V5X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-absolute) }}
-      #V5X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-percentage) }}
-      #V6X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-absolute) }}
-      #V6X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-percentage) }}
-    #if: ${{ github.event.pull_request }}
-    #steps:
-      #- name: Find Comment
-        #uses: peter-evans/find-comment@v3
-        #id: fc
-        #with:
-          #issue-number: ${{ github.event.pull_request.number }}
-          #comment-author: 'github-actions[bot]'
-          #body-includes: FLASH Analysis
+  post_pr_comment:
+    name: Publish Results
+    runs-on: [runs-on,runner=1cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false]
+    needs: [analyze_flash]
+    env:
+      V5X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-absolute) }}
+      V5X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v5x-bloaty-summary-map).vm-percentage) }}
+      V6X-SUMMARY-MAP-ABS: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-absolute) }}
+      V6X-SUMMARY-MAP-PERC: ${{ fromJSON(fromJSON(needs.analyze_flash.outputs.px4_fmu-v6x-bloaty-summary-map).vm-percentage) }}
+    if: ${{ github.event.pull_request }}
+    steps:
+      - name: Find Comment
+        uses: peter-evans/find-comment@v3
+        id: fc
+        with:
+          issue-number: ${{ github.event.pull_request.number }}
+          comment-author: 'github-actions[bot]'
+          body-includes: FLASH Analysis

-      #- name: Set Build Time
-        #id: bt
-        #run: |
-          #echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT
+      - name: Set Build Time
+        id: bt
+        run: |
+          echo "timestamp=$(date +'%Y-%m-%dT%H:%M:%S')" >> $GITHUB_OUTPUT

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

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

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

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

-            #**Updated: _${{ steps.bt.outputs.timestamp }}_**
-          #edit-mode: replace
+            **Updated: _${{ steps.bt.outputs.timestamp }}_**
+          edit-mode: replace
@@ -73,6 +73,13 @@ menu "Toolchain"
        help
            relative path to the ROMFS root directory

+    config BOARD_ROOTFSDIR
+        string "Root directory"
+        depends on PLATFORM_POSIX
+        default "."
+        help
+            Configure the root directory in the file system for PX4 files
+
    config BOARD_IO
        string "IO board name"
        default "px4_io-v2_default"
@@ -17,19 +17,17 @@ PX4 is highly portable, OS-independent and supports Linux, NuttX and MacOS out o
  * [VTOL](https://docs.px4.io/main/en/frames_vtol/)
  * [Autogyro](https://docs.px4.io/main/en/frames_autogyro/)
  * [Rover](https://docs.px4.io/main/en/frames_rover/)
-  * many more experimental types (Blimps, Boats, Submarines, High Altitude Balloons, Spacecraft, etc)
+  * many more experimental types (Blimps, Boats, Submarines, High altitude balloons, etc)
 * Releases: [Downloads](https://github.com/PX4/PX4-Autopilot/releases)

-## Releases
-
-Release notes and supporting information for PX4 releases can be found on the [Developer Guide](https://docs.px4.io/main/en/releases/).

 ## Building a PX4 based drone, rover, boat or robot

-The [PX4 User Guide](https://docs.px4.io/main/en/) explains how to assemble [supported vehicles](https://docs.px4.io/main/en/airframes/airframe_reference.html) and fly drones with PX4. See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help!
+The [PX4 User Guide](https://docs.px4.io/main/en/) explains how to assemble [supported vehicles](https://docs.px4.io/main/en/airframes/airframe_reference.html) and fly drones with PX4.
+See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help!


-## Changing Code and Contributing
+## Changing code and contributing

 This [Developer Guide](https://docs.px4.io/main/en/development/development.html) is for software developers who want to modify the flight stack and middleware (e.g. to add new flight modes), hardware integrators who want to support new flight controller boards and peripherals, and anyone who wants to get PX4 working on a new (unsupported) airframe/vehicle.

@@ -37,7 +35,7 @@ Developers should read the [Guide for Contributions](https://docs.px4.io/main/en
 See the [forum and chat](https://docs.px4.io/main/en/#getting-help) if you need help!


-## Weekly Dev Call
+### Weekly Dev Call

 The PX4 Dev Team syncs up on a [weekly dev call](https://docs.px4.io/main/en/contribute/).

@@ -52,11 +50,69 @@ For the latest stats on contributors please see the latest stats for the Droneco

 ## Supported Hardware

-For the most up to date information, please visit [PX4 User Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/).
+Pixhawk standard boards and proprietary boards are shown below (discontinued boards aren't listed).
+
+For the most up to date information, please visit [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/main/en/flight_controller/).
+
+### Pixhawk Standard Boards
+
+These boards fully comply with Pixhawk Standard, and are maintained by the PX4-Autopilot maintainers and Dronecode team
+
+* FMUv6X and FMUv6C
+  * [CUAV Pixahwk V6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/cuav_pixhawk_v6x.html)
+  * [Holybro Pixhawk 6X (FMUv6X)](https://docs.px4.io/main/en/flight_controller/pixhawk6x.html)
+  * [Holybro Pixhawk 6C (FMUv6C)](https://docs.px4.io/main/en/flight_controller/pixhawk6c.html)
+  * [Holybro Pix32 v6 (FMUv6C)](https://docs.px4.io/main/en/flight_controller/holybro_pix32_v6.html)
+* FMUv5 and FMUv5X (STM32F7, 2019/20)
+  * [Pixhawk 4 (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4.html)
+  * [Pixhawk 4 mini (FMUv5)](https://docs.px4.io/main/en/flight_controller/pixhawk4_mini.html)
+  * [CUAV V5+ (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_plus.html)
+  * [CUAV V5 nano (FMUv5)](https://docs.px4.io/main/en/flight_controller/cuav_v5_nano.html)
+  * [Auterion Skynode (FMUv5X)](https://docs.auterion.com/avionics/skynode)
+* FMUv4 (STM32F4, 2015)
+  * [Pixracer](https://docs.px4.io/main/en/flight_controller/pixracer.html)
+  * [Pixhawk 3 Pro](https://docs.px4.io/main/en/flight_controller/pixhawk3_pro.html)
+* FMUv3 (STM32F4, 2014)
+  * [Pixhawk 2](https://docs.px4.io/main/en/flight_controller/pixhawk-2.html)
+  * [Pixhawk Mini](https://docs.px4.io/main/en/flight_controller/pixhawk_mini.html)
+  * [CUAV Pixhack v3](https://docs.px4.io/main/en/flight_controller/pixhack_v3.html)
+* FMUv2 (STM32F4, 2013)
+  * [Pixhawk](https://docs.px4.io/main/en/flight_controller/pixhawk.html)
+
+### Manufacturer supported
+
+These boards are maintained to be compatible with PX4-Autopilot by the Manufacturers.
+
+* [ARK Electronics ARKV6X](https://docs.px4.io/main/en/flight_controller/ark_v6x.html)
+* [CubePilot Cube Orange+](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orangeplus.html)
+* [CubePilot Cube Orange](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_orange.html)
+* [CubePilot Cube Yellow](https://docs.px4.io/main/en/flight_controller/cubepilot_cube_yellow.html)
+* [Holybro Durandal](https://docs.px4.io/main/en/flight_controller/durandal.html)
+* [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf)
+* [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf)
+* [Holybro Kakute F7](https://docs.px4.io/main/en/flight_controller/kakutef7.html)
+
+### Community supported
+
+These boards don't fully comply industry standards, and thus is solely maintained by the PX4 public community members.
+
+### Experimental
+
+These boards are not maintained by PX4 team nor Manufacturer, and is not guaranteed to be compatible with up to date PX4 releases.
+
+* [Raspberry PI with Navio 2](https://docs.px4.io/main/en/flight_controller/raspberry_pi_navio2.html)
+* [Bitcraze Crazyflie 2.0](https://docs.px4.io/main/en/complete_vehicles/crazyflie2.html)
+
+## Project Roadmap
+
+**Note: Outdated**
+
+A high level project roadmap is available [here](https://github.com/orgs/PX4/projects/25).

 ## Project Governance

 The PX4 Autopilot project including all of its trademarks is hosted under [Dronecode](https://www.dronecode.org/), part of the Linux Foundation.

-<a href="https://www.dronecode.org/" style="padding:20px" ><img src="https://dronecode.org/wp-content/uploads/sites/24/2020/08/dronecode_logo_default-1.png" alt="Dronecode Logo" width="110px"/></a>
+<a href="https://www.dronecode.org/" style="padding:20px" ><img src="https://mavlink.io/assets/site/logo_dronecode.png" alt="Dronecode Logo" width="110px"/></a>
+<a href="https://www.linuxfoundation.org/projects" style="padding:20px;"><img src="https://mavlink.io/assets/site/logo_linux_foundation.png" alt="Linux Foundation Logo" width="80px" /></a>
 <div style="padding:10px">&nbsp;</div>
@@ -1,34 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-add_subdirectory(init.d)
@@ -1,68 +0,0 @@
-#!/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 /
-
-#
-# 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
-
-#
-# Try to mount the microSD card.
-#
-mount -t vfat /dev/mmcsd0 /fs/microsd
-if [ $? = 0 ]
-then
-	echo "SD card mounted at /fs/microsd"
-else
-	echo "No SD card found"
-fi
-
-unset R
-
-echo ""
-echo "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
-echo "!!!!!!     This is the PERFORMANCE TESTING firmware! WARNs and ERRORs are expected!     !!!!!"
-echo "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
-echo ""
@@ -11,38 +11,28 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover_mecanum}

 param set-default SIM_GZ_EN 1 # Gazebo bridge

-param set-default NAV_ACC_RAD 0.5
-
-# Mecanum Parameters
+# Rover parameters
 param set-default RM_WHEEL_TRACK 0.3
+param set-default RM_YAW_RATE_I 0.1
+param set-default RM_YAW_RATE_P 0.1
+param set-default RM_MAX_ACCEL 3
+param set-default RM_MAX_DECEL 5
+param set-default RM_MAX_JERK 5
+param set-default RM_MAX_SPEED 2
+param set-default RM_MAX_THR_SPD 2.2
 param set-default RM_MAX_THR_YAW_R 1.2
-param set-default RM_MISS_SPD_GAIN 1
+param set-default RM_YAW_P 5
+param set-default RM_YAW_I 0.1
+param set-default RM_MAX_YAW_RATE 120
+param set-default RM_MAX_YAW_ACCEL 240
+param set-default RM_MISS_VEL_GAIN 1
+param set-default RM_SPEED_I 0.01
+param set-default RM_SPEED_P 0.1

-# Rover Control Parameters
-param set-default RO_ACCEL_LIM 3
-param set-default RO_DECEL_LIM 5
-param set-default RO_JERK_LIM 30
-param set-default RO_MAX_THR_SPEED 2.1
-
-# Rover Rate Control Parameters
-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_ACCEL_LIM 240
-param set-default RO_YAW_DECEL_LIM 1000
-
-# Rover Attitude Control Parameters
-param set-default RO_YAW_P 5
-
-# Rover Position Control Parameters
-param set-default RO_SPEED_LIM 2
-param set-default RO_SPEED_I 0.5
-param set-default RO_SPEED_P 1
-
-# Pure Pursuit parameters
-param set-default PP_LOOKAHD_GAIN 0.5
+# Pure pursuit parameters
 param set-default PP_LOOKAHD_MAX 10
 param set-default PP_LOOKAHD_MIN 1
+param set-default PP_LOOKAHD_GAIN 0.5

 # Simulated sensors
 param set-default SENS_EN_GPSSIM 1
@@ -69,7 +69,7 @@ def process_target(px4board_file, target_name):
    group = None

    if px4board_file.endswith("default.px4board") or \
-        px4board_file.endswith("performance-test.px4board") or \
+        px4board_file.endswith("recovery.px4board") or \
        px4board_file.endswith("bootloader.px4board"):
        kconf.load_config(px4board_file, replace=True)
    else: # Merge config with default.px4board
@@ -87,17 +87,20 @@ def process_target(px4board_file, target_name):

    if platform not in excluded_platforms:
        # get the container based on the platform and toolchain
-        container = 'ghcr.io/px4/px4-dev:main'
        if platform == 'posix':
+            container = 'px4io/px4-dev-base-focal:2021-09-08'
            group = 'base'
            if toolchain:
                if toolchain.startswith('aarch64'):
+                    container = 'px4io/px4-dev-aarch64:2022-08-12'
                    group = 'aarch64'
                elif toolchain == 'arm-linux-gnueabihf':
+                    container = 'px4io/px4-dev-armhf:2023-06-26'
                    group = 'armhf'
                else:
                    if verbose: print(f'unmatched toolchain: {toolchain}')
        elif platform == 'nuttx':
+            container = 'px4io/px4-dev-nuttx-focal:2022-08-12'
            group = 'nuttx'
        else:
            if verbose: print(f'unmatched platform: {platform}')
@@ -121,7 +124,7 @@ if(verbose):
 # - Events
 metadata_targets = ['airframe_metadata', 'parameters_metadata', 'extract_events']
 grouped_targets['base'] = {}
-grouped_targets['base']['container'] = 'ghcr.io/px4/px4-dev:main'
+grouped_targets['base']['container'] = 'px4io/px4-dev-base-focal:2021-09-08'
 grouped_targets['base']['manufacturers'] = {}
 grouped_targets['base']['manufacturers']['px4'] = []
 grouped_targets['base']['manufacturers']['px4'] += metadata_targets
@@ -4,9 +4,8 @@ mkdir artifacts
 cp **/**/*.px4 artifacts/
 cp **/**/*.elf artifacts/
 for build_dir_path in build/*/ ; do
-  build_dir_path=${build_dir_path::${#build_dir_path}-1}
  build_dir=${build_dir_path#*/}
-  target_name=$build_dir
+  build_dir=${build_dir::${#build_dir}-1}
  mkdir artifacts/$build_dir
  find artifacts/ -maxdepth 1 -type f -name "*$build_dir*"
  # Airframe
@@ -27,23 +26,21 @@ for build_dir_path in build/*/ ; do
  echo "----------"
 done

-if [ -d artifacts/px4_sitl_default ]; then
-  # general metadata
-  mkdir artifacts/_general/
-  cp artifacts/px4_sitl_default/airframes.xml artifacts/_general/
-  # Airframe
-  cp artifacts/px4_sitl_default/airframes.xml artifacts/_general/
-  # Parameters
-  cp artifacts/px4_sitl_default/parameters.xml artifacts/_general/
-  cp artifacts/px4_sitl_default/parameters.json artifacts/_general/
-  cp artifacts/px4_sitl_default/parameters.json.xz artifacts/_general/
-  # Actuators
-  cp artifacts/px4_sitl_default/actuators.json artifacts/_general/
-  cp artifacts/px4_sitl_default/actuators.json.xz artifacts/_general/
-  # Events
-  cp artifacts/px4_sitl_default/events/all_events.json.xz artifacts/_general/
-  # ROS 2 msgs
-  cp artifacts/px4_sitl_default/events/all_events.json.xz artifacts/_general/
-  # Module Docs
-  ls -la artifacts/_general/
-fi
+# general metadata
+mkdir artifacts/_general/
+cp artifacts/px4_sitl_default/airframes.xml artifacts/_general/
+# Airframe
+cp artifacts/px4_sitl_default/airframes.xml artifacts/_general/
+# Parameters
+cp artifacts/px4_sitl_default/parameters.xml artifacts/_general/
+cp artifacts/px4_sitl_default/parameters.json artifacts/_general/
+cp artifacts/px4_sitl_default/parameters.json.xz artifacts/_general/
+# Actuators
+cp artifacts/px4_sitl_default/actuators.json artifacts/_general/
+cp artifacts/px4_sitl_default/actuators.json.xz artifacts/_general/
+# Events
+cp artifacts/px4_sitl_default/events/all_events.json.xz artifacts/_general/
+# ROS 2 msgs
+cp artifacts/px4_sitl_default/events/all_events.json.xz artifacts/_general/
+# Module Docs
+ls -la artifacts/_general/
@@ -74,7 +74,6 @@ exception_list_sitl = [
    'SYSTEMCMDS_I2CDETECT', # Not supported in SITL
    'SYSTEMCMDS_DMESG', # Not supported in SITL
    'SYSTEMCMDS_USB_CONNECTED', # Not supported in SITL
-    'SYSTEMCMDS_MFT_CFG', # Not supported in SITL
    'MODULES_SPACECRAFT', # Clashes with Control Allocation (mom's spaghetti code)
 ]

@@ -1,27 +1,19 @@
 #!/bin/bash

-GREEN='\033[0;32m'
-NC='\033[0m' # No Color
-FILE_DESCRIPTOR="${GREEN}[docker-entrypoint.sh]${NC}"
-
-echo -e "$FILE_DESCRIPTOR Starting"
-
 # Start virtual X server in the background
 # - DISPLAY default is :99, set in dockerfile
 # - Users can override with `-e DISPLAY=` in `docker run` command to avoid
 #   running Xvfb and attach their screen
 if [[ -x "$(command -v Xvfb)" && "$DISPLAY" == ":99" ]]; then
-	echo -e "$FILE_DESCRIPTOR Starting Xvfb"
+	echo "[docker-entrypoint.sh] Starting Xvfb"
 	Xvfb :99 -screen 0 1600x1200x24+32 &
 fi

 # Check if the ROS_DISTRO is passed and use it
 # to source the ROS environment
 if [ -n "${ROS_DISTRO}" ]; then
-	echo -e "$FILE_DESCRIPTOR ROS: ${ROS_DISTRO}"
+	echo "[docker-entrypoint.sh] ROS: ${ROS_DISTRO}"
 	source "/opt/ros/$ROS_DISTRO/setup.bash"
 fi

-echo -e "$FILE_DESCRIPTOR ($( uname -m ))"
-
 exec "$@"
@@ -27,12 +27,9 @@ do
 	fi
 done

-echo "[ubuntu.sh] Starting..."
-echo "[ubuntu.sh] arch: ${GREEN}$INSTALL_ARCH${NC}"
-
 # detect if running in docker
 if [ -f /.dockerenv ]; then
-	echo "[ubuntu.sh] Running within docker, installing initial dependencies";
+	echo "Running within docker, installing initial dependencies";
 	apt-get --quiet -y update && DEBIAN_FRONTEND=noninteractive apt-get --quiet -y install \
 		ca-certificates \
 		gnupg \
@@ -50,7 +47,7 @@ DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
 # check requirements.txt exists (script not run in source tree)
 REQUIREMENTS_FILE="requirements.txt"
 if [[ ! -f "${DIR}/${REQUIREMENTS_FILE}" ]]; then
-	echo "[ubuntu.sh] FAILED: ${REQUIREMENTS_FILE} needed in same directory as ubuntu.sh (${DIR})."
+	echo "FAILED: ${REQUIREMENTS_FILE} needed in same directory as ubuntu.sh (${DIR})."
 	return 1
 fi

@@ -58,8 +55,10 @@ fi
 # check ubuntu version
 # otherwise warn and point to docker?
 UBUNTU_RELEASE="`lsb_release -rs`"
-echo "[ubuntu.sh] Ubuntu ${GREEN}${UBUNTU_RELEASE}${NC}"
-echo "[ubuntu.sh] Installing PX4 general dependencies"
+echo "Ubuntu ${UBUNTU_RELEASE}"
+
+echo
+echo "Installing PX4 general dependencies"

 sudo apt-get update -y --quiet
 sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
@@ -92,7 +91,7 @@ sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends i

 # Python3 dependencies
 echo
-echo "[ubuntu.sh] Installing PX4 Python3 dependencies"
+echo "Installing PX4 Python3 dependencies"
 PYTHON_VERSION=$(python3 --version 2>&1 | awk '{print $2}')
 REQUIRED_VERSION="3.11"
 if [[ "$(printf '%s\n' "$REQUIRED_VERSION" "$PYTHON_VERSION" | sort -V | head -n1)" == "$REQUIRED_VERSION" ]]; then
@@ -110,8 +109,8 @@ fi
 if [[ $INSTALL_NUTTX == "true" ]]; then

 	echo
-	echo "[ubuntu.sh] NuttX Installing Dependencies"
-	sudo apt-get update -y --quiet
+	echo "Installing NuttX dependencies"
+
 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		automake \
 		binutils-dev \
@@ -119,6 +118,9 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 		build-essential \
 		curl \
 		flex \
+		g++-multilib \
+		gcc-arm-none-eabi \
+		gcc-multilib \
 		gdb-multiarch \
 		genromfs \
 		gettext \
@@ -145,25 +147,6 @@ if [[ $INSTALL_NUTTX == "true" ]]; then
 		vim-common \
 		;

-
-	echo
-	echo "[ubuntu.sh] NuttX Installing Dependencies ($INSTALL_ARCH)"
-
-	if [[ "${INSTALL_ARCH}" == "x86_64" ]]; then
-		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-			g++-multilib \
-			gcc-arm-none-eabi \
-			gcc-multilib \
-			;
-	fi
-
-	if [[ "${INSTALL_ARCH}" == "aarch64" ]]; then
-		sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
-			g++-aarch64-linux-gnu \
-			g++-arm-linux-gnueabihf \
-			;
-	fi
-
 	if [ -n "$USER" ]; then
 		# add user to dialout group (serial port access)
 		sudo usermod -aG dialout $USER
@@ -174,7 +157,7 @@ fi
 if [[ $INSTALL_SIM == "true" ]]; then

 	echo
-	echo "[ubuntu.sh] Installing PX4 simulation dependencies"
+	echo "Installing PX4 simulation dependencies"

 	# General simulation dependencies
 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
@@ -199,8 +182,8 @@ if [[ $INSTALL_SIM == "true" ]]; then
 		fi
 	else
 		# Expects Ubuntu 22.04 > by default
-		echo "[ubuntu.sh] Gazebo (Harmonic) will be installed"
-		echo "[ubuntu.sh] Earlier versions will be removed"
+		echo "Gazebo (Harmonic) will be installed"
+		echo "Earlier versions will be removed"
 		# Add Gazebo binary repository
 		sudo wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
 		echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | sudo tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
@@ -1,7 +1,7 @@
 CONFIG_PLATFORM_POSIX=y
 CONFIG_BOARD_LINUX_TARGET=y
 CONFIG_BOARD_TOOLCHAIN="aarch64-linux-gnu"
-CONFIG_BOARD_ROOT_PATH="/data/px4"
+CONFIG_BOARD_ROOTFSDIR="/data/px4"
 CONFIG_DRIVERS_ACTUATORS_VOXL_ESC=y
 CONFIG_DRIVERS_GPS=y
 CONFIG_DRIVERS_OSD_MSP_OSD=y
@@ -1,31 +0,0 @@
-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_EXT2="/dev/ttyS3"
-CONFIG_BOARD_ROMFSROOT="performance-test"
-CONFIG_DRIVERS_ADC_BOARD_ADC=y
-CONFIG_SYSTEMCMDS_DMESG=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_MTD=y
-CONFIG_SYSTEMCMDS_MFT_CFG=y
-CONFIG_SYSTEMCMDS_PARAM=y
-CONFIG_SYSTEMCMDS_PERF=y
-CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_SD_BENCH=y
-CONFIG_SYSTEMCMDS_SD_STRESS=y
-CONFIG_SYSTEMCMDS_SERIAL_TEST=y
-CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
-CONFIG_SYSTEMCMDS_TOP=y
-CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_SYSTEMCMDS_VER=y
-CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -1,31 +0,0 @@
-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_EXT2="/dev/ttyS3"
-CONFIG_BOARD_ROMFSROOT="performance-test"
-CONFIG_DRIVERS_ADC_BOARD_ADC=y
-CONFIG_SYSTEMCMDS_DMESG=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_MTD=y
-CONFIG_SYSTEMCMDS_MFT_CFG=y
-CONFIG_SYSTEMCMDS_PARAM=y
-CONFIG_SYSTEMCMDS_PERF=y
-CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_SD_BENCH=y
-CONFIG_SYSTEMCMDS_SD_STRESS=y
-CONFIG_SYSTEMCMDS_SERIAL_TEST=y
-CONFIG_SYSTEMCMDS_SYSTEM_TIME=y
-CONFIG_SYSTEMCMDS_TOP=y
-CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
-CONFIG_SYSTEMCMDS_UORB=y
-CONFIG_SYSTEMCMDS_VER=y
-CONFIG_SYSTEMCMDS_WORK_QUEUE=y
@@ -35,7 +35,7 @@ if(EXISTS ${BOARD_DEFCONFIG})
 	# Depend on BOARD_DEFCONFIG so that we reconfigure on config change
 	set_property(DIRECTORY APPEND PROPERTY CMAKE_CONFIGURE_DEPENDS ${BOARD_DEFCONFIG})

-	if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "performance-test" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader")
+	if(${LABEL} MATCHES "default" OR ${LABEL} MATCHES "recovery" OR ${LABEL} MATCHES "bootloader" OR ${LABEL} MATCHES "canbootloader")
 		# Generate boardconfig from saved defconfig
 		execute_process(
 			COMMAND ${CMAKE_COMMAND} -E env ${COMMON_KCONFIG_ENV_SETTINGS}
@@ -173,11 +173,14 @@ set(msg_files
 	RcParameterMap.msg
 	RoverAttitudeSetpoint.msg
 	RoverAttitudeStatus.msg
+	RoverVelocityStatus.msg
 	RoverRateSetpoint.msg
 	RoverRateStatus.msg
 	RoverSteeringSetpoint.msg
 	RoverThrottleSetpoint.msg
-	RoverVelocityStatus.msg
+	RoverMecanumGuidanceStatus.msg
+	RoverMecanumSetpoint.msg
+	RoverMecanumStatus.msg
 	Rpm.msg
 	RtlStatus.msg
 	RtlTimeEstimate.msg
@@ -0,0 +1,7 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller
+float32 heading_error      # [rad] Heading error of the pure pursuit controller
+float32 desired_speed      # [m/s] Desired velocity magnitude (speed)
+
+# TOPICS rover_mecanum_guidance_status
@@ -0,0 +1,11 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 forward_speed_setpoint            # [m/s] Desired forward speed
+float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized forward speed
+float32 lateral_speed_setpoint            # [m/s] Desired lateral speed
+float32 lateral_speed_setpoint_normalized # [-1, 1] Desired normalized lateral speed
+float32 yaw_rate_setpoint                 # [rad/s] Desired yaw rate
+float32 speed_diff_setpoint_normalized    # [-1, 1] Normalized speed difference between the left and right wheels
+float32 yaw_setpoint 	                  # [rad] Desired yaw (heading)
+
+# TOPICS rover_mecanum_setpoint
@@ -0,0 +1,17 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 measured_forward_speed          # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
+float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate
+float32 measured_lateral_speed          # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
+float32 adjusted_lateral_speed_setpoint # [m/s] Speed setpoint after applying slew rate
+float32 measured_yaw_rate  	        # [rad/s] Measured yaw rate
+float32 clyaw_yaw_rate_setpoint         # [rad/s] Yaw rate setpoint output by the closed loop yaw controller
+float32 adjusted_yaw_rate_setpoint      # [rad/s] Yaw rate setpoint from the closed loop yaw controller
+float32 measured_yaw        	        # [rad] Measured yaw
+float32 adjusted_yaw_setpoint           # [rad] Yaw setpoint after applying slew rate
+float32 pid_yaw_rate_integral  	        # Integral of the PID for the closed loop yaw rate controller
+float32 pid_yaw_integral       	        # Integral of the PID for the closed loop yaw controller
+float32 pid_forward_throttle_integral   # Integral of the PID for the closed loop forward speed controller
+float32 pid_lateral_throttle_integral   # Integral of the PID for the closed loop lateral speed controller
+
+# TOPICS rover_mecanum_status
@@ -3,7 +3,7 @@ uint64 timestamp_sample

 uint32 device_id               # unique device ID for the sensor that does not change between power cycles

-float32[2] pixel_flow          # (radians) optical flow in radians where a positive value is produced by a RH rotation of the sensor about the body axis
+float32[2] pixel_flow          # (radians) optical flow in radians where a positive value is produced by a RH rotation about the body axis

 float32[3] delta_angle         # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data.
 bool delta_angle_available
@@ -7,7 +7,7 @@ uint32 device_id               # unique device ID for the sensor that does not c

 float32[2] pixel_flow          # (radians) accumulated optical flow in radians where a positive value is produced by a RH rotation about the body axis

-float32[3] delta_angle         # (radians) accumulated gyro radians where a positive value is produced by a RH rotation of the sensor about the body axis. (NAN if unavailable)
+float32[3] delta_angle         # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. (NAN if unavailable)

 float32 distance_m             # (meters) Distance to the center of the flow field (NAN if unavailable)

@@ -97,7 +97,7 @@ __BEGIN_DECLS
 extern long PX4_TICKS_PER_SEC;
 __END_DECLS

-#define PX4_ROOTFSDIR CONFIG_BOARD_ROOT_PATH
+#define PX4_ROOTFSDIR CONFIG_BOARD_ROOTFSDIR

 // Qurt doesn't have an SD card for storage
 #ifndef __PX4_QURT
@@ -208,6 +208,10 @@ float ActuatorEffectivenessHelicopter::getLinearServoOutput(float input) const
 	// make sure a the maximal input of [-1,1] maps to the maximal vertical deflection the servo can reach of sin(CA_MAX_SVO_THROW)
 	float servo_height = _geometry.max_servo_height * input;

+	if (!PX4_ISFINITE(servo_height)) {
+		servo_height = 0.f;
+	}
+
 	// mulitply by 1 over max arm roation in radians to normalise
 	return _geometry.inverse_max_servo_throw * asinf(servo_height);
 }
@@ -159,11 +159,6 @@ bool FlightTaskAuto::update()
 	_checkEmergencyBraking();
 	Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp};

-	if (isTargetModified()) {
-		// In case the target has been modified, we take this as the next waypoints
-		waypoints[2] = _position_setpoint;
-	}
-
 	const bool should_wait_for_yaw_align = _param_mpc_yaw_mode.get() == int32_t(yaw_mode::towards_waypoint_yaw_first)
 					       && !_yaw_sp_aligned;
 	const bool force_zero_velocity_setpoint = should_wait_for_yaw_align || _is_emergency_braking_active;
@@ -764,15 +759,6 @@ bool FlightTaskAuto::_generateHeadingAlongTraj()
 	return res;
 }

-bool FlightTaskAuto::isTargetModified() const
-{
-	const bool xy_modified = (_target - _position_setpoint).xy().longerThan(FLT_EPSILON);
-	const bool z_valid = PX4_ISFINITE(_position_setpoint(2));
-	const bool z_modified =  z_valid && std::fabs((_target - _position_setpoint)(2)) > FLT_EPSILON;
-
-	return xy_modified || z_modified;
-}
-
 void FlightTaskAuto::_updateTrajConstraints()
 {
 	// update params of the position smoothing
@@ -108,7 +108,6 @@ protected:

 	void _checkEmergencyBraking();
 	bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
-	bool isTargetModified() const;
 	void _updateTrajConstraints();

 	void rcHelpModifyYaw(float &yaw_sp);
@@ -355,52 +355,31 @@ int InternalCombustionEngineControl::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-		
 The module controls internal combustion engine (ICE) features including:
-ignition (on/off), throttle and choke level, starter engine delay, and user request.
+ignition (on/off),throttle and choke level, starter engine delay, and user request.
+The module publishes [InternalCombustionEngineControl.msg](../msg_docs/InternalCombustionEngineControl.md).
+The architecture is as shown below.:
+![Architecture](../../assets/diagrams/ice_control_diagram.png)

 ### Enabling
-
 This feature is not enabled by default needs to be configured in the
 build target for your board together with the rpm capture driver:
-
-```
 CONFIG_MODULES_INTERNAL_COMBUSTION_ENGINE_CONTROL=y
 CONFIG_DRIVERS_RPM_CAPTURE=y
-```

 Additionally, to enable the module:
-
- Set [ICE_EN](../advanced_config/parameter_reference.md#ICE_EN)
-to true and adjust the other `ICE_` module parameters according to your needs.
- Set [RPM_CAP_ENABLE](../advanced_config/parameter_reference.md#RPM_CAP_ENABLE) to true.
-
-The module outputs control signals for ignition, throttle, and choke,
-and takes inputs from an RPM sensor.
-These must be mapped to AUX outputs/inputs in the [Actuator configuration](../config/actuators.md),
-similar to the setup shown below.
-
-![Actuator setup for ICE](../../assets/hardware/ice/ice_actuator_setup.png)
+- set [ICE_EN](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#ICE_EN)
+to true and adjust the other module parameters ICE_ according to your needs.
+- set [RPM_CAP_ENABLE](https://docs.px4.io/main/en/advanced_config/parameter_reference.html#RPM_CAP_ENABLE) to true.

 ### Implementation
-
 The ICE is implemented with a (4) state machine:
-
-![Architecture](../../assets/hardware/ice/ice_control_state_machine.png)
-
+![Architecture](../../assets/diagrams/ice_control_state_machine.png)
 The state machine:
-		
- Checks if [Rpm.msg](../msg_docs/Rpm.md) is updated to know if the engine is running
- Allows for user inputs from:
-  - AUX{N}
-  - Arming state in [VehicleStatus.msg](../msg_docs/VehicleStatus.md)
-
-The module publishes [InternalCombustionEngineControl.msg](../msg_docs/InternalCombustionEngineControl.md).
-		
-The architecture is as shown below:
-
-![Architecture](../../assets/hardware/ice/ice_control_diagram.png)
-		
+- checks if [Rpm.msg](../msg_docs/Rpm.md) is updated to know if the engine is running
+- allows for user inputs from
+- AUX{N}
+- Arming state in [VehicleStatus.msg(../msg_docs/VehicleStatus.md)
 <a id="internal_combustion_engine_control_usage"></a>
 )DESCR_STR");

@@ -105,11 +105,14 @@ void LoggedTopics::add_default_topics()
 	add_topic("radio_status");
 	add_optional_topic("rover_attitude_setpoint", 100);
 	add_optional_topic("rover_attitude_status", 100);
+	add_optional_topic("rover_velocity_status", 100);
 	add_optional_topic("rover_rate_setpoint", 100);
 	add_optional_topic("rover_rate_status", 100);
 	add_optional_topic("rover_steering_setpoint", 100);
 	add_optional_topic("rover_throttle_setpoint", 100);
-	add_optional_topic("rover_velocity_status", 100);
+	add_optional_topic("rover_mecanum_guidance_status", 100);
+	add_optional_topic("rover_mecanum_setpoint", 100);
+	add_optional_topic("rover_mecanum_status", 100);
 	add_topic("rtl_time_estimate", 1000);
 	add_topic("rtl_status", 2000);
 	add_optional_topic("sensor_airflow", 100);
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,9 +31,8 @@
 #
 ############################################################################

-add_subdirectory(MecanumRateControl)
-add_subdirectory(MecanumAttControl)
-add_subdirectory(MecanumPosVelControl)
+add_subdirectory(RoverMecanumGuidance)
+add_subdirectory(RoverMecanumControl)

 px4_add_module(
 	MODULE modules__rover_mecanum
@@ -42,11 +41,10 @@ px4_add_module(
 		RoverMecanum.cpp
 		RoverMecanum.hpp
 	DEPENDS
-		MecanumRateControl
-		MecanumAttControl
-		MecanumPosVelControl
+		RoverMecanumGuidance
+		RoverMecanumControl
 		px4_work_queue
-		rover_control
+		pure_pursuit
 	MODULE_CONFIG
 		module.yaml
 )
@@ -1,5 +1,6 @@
 menuconfig MODULES_ROVER_MECANUM
 	bool "rover_mecanum"
 	default n
+	depends on MODULES_CONTROL_ALLOCATOR
 	---help---
-		Enable support for mecanum rovers
+		Enable support for control of mecanum rovers
@@ -1,39 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-px4_add_library(MecanumAttControl
-	MecanumAttControl.cpp
-)
-
-target_link_libraries(MecanumAttControl PUBLIC PID)
-target_include_directories(MecanumAttControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -1,215 +0,0 @@
-/****************************************************************************
- *
- *   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 "MecanumAttControl.hpp"
-
-using namespace time_literals;
-
-MecanumAttControl::MecanumAttControl(ModuleParams *parent) : ModuleParams(parent)
-{
-	_rover_rate_setpoint_pub.advertise();
-	_rover_throttle_setpoint_pub.advertise();
-	_rover_attitude_setpoint_pub.advertise();
-	_rover_attitude_status_pub.advertise();
-	updateParams();
-}
-
-void MecanumAttControl::updateParams()
-{
-	ModuleParams::updateParams();
-
-	if (_param_ro_yaw_rate_limit.get() > FLT_EPSILON) {
-		_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
-	}
-
-	_pid_yaw.setGains(_param_ro_yaw_p.get(), 0.f, 0.f);
-	_pid_yaw.setIntegralLimit(_max_yaw_rate);
-	_pid_yaw.setOutputLimit(_max_yaw_rate);
-	_adjusted_yaw_setpoint.setSlewRate(_max_yaw_rate);
-}
-
-void MecanumAttControl::updateAttControl()
-{
-	const hrt_abstime timestamp_prev = _timestamp;
-	_timestamp = hrt_absolute_time();
-	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
-
-	if (_vehicle_control_mode_sub.updated()) {
-		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
-	}
-
-	if (_vehicle_attitude_sub.updated()) {
-		vehicle_attitude_s vehicle_attitude{};
-		_vehicle_attitude_sub.copy(&vehicle_attitude);
-		matrix::Quatf vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
-		_vehicle_yaw = matrix::Eulerf(vehicle_attitude_quaternion).psi();
-	}
-
-	if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {
-
-		if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) {
-			generateAttitudeSetpoint();
-		}
-
-		generateRateSetpoint();
-
-	} else { // Reset pid and slew rate when attitude control is not active
-		_pid_yaw.resetIntegral();
-		_adjusted_yaw_setpoint.setForcedValue(0.f);
-	}
-
-	// Publish attitude controller status (logging only)
-	rover_attitude_status_s rover_attitude_status;
-	rover_attitude_status.timestamp = _timestamp;
-	rover_attitude_status.measured_yaw = _vehicle_yaw;
-	rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState();
-	_rover_attitude_status_pub.publish(rover_attitude_status);
-
-}
-
-void MecanumAttControl::generateAttitudeSetpoint()
-{
-	const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
-				       && !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled;
-
-	if (stab_mode_enabled && _manual_control_setpoint_sub.updated()) { // Stab Mode
-		manual_control_setpoint_s manual_control_setpoint{};
-
-		if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-
-			rover_throttle_setpoint_s rover_throttle_setpoint{};
-			rover_throttle_setpoint.timestamp = _timestamp;
-			rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
-			rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
-			_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
-
-			const float yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
-							_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
-
-			if (fabsf(yaw_rate_setpoint) > FLT_EPSILON) { // Closed loop yaw rate control
-				_stab_yaw_setpoint = NAN;
-				_adjusted_yaw_setpoint.setForcedValue(0.f);
-				rover_rate_setpoint_s rover_rate_setpoint{};
-				rover_rate_setpoint.timestamp = _timestamp;
-				rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint;
-				_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-
-			} else if (fabsf(rover_throttle_setpoint.throttle_body_x) > FLT_EPSILON
-				   || fabsf(rover_throttle_setpoint.throttle_body_y) >
-				   FLT_EPSILON) { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
-				if (!PX4_ISFINITE(_stab_yaw_setpoint)) {
-					_stab_yaw_setpoint = _vehicle_yaw;
-				}
-
-				rover_attitude_setpoint_s rover_attitude_setpoint{};
-				rover_attitude_setpoint.timestamp = _timestamp;
-				rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint;
-				_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-
-			} else { // Reset yaw control  and yaw rate setpoint
-				_stab_yaw_setpoint = NAN;
-				_adjusted_yaw_setpoint.setForcedValue(0.f);
-				rover_rate_setpoint_s rover_rate_setpoint{};
-				rover_rate_setpoint.timestamp = _timestamp;
-				rover_rate_setpoint.yaw_rate_setpoint = 0.f;
-				_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-			}
-
-
-		}
-
-	} else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control
-		trajectory_setpoint_s trajectory_setpoint{};
-		_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-
-		if (_offboard_control_mode_sub.updated()) {
-			_offboard_control_mode_sub.copy(&_offboard_control_mode);
-		}
-
-		const bool offboard_att_control = _offboard_control_mode.attitude;
-
-		if (offboard_att_control && PX4_ISFINITE(trajectory_setpoint.yaw)) {
-			rover_attitude_setpoint_s rover_attitude_setpoint{};
-			rover_attitude_setpoint.timestamp = _timestamp;
-			rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw;
-			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-		}
-	}
-}
-
-void MecanumAttControl::generateRateSetpoint()
-{
-	if (_rover_attitude_setpoint_sub.updated()) {
-		_rover_attitude_setpoint_sub.copy(&_rover_attitude_setpoint);
-	}
-
-	if (_rover_rate_setpoint_sub.updated()) {
-		_rover_rate_setpoint_sub.copy(&_rover_rate_setpoint);
-	}
-
-	// Check if a new rate setpoint was already published from somewhere else
-	if (_rover_rate_setpoint.timestamp > _last_rate_setpoint_update
-	    && _rover_rate_setpoint.timestamp > _rover_attitude_setpoint.timestamp) {
-		return;
-	}
-
-	const float yaw_rate_setpoint = RoverControl::attitudeControl(_adjusted_yaw_setpoint, _pid_yaw, _max_yaw_rate,
-					_vehicle_yaw, _rover_attitude_setpoint.yaw_setpoint, _dt);
-
-	_last_rate_setpoint_update = _timestamp;
-	rover_rate_setpoint_s rover_rate_setpoint{};
-	rover_rate_setpoint.timestamp = _timestamp;
-	rover_rate_setpoint.yaw_rate_setpoint = math::constrain(yaw_rate_setpoint, -_max_yaw_rate, _max_yaw_rate);
-	_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-}
-
-bool MecanumAttControl::runSanityChecks()
-{
-	bool ret = true;
-
-	if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
-		ret = false;
-	}
-
-	if (_param_ro_yaw_p.get() < FLT_EPSILON) {
-		ret = false;
-
-		if (_prev_param_check_passed) {
-			events::send<float>(events::ID("mecanum_att_control_conf_invalid_yaw_p"), events::Log::Error,
-					    "Invalid configuration of necessary parameter RO_YAW_P", _param_ro_yaw_p.get());
-		}
-	}
-
-	_prev_param_check_passed = ret;
-	return ret;
-}
@@ -1,142 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-#pragma once
-
-// PX4 includes
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/events.h>
-
-// Libraries
-#include <lib/rover_control/RoverControl.hpp>
-#include <lib/pid/PID.hpp>
-#include <lib/slew_rate/SlewRateYaw.hpp>
-#include <math.h>
-#include <matrix/matrix/math.hpp>
-
-// uORB includes
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/rover_rate_setpoint.h>
-#include <uORB/topics/rover_throttle_setpoint.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/rover_attitude_status.h>
-#include <uORB/topics/rover_attitude_setpoint.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/offboard_control_mode.h>
-#include <uORB/topics/trajectory_setpoint.h>
-
-/**
- * @brief Class for mecanum attitude control.
- */
-class MecanumAttControl : public ModuleParams
-{
-public:
-	/**
-	 * @brief Constructor for MecanumAttControl.
-	 * @param parent The parent ModuleParams object.
-	 */
-	MecanumAttControl(ModuleParams *parent);
-	~MecanumAttControl() = default;
-
-	/**
-	 * @brief Update attitude controller.
-	 */
-	void updateAttControl();
-
-protected:
-	/**
-	 * @brief Update the parameters of the module.
-	 */
-	void updateParams() override;
-
-private:
-	/**
-	 * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode)
-	 * 	  or trajectorySetpoint (Offboard attitude control).
-	 */
-	void generateAttitudeSetpoint();
-
-	/**
-	 * @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
-	 */
-	void generateRateSetpoint();
-
-	/**
-	 * @brief Check if the necessary parameters are set.
-	 * @return True if all checks pass.
-	 */
-	bool runSanityChecks();
-
-	// uORB subscriptions
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
-	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
-	uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)};
-	uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
-	vehicle_control_mode_s _vehicle_control_mode{};
-	rover_attitude_setpoint_s _rover_attitude_setpoint{};
-	rover_rate_setpoint_s _rover_rate_setpoint{};
-	offboard_control_mode_s _offboard_control_mode{};
-
-	// uORB publications
-	uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
-	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
-	uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
-	uORB::Publication<rover_attitude_status_s> _rover_attitude_status_pub{ORB_ID(rover_attitude_status)};
-
-	// Variables
-	hrt_abstime _timestamp{0};
-	hrt_abstime _last_rate_setpoint_update{0};
-	float _vehicle_yaw{0.f};
-	float _dt{0.f};
-	float _max_yaw_rate{0.f};
-	float _stab_yaw_setpoint{0.f}; // Yaw setpoint for stab mode, NAN if yaw rate is manually controlled [rad]
-	bool _prev_param_check_passed{true};
-
-	// Controllers
-	PID _pid_yaw;
-	SlewRateYaw<float> _adjusted_yaw_setpoint;
-
-	// Parameters
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RO_YAW_RATE_LIM>)  _param_ro_yaw_rate_limit,
-		(ParamFloat<px4::params::RO_YAW_P>)         _param_ro_yaw_p,
-		(ParamFloat<px4::params::RO_YAW_STICK_DZ>)  _param_ro_yaw_stick_dz
-	)
-};
@@ -1,40 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-px4_add_library(MecanumPosVelControl
-	MecanumPosVelControl.cpp
-)
-
-target_link_libraries(MecanumPosVelControl PUBLIC PID)
-target_link_libraries(MecanumPosVelControl PUBLIC pure_pursuit)
-target_include_directories(MecanumPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -1,426 +0,0 @@
-/****************************************************************************
- *
- *   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 "MecanumPosVelControl.hpp"
-
-using namespace time_literals;
-
-MecanumPosVelControl::MecanumPosVelControl(ModuleParams *parent) : ModuleParams(parent)
-{
-	_rover_rate_setpoint_pub.advertise();
-	_rover_throttle_setpoint_pub.advertise();
-	_rover_attitude_setpoint_pub.advertise();
-	_rover_velocity_status_pub.advertise();
-	updateParams();
-}
-
-void MecanumPosVelControl::updateParams()
-{
-	ModuleParams::updateParams();
-	_pid_speed_x.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f);
-	_pid_speed_x.setIntegralLimit(1.f);
-	_pid_speed_x.setOutputLimit(1.f);
-	_pid_speed_y.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f);
-	_pid_speed_y.setIntegralLimit(1.f);
-	_pid_speed_y.setOutputLimit(1.f);
-	_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
-
-	if (_param_ro_accel_limit.get() > FLT_EPSILON) {
-		_speed_x_setpoint.setSlewRate(_param_ro_accel_limit.get());
-		_speed_y_setpoint.setSlewRate(_param_ro_accel_limit.get());
-	}
-}
-
-void MecanumPosVelControl::updatePosControl()
-{
-	const hrt_abstime timestamp_prev = _timestamp;
-	_timestamp = hrt_absolute_time();
-	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
-
-	updateSubscriptions();
-
-	if ((_vehicle_control_mode.flag_control_position_enabled || _vehicle_control_mode.flag_control_velocity_enabled)
-	    && _vehicle_control_mode.flag_armed && runSanityChecks()) {
-		generateAttitudeSetpoint();
-
-		if (_param_ro_max_thr_speed.get() > FLT_EPSILON) { // Adjust speed setpoints if infeasible
-			if (_rover_steering_setpoint_sub.updated()) {
-				_rover_steering_setpoint_sub.copy(&_rover_steering_setpoint);
-			}
-
-			float speed_body_x_setpoint_normalized = math::interpolate<float>(_speed_body_x_setpoint,
-					-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
-
-			float speed_body_y_setpoint_normalized = math::interpolate<float>(_speed_body_y_setpoint,
-					-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
-
-			const float total_speed = fabsf(speed_body_x_setpoint_normalized) + fabsf(speed_body_y_setpoint_normalized) + fabsf(
-							  _rover_steering_setpoint.normalized_speed_diff);
-
-			if (total_speed > 1.f) {
-				const float theta = atan2f(fabsf(speed_body_y_setpoint_normalized), fabsf(speed_body_x_setpoint_normalized));
-				const float magnitude = (1.f - fabsf(_rover_steering_setpoint.normalized_speed_diff)) / (sinf(theta) + cosf(theta));
-				const float normalization = 1.f / (sqrtf(powf(speed_body_x_setpoint_normalized,
-								   2.f) + powf(speed_body_y_setpoint_normalized, 2.f)));
-				speed_body_x_setpoint_normalized *= magnitude * normalization;
-				speed_body_y_setpoint_normalized *= magnitude * normalization;
-				_speed_body_x_setpoint = math::interpolate<float>(speed_body_x_setpoint_normalized, -1.f, 1.f,
-							 -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
-				_speed_body_y_setpoint = math::interpolate<float>(speed_body_y_setpoint_normalized, -1.f, 1.f,
-							 -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get());
-			}
-		}
-
-		rover_throttle_setpoint_s rover_throttle_setpoint{};
-		rover_throttle_setpoint.timestamp = _timestamp;
-		_speed_body_x_setpoint = fabsf(_speed_body_x_setpoint) > _param_ro_speed_th.get() ? _speed_body_x_setpoint : 0.f;
-		_speed_body_y_setpoint = fabsf(_speed_body_y_setpoint) > _param_ro_speed_th.get() ? _speed_body_y_setpoint : 0.f;
-		rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_speed_x_setpoint, _pid_speed_x,
-				_speed_body_x_setpoint, _vehicle_speed_body_x, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
-				_param_ro_max_thr_speed.get(), _dt);
-		rover_throttle_setpoint.throttle_body_y = RoverControl::speedControl(_speed_y_setpoint, _pid_speed_y,
-				_speed_body_y_setpoint, _vehicle_speed_body_y, _param_ro_accel_limit.get(), _param_ro_decel_limit.get(),
-				_param_ro_max_thr_speed.get(), _dt);
-		_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
-
-	} else { // Reset controller and slew rate when position control is not active
-		_pid_speed_x.resetIntegral();
-		_speed_x_setpoint.setForcedValue(0.f);
-		_pid_speed_y.resetIntegral();
-		_speed_y_setpoint.setForcedValue(0.f);
-	}
-
-	// Publish position controller status (logging only)
-	rover_velocity_status_s rover_velocity_status;
-	rover_velocity_status.timestamp = _timestamp;
-	rover_velocity_status.measured_speed_body_x = _vehicle_speed_body_x;
-	rover_velocity_status.speed_body_x_setpoint = _speed_body_x_setpoint;
-	rover_velocity_status.adjusted_speed_body_x_setpoint = _speed_x_setpoint.getState();
-	rover_velocity_status.measured_speed_body_y = _vehicle_speed_body_y;
-	rover_velocity_status.speed_body_y_setpoint = _speed_body_y_setpoint;
-	rover_velocity_status.adjusted_speed_body_y_setpoint = _speed_y_setpoint.getState();
-	rover_velocity_status.pid_throttle_body_x_integral = _pid_speed_x.getIntegral();
-	rover_velocity_status.pid_throttle_body_y_integral = _pid_speed_y.getIntegral();
-	_rover_velocity_status_pub.publish(rover_velocity_status);
-}
-
-void MecanumPosVelControl::updateSubscriptions()
-{
-	if (_vehicle_control_mode_sub.updated()) {
-		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
-	}
-
-	if (_vehicle_attitude_sub.updated()) {
-		vehicle_attitude_s vehicle_attitude{};
-		_vehicle_attitude_sub.copy(&vehicle_attitude);
-		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
-		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
-	}
-
-	if (_vehicle_local_position_sub.updated()) {
-		vehicle_local_position_s vehicle_local_position{};
-		_vehicle_local_position_sub.copy(&vehicle_local_position);
-
-		if (!_global_ned_proj_ref.isInitialized()
-		    || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
-			_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
-							   vehicle_local_position.ref_timestamp);
-		}
-
-		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
-		Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
-		Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
-		_vehicle_speed_body_x = fabsf(velocity_in_body_frame(0)) > _param_ro_speed_th.get() ? velocity_in_body_frame(0) : 0.f;
-		_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
-	}
-
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-		_vehicle_status_sub.copy(&vehicle_status);
-		_nav_state = vehicle_status.nav_state;
-	}
-
-}
-
-void MecanumPosVelControl::generateAttitudeSetpoint()
-{
-	if (_vehicle_control_mode.flag_control_manual_enabled
-	    && _vehicle_control_mode.flag_control_position_enabled) { // Position Mode
-		manualPositionMode();
-
-	} else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Control
-		if (_offboard_control_mode_sub.updated()) {
-			_offboard_control_mode_sub.copy(&_offboard_control_mode);
-		}
-
-		if (_offboard_control_mode.position) {
-			offboardPositionMode();
-
-		} else if (_offboard_control_mode.velocity) {
-			offboardVelocityMode();
-		}
-
-	} else if (_vehicle_control_mode.flag_control_auto_enabled) { // Auto Mode
-		autoPositionMode();
-	}
-}
-
-void MecanumPosVelControl::manualPositionMode()
-{
-	manual_control_setpoint_s manual_control_setpoint{};
-
-	if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-		_speed_body_x_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
-					 -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
-		_speed_body_y_setpoint = math::interpolate<float>(manual_control_setpoint.roll,
-					 -1.f, 1.f, -_param_ro_speed_limit.get(), _param_ro_speed_limit.get());
-		const float yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
-						_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
-
-		if (fabsf(yaw_rate_setpoint) > FLT_EPSILON) { // Closed loop yaw rate control
-			_pos_ctl_yaw_setpoint = NAN;
-			rover_rate_setpoint_s rover_rate_setpoint{};
-			rover_rate_setpoint.timestamp = _timestamp;
-			rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint;
-			_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-
-		} else if ((fabsf(_speed_body_x_setpoint) > FLT_EPSILON
-			    || fabsf(_speed_body_y_setpoint) >
-			    FLT_EPSILON)) { // Course control if the steering input is zero (keep driving on a straight line)
-			const Vector3f velocity = Vector3f(_speed_body_x_setpoint, _speed_body_y_setpoint, 0.f);
-			const float velocity_magnitude_setpoint = velocity.norm();
-			const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized());
-			const Vector2f pos_ctl_course_direction_temp = Vector2f(pos_ctl_course_direction_local(0),
-					pos_ctl_course_direction_local(1));
-
-			// Reset course control if course direction change is above threshold
-			if (fabsf(asinf(pos_ctl_course_direction_temp % _pos_ctl_course_direction)) > _param_rm_course_ctl_th.get()) {
-				_pos_ctl_yaw_setpoint = NAN;
-			}
-
-			if (!PX4_ISFINITE(_pos_ctl_yaw_setpoint)) {
-				_pos_ctl_start_position_ned = _curr_pos_ned;
-				_pos_ctl_yaw_setpoint = _vehicle_yaw;
-				_pos_ctl_course_direction = pos_ctl_course_direction_temp;
-			}
-
-			// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
-			const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
-								2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
-			const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction;
-			const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned,
-						       _curr_pos_ned, velocity_magnitude_setpoint);
-			const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
-			_speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame);
-			_speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame);
-			rover_attitude_setpoint_s rover_attitude_setpoint{};
-			rover_attitude_setpoint.timestamp = _timestamp;
-			rover_attitude_setpoint.yaw_setpoint = _pos_ctl_yaw_setpoint;
-			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-
-		} else { // Reset course control and yaw rate setpoint
-			_pos_ctl_yaw_setpoint = NAN;
-			rover_rate_setpoint_s rover_rate_setpoint{};
-			rover_rate_setpoint.timestamp = _timestamp;
-			rover_rate_setpoint.yaw_rate_setpoint = 0.f;
-			_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-		}
-	}
-}
-
-void MecanumPosVelControl::offboardPositionMode()
-{
-	trajectory_setpoint_s trajectory_setpoint{};
-	_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-
-	// Translate trajectory setpoint to rover setpoints
-	const Vector2f target_waypoint_ned(trajectory_setpoint.position[0], trajectory_setpoint.position[1]);
-	const float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
-
-	if (target_waypoint_ned.isAllFinite() && distance_to_target > _param_nav_acc_rad.get()) {
-		const float velocity_magnitude_setpoint = math::min(math::trajectory::computeMaxSpeedFromDistance(
-					_param_ro_jerk_limit.get(),
-					_param_ro_decel_limit.get(), distance_to_target, 0.f), _param_ro_speed_limit.get());
-		const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned,
-					       _curr_pos_ned, fabsf(_speed_body_x_setpoint));
-		const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
-		_speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame);
-		_speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame);
-
-	} else {
-		_speed_body_x_setpoint = 0.f;
-		_speed_body_y_setpoint = 0.f;
-	}
-}
-
-void MecanumPosVelControl::offboardVelocityMode()
-{
-	trajectory_setpoint_s trajectory_setpoint{};
-	_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-
-	const Vector3f velocity_in_local_frame(trajectory_setpoint.velocity[0], trajectory_setpoint.velocity[1],
-					       trajectory_setpoint.velocity[2]);
-	const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
-
-	if (velocity_in_body_frame.isAllFinite()) {
-		_speed_body_x_setpoint = velocity_in_body_frame(0);
-		_speed_body_y_setpoint = velocity_in_body_frame(1);
-
-	} else {
-		_speed_body_x_setpoint = 0.f;
-		_speed_body_y_setpoint = 0.f;
-	}
-}
-
-void MecanumPosVelControl::autoPositionMode()
-{
-	updateAutoSubscriptions();
-
-	const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
-					       2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
-
-	if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival
-		_mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get();
-	}
-
-	const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
-					 _param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(),
-					 _nav_state);
-	const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
-				       velocity_magnitude);
-	const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
-	Vector2f desired_velocity(0.f, 0.f);
-	_speed_body_x_setpoint = _mission_finished ? 0.f : velocity_magnitude * cosf(bearing_setpoint_body_frame);
-	_speed_body_y_setpoint = _mission_finished ? 0.f : velocity_magnitude * sinf(bearing_setpoint_body_frame);
-	rover_attitude_setpoint_s rover_attitude_setpoint{};
-	rover_attitude_setpoint.timestamp = _timestamp;
-	rover_attitude_setpoint.yaw_setpoint = _auto_yaw;
-	_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-}
-
-void MecanumPosVelControl::updateAutoSubscriptions()
-{
-	if (_home_position_sub.updated()) {
-		home_position_s home_position{};
-		_home_position_sub.copy(&home_position);
-		_home_position = Vector2d(home_position.lat, home_position.lon);
-	}
-
-	if (_position_setpoint_triplet_sub.updated()) {
-		position_setpoint_triplet_s position_setpoint_triplet{};
-		_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
-
-		RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
-				_curr_pos_ned, _home_position, _global_ned_proj_ref);
-
-		_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
-
-		// Waypoint cruising speed
-		_auto_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
-				      position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
-
-		// Waypoint yaw setpoint
-		if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) {
-			_auto_yaw = position_setpoint_triplet.current.yaw;
-
-		} else {
-			_auto_yaw = _vehicle_yaw;
-		}
-	}
-
-	if (_mission_result_sub.updated()) {
-		mission_result_s mission_result{};
-		_mission_result_sub.copy(&mission_result);
-		_mission_finished = mission_result.finished;
-	}
-}
-
-float MecanumPosVelControl::calcVelocityMagnitude(const float auto_speed, const float distance_to_curr_wp,
-		const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed,
-		const float miss_spd_gain, const int nav_state)
-{
-	float velocity_magnitude{auto_speed};
-
-	if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON
-	    && miss_spd_gain > FLT_EPSILON) {
-		float max_velocity_magnitude = velocity_magnitude;
-
-		if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
-			max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
-						 max_decel, distance_to_curr_wp, 0.f);
-
-		} else if (PX4_ISFINITE(waypoint_transition_angle)) {
-			const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
-						      M_PI_F, 0.f, 1.f), 0.f, 1.f);
-			max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp,
-						 max_speed * (1.f - speed_reduction));
-		}
-
-		velocity_magnitude = math::constrain(max_velocity_magnitude, -auto_speed, auto_speed);
-	}
-
-	return velocity_magnitude;
-}
-
-bool MecanumPosVelControl::runSanityChecks()
-{
-	bool ret = true;
-
-	if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
-		ret = false;
-	}
-
-	if (_param_ro_speed_limit.get() < FLT_EPSILON) {
-		ret = false;
-
-		if (_prev_param_check_passed) {
-			events::send<float>(events::ID("mecanum_posVel_control_conf_invalid_speed_lim"), events::Log::Error,
-					    "Invalid configuration of necessary parameter RO_SPEED_LIM", _param_ro_speed_limit.get());
-		}
-
-	}
-
-	if (_param_ro_max_thr_speed.get() < FLT_EPSILON && _param_ro_speed_p.get() < FLT_EPSILON) {
-		ret = false;
-
-		if (_prev_param_check_passed) {
-			events::send<float, float>(events::ID("mecanum_posVel_control_conf_invalid_speed_control"), events::Log::Error,
-						   "Invalid configuration for speed control: Neither feed forward (RO_MAX_THR_SPD) nor feedback (RO_SPEED_P) is setup",
-						   _param_ro_max_thr_speed.get(),
-						   _param_ro_speed_p.get());
-		}
-	}
-
-	_prev_param_check_passed = ret;
-	return ret;
-}
@@ -1,233 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-#pragma once
-
-// PX4 includes
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/events.h>
-
-// Libraries
-#include <lib/rover_control/RoverControl.hpp>
-#include <lib/pid/PID.hpp>
-#include <matrix/matrix/math.hpp>
-#include <lib/slew_rate/SlewRate.hpp>
-#include <lib/pure_pursuit/PurePursuit.hpp>
-#include <lib/geo/geo.h>
-#include <math.h>
-
-// uORB includes
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/rover_rate_setpoint.h>
-#include <uORB/topics/rover_throttle_setpoint.h>
-#include <uORB/topics/rover_velocity_status.h>
-#include <uORB/topics/rover_attitude_setpoint.h>
-#include <uORB/topics/rover_steering_setpoint.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/trajectory_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/offboard_control_mode.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/position_controller_status.h>
-#include <uORB/topics/mission_result.h>
-#include <uORB/topics/home_position.h>
-
-using namespace matrix;
-
-/**
- * @brief Class for mecanum position/velocity control.
- */
-class MecanumPosVelControl : public ModuleParams
-{
-public:
-	/**
-	 * @brief Constructor for MecanumPosVelControl.
-	 * @param parent The parent ModuleParams object.
-	 */
-	MecanumPosVelControl(ModuleParams *parent);
-	~MecanumPosVelControl() = default;
-
-	/**
-	 * @brief Update position controller.
-	 */
-	void updatePosControl();
-
-protected:
-	/**
-	 * @brief Update the parameters of the module.
-	 */
-	void updateParams() override;
-
-private:
-	/**
-	 * @brief Update uORB subscriptions used in position controller.
-	 */
-	void updateSubscriptions();
-
-	/**
-	 * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Position Mode) or
-	 * 	  trajcetorySetpoint (Offboard position or velocity control) or
-	 * 	  positionSetpointTriplet (Auto Mode).
-	 */
-	void generateAttitudeSetpoint();
-
-	/**
-	 * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint or roverRateSetpoint from manualControlSetpoint.
-	 */
-	void manualPositionMode();
-
-	/**
-	 * @brief Generate and publish roverAttitudeSetpoint from position of trajectorySetpoint.
-	 */
-	void offboardPositionMode();
-
-	/**
-	 * @brief Generate and publish roverAttitudeSetpoint from velocity of trajectorySetpoint.
-	 */
-	void offboardVelocityMode();
-
-	/**
-	 * @brief Generate and publish roverThrottleSetpoint and roverAttitudeSetpoint from positionSetpointTriplet.
-	 */
-	void autoPositionMode();
-
-	/**
-	 * @brief Update uORB subscriptions used for auto modes.
-	 */
-	void updateAutoSubscriptions();
-
-	/**
-	 * @brief Calculate the velocity magnitude setpoint. During waypoint transition the speed is restricted to
-	 * Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN).
-	 * On straight lines it is based on a speed trajectory such that the rover will arrive at the next waypoint transition
-	 * with the desired waypoiny transition speed under consideration of the maximum deceleration and jerk.
-	 * @param auto_speed Default auto speed [m/s].
-	 * @param distance_to_curr_wp Distance to the current waypoint [m].
-	 * @param max_decel Maximum allowed deceleration [m/s^2].
-	 * @param max_jerk Maximum allowed jerk [m/s^3].
-	 * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
-	 * @param max_speed Maximum velocity magnitude setpoint [m/s]
-	 * @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition.
-	 * @param nav_state Vehicle navigation state
-	 * @return Velocity magnitude setpoint [m/s].
-	 */
-	float calcVelocityMagnitude(float auto_speed, float distance_to_curr_wp, float max_decel, float max_jerk,
-				    float waypoint_transition_angle, float max_speed, float miss_spd_gain, int nav_state);
-
-	/**
-	 * @brief Check if the necessary parameters are set.
-	 * @return True if all checks pass.
-	 */
-	bool runSanityChecks();
-
-	// uORB subscriptions
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
-	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
-	uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
-	uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
-	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
-	uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
-	vehicle_control_mode_s    _vehicle_control_mode{};
-	offboard_control_mode_s   _offboard_control_mode{};
-	rover_steering_setpoint_s _rover_steering_setpoint{};
-
-	// uORB publications
-	uORB::Publication<rover_rate_setpoint_s>        _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
-	uORB::Publication<rover_throttle_setpoint_s>    _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
-	uORB::Publication<rover_attitude_setpoint_s>    _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
-	uORB::Publication<rover_velocity_status_s>      _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
-	uORB::Publication<position_controller_status_s>	_position_controller_status_pub{ORB_ID(position_controller_status)};
-
-	// Variables
-	hrt_abstime _timestamp{0};
-	Quatf _vehicle_attitude_quaternion{};
-	Vector2d _home_position{};
-	Vector2f _curr_pos_ned{};
-	Vector2f _pos_ctl_course_direction{};
-	Vector2f _pos_ctl_start_position_ned{};
-	Vector2f _curr_wp_ned{};
-	Vector2f _prev_wp_ned{};
-	Vector2f _next_wp_ned{};
-	float _vehicle_speed_body_x{0.f};
-	float _vehicle_speed_body_y{0.f};
-	float _vehicle_yaw{0.f};
-	float _max_yaw_rate{0.f};
-	float _speed_body_x_setpoint{0.f};
-	float _speed_body_y_setpoint{0.f};
-	float _pos_ctl_yaw_setpoint{0.f}; // Yaw setpoint for manual position mode, NAN if yaw rate is manually controlled [rad]
-	float _dt{0.f};
-	float _auto_speed{0.f};
-	float _auto_yaw{0.f};
-	float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
-	int _nav_state{0};
-	bool _mission_finished{false};
-	bool _prev_param_check_passed{true};
-
-	// Controllers
-	PID _pid_speed_x;
-	PID _pid_speed_y;
-	SlewRate<float> _speed_x_setpoint;
-	SlewRate<float> _speed_y_setpoint;
-
-	// Class Instances
-	PurePursuit   _posctl_pure_pursuit{this}; // Pure pursuit library
-	MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RM_MISS_SPD_GAIN>) _param_rm_miss_spd_gain,
-		(ParamFloat<px4::params::RM_COURSE_CTL_TH>) _param_rm_course_ctl_th,
-		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
-		(ParamFloat<px4::params::RO_SPEED_P>) 	    _param_ro_speed_p,
-		(ParamFloat<px4::params::RO_SPEED_I>)       _param_ro_speed_i,
-		(ParamFloat<px4::params::RO_YAW_STICK_DZ>)  _param_ro_yaw_stick_dz,
-		(ParamFloat<px4::params::RO_ACCEL_LIM>)     _param_ro_accel_limit,
-		(ParamFloat<px4::params::RO_DECEL_LIM>)     _param_ro_decel_limit,
-		(ParamFloat<px4::params::RO_JERK_LIM>)      _param_ro_jerk_limit,
-		(ParamFloat<px4::params::RO_SPEED_LIM>)     _param_ro_speed_limit,
-		(ParamFloat<px4::params::RO_SPEED_TH>)      _param_ro_speed_th,
-		(ParamFloat<px4::params::PP_LOOKAHD_MAX>)   _param_pp_lookahd_max,
-		(ParamFloat<px4::params::RO_YAW_RATE_LIM>)  _param_ro_yaw_rate_limit,
-		(ParamFloat<px4::params::NAV_ACC_RAD>)      _param_nav_acc_rad
-
-	)
-};
@@ -1,190 +0,0 @@
-/****************************************************************************
- *
- *   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 "MecanumRateControl.hpp"
-
-using namespace time_literals;
-
-MecanumRateControl::MecanumRateControl(ModuleParams *parent) : ModuleParams(parent)
-{
-	_rover_rate_setpoint_pub.advertise();
-	_rover_throttle_setpoint_pub.advertise();
-	_rover_steering_setpoint_pub.advertise();
-	_rover_rate_status_pub.advertise();
-	updateParams();
-}
-
-void MecanumRateControl::updateParams()
-{
-	ModuleParams::updateParams();
-	_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
-	_max_yaw_accel = _param_ro_yaw_accel_limit.get() * M_DEG_TO_RAD_F;
-	_max_yaw_decel = _param_ro_yaw_decel_limit.get() * M_DEG_TO_RAD_F;
-	_pid_yaw_rate.setGains(_param_ro_yaw_rate_p.get(), _param_ro_yaw_rate_i.get(), 0.f);
-	_pid_yaw_rate.setIntegralLimit(1.f);
-	_pid_yaw_rate.setOutputLimit(1.f);
-	_adjusted_yaw_rate_setpoint.setSlewRate(_max_yaw_accel);
-}
-
-void MecanumRateControl::updateRateControl()
-{
-	const hrt_abstime timestamp_prev = _timestamp;
-	_timestamp = hrt_absolute_time();
-	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
-
-	if (_vehicle_control_mode_sub.updated()) {
-		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
-	}
-
-	if (_vehicle_angular_velocity_sub.updated()) {
-		vehicle_angular_velocity_s vehicle_angular_velocity{};
-		_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
-		_vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > _param_ro_yaw_rate_th.get() * M_DEG_TO_RAD_F ?
-				    vehicle_angular_velocity.xyz[2] : 0.f;
-	}
-
-	if (_vehicle_control_mode.flag_control_rates_enabled  && _vehicle_control_mode.flag_armed && runSanityChecks()) {
-		if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) {
-			generateRateSetpoint();
-		}
-
-		generateSteeringSetpoint();
-
-	} else { // Reset controller and slew rate when rate control is not active
-		_pid_yaw_rate.resetIntegral();
-		_adjusted_yaw_rate_setpoint.setForcedValue(0.f);
-	}
-
-	// Publish rate controller status (logging only)
-	rover_rate_status_s rover_rate_status;
-	rover_rate_status.timestamp = _timestamp;
-	rover_rate_status.measured_yaw_rate = _vehicle_yaw_rate;
-	rover_rate_status.adjusted_yaw_rate_setpoint = _adjusted_yaw_rate_setpoint.getState();
-	rover_rate_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral();
-	_rover_rate_status_pub.publish(rover_rate_status);
-
-}
-
-void MecanumRateControl::generateRateSetpoint()
-{
-	const bool acro_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
-				       && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled;
-
-	if (acro_mode_enabled && _manual_control_setpoint_sub.updated()) { // Acro Mode
-		manual_control_setpoint_s manual_control_setpoint{};
-
-		if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-			rover_throttle_setpoint_s rover_throttle_setpoint{};
-			rover_throttle_setpoint.timestamp = _timestamp;
-			rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
-			rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
-			_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
-			rover_rate_setpoint_s rover_rate_setpoint{};
-			rover_rate_setpoint.timestamp = _timestamp;
-			rover_rate_setpoint.yaw_rate_setpoint = math::interpolate<float> (manual_control_setpoint.yaw, -1.f, 1.f,
-								-_max_yaw_rate, _max_yaw_rate);
-			_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-		}
-
-	} else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard rate control
-		trajectory_setpoint_s trajectory_setpoint{};
-		_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-
-		if (_offboard_control_mode_sub.updated()) {
-			_offboard_control_mode_sub.copy(&_offboard_control_mode);
-		}
-
-		const bool offboard_rate_control = _offboard_control_mode.body_rate && !_offboard_control_mode.attitude;
-
-		if (offboard_rate_control && PX4_ISFINITE(trajectory_setpoint.yawspeed)) {
-			rover_rate_setpoint_s rover_rate_setpoint{};
-			rover_rate_setpoint.timestamp = _timestamp;
-			rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed;
-			_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-		}
-	}
-}
-
-void MecanumRateControl::generateSteeringSetpoint()
-{
-	if (_rover_rate_setpoint_sub.updated()) {
-		_rover_rate_setpoint_sub.copy(&_rover_rate_setpoint);
-
-	}
-
-	float speed_diff_normalized{0.f};
-
-	if (PX4_ISFINITE(_rover_rate_setpoint.yaw_rate_setpoint) && PX4_ISFINITE(_vehicle_yaw_rate)) {
-		const float yaw_rate_setpoint = fabsf(_rover_rate_setpoint.yaw_rate_setpoint) > _param_ro_yaw_rate_th.get() *
-						M_DEG_TO_RAD_F ?
-						_rover_rate_setpoint.yaw_rate_setpoint : 0.f;
-		speed_diff_normalized = RoverControl::rateControl(_adjusted_yaw_rate_setpoint, _pid_yaw_rate,
-					yaw_rate_setpoint, _vehicle_yaw_rate, _param_rm_max_thr_yaw_r.get(), _max_yaw_accel,
-					_max_yaw_decel, _param_rm_wheel_track.get(), _dt);
-	}
-
-	rover_steering_setpoint_s rover_steering_setpoint{};
-	rover_steering_setpoint.timestamp = _timestamp;
-	rover_steering_setpoint.normalized_speed_diff = speed_diff_normalized;
-	_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
-}
-
-bool MecanumRateControl::runSanityChecks()
-{
-	bool ret = true;
-
-	if (_param_ro_yaw_rate_limit.get() < FLT_EPSILON) {
-		ret = false;
-
-		if (_prev_param_check_passed) {
-			events::send<float>(events::ID("mecanum_rate_control_conf_invalid_yaw_rate_lim"), events::Log::Error,
-					    "Invalid configuration of necessary parameter RO_YAW_RATE_LIM", _param_ro_yaw_rate_limit.get());
-		}
-
-	}
-
-	if ((_param_rm_wheel_track.get() < FLT_EPSILON || _param_rm_max_thr_yaw_r.get() < FLT_EPSILON)
-	    && _param_ro_yaw_rate_p.get() < FLT_EPSILON) {
-		ret = false;
-
-		if (_prev_param_check_passed) {
-			events::send<float, float, float>(events::ID("mecanum_rate_control_conf_invalid_rate_control"), events::Log::Error,
-							  "Invalid configuration for rate control: Neither feed forward (RM_MAX_THR_YAW_R) nor feedback (RO_YAW_RATE_P) is setup",
-							  _param_rm_wheel_track.get(),
-							  _param_rm_max_thr_yaw_r.get(), _param_ro_yaw_rate_p.get());
-		}
-	}
-
-	_prev_param_check_passed = ret;
-	return ret;
-}
@@ -1,144 +0,0 @@
-/****************************************************************************
- *
- *   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.
- *
- ****************************************************************************/
-
-#pragma once
-
-// PX4 includes
-#include <px4_platform_common/module_params.h>
-#include <px4_platform_common/events.h>
-
-// Libraries
-#include <lib/rover_control/RoverControl.hpp>
-#include <lib/pid/PID.hpp>
-#include <lib/slew_rate/SlewRate.hpp>
-#include <math.h>
-
-// uORB includes
-#include <uORB/Publication.hpp>
-#include <uORB/Subscription.hpp>
-#include <uORB/topics/rover_rate_setpoint.h>
-#include <uORB/topics/rover_throttle_setpoint.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_angular_velocity.h>
-#include <uORB/topics/rover_steering_setpoint.h>
-#include <uORB/topics/rover_rate_status.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/offboard_control_mode.h>
-#include <uORB/topics/trajectory_setpoint.h>
-
-/**
- * @brief Class for mecanum rate control.
- */
-class MecanumRateControl : public ModuleParams
-{
-public:
-	/**
-	 * @brief Constructor for MecanumRateControl.
-	 * @param parent The parent ModuleParams object.
-	 */
-	MecanumRateControl(ModuleParams *parent);
-	~MecanumRateControl() = default;
-
-	/**
-	 * @brief Update rate controller.
-	 */
-	void updateRateControl();
-
-protected:
-	/**
-	 * @brief Update the parameters of the module.
-	 */
-	void updateParams() override;
-
-private:
-
-	/**
-	 * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode)
-	 * 	  or trajectorySetpoint (Offboard rate control).
-	 */
-	void generateRateSetpoint();
-
-	/**
-	 * @brief Generate and publish roverSteeringSetpoint from RoverRateSetpoint.
-	 */
-	void generateSteeringSetpoint();
-
-	/**
-	 * @brief Check if the necessary parameters are set.
-	 * @return True if all checks pass.
-	 */
-	bool runSanityChecks();
-
-	// uORB subscriptions
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
-	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
-	uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
-	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
-	vehicle_control_mode_s  _vehicle_control_mode{};
-	offboard_control_mode_s _offboard_control_mode{};
-	rover_rate_setpoint_s   _rover_rate_setpoint{};
-
-	// uORB publications
-	uORB::Publication<rover_rate_setpoint_s>     _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
-	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
-	uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
-	uORB::Publication<rover_rate_status_s>       _rover_rate_status_pub{ORB_ID(rover_rate_status)};
-
-	// Variables
-	hrt_abstime _timestamp{0};
-	float _max_yaw_rate{0.f};
-	float _max_yaw_accel{0.f};
-	float _max_yaw_decel{0.f};
-	float _vehicle_yaw_rate{0.f};
-	float _dt{0.f}; // Time since last update [s].
-	bool _prev_param_check_passed{true};
-
-	// Controllers
-	PID _pid_yaw_rate;
-	SlewRate<float> _adjusted_yaw_rate_setpoint{0.f};
-
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::RM_WHEEL_TRACK>)   _param_rm_wheel_track,
-		(ParamFloat<px4::params::RM_MAX_THR_YAW_R>) _param_rm_max_thr_yaw_r,
-		(ParamFloat<px4::params::RO_YAW_RATE_LIM>)  _param_ro_yaw_rate_limit,
-		(ParamFloat<px4::params::RO_YAW_RATE_TH>)   _param_ro_yaw_rate_th,
-		(ParamFloat<px4::params::RO_YAW_RATE_P>)    _param_ro_yaw_rate_p,
-		(ParamFloat<px4::params::RO_YAW_RATE_I>)    _param_ro_yaw_rate_i,
-		(ParamFloat<px4::params::RO_YAW_ACCEL_LIM>) _param_ro_yaw_accel_limit,
-		(ParamFloat<px4::params::RO_YAW_DECEL_LIM>) _param_ro_yaw_decel_limit
-	)
-};
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -32,16 +32,15 @@
 ****************************************************************************/

 #include "RoverMecanum.hpp"
-
+using namespace matrix;
 using namespace time_literals;

 RoverMecanum::RoverMecanum() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
 {
-	_rover_throttle_setpoint_pub.advertise();
-	_rover_steering_setpoint_pub.advertise();
 	updateParams();
+	_rover_mecanum_setpoint_pub.advertise();
 }

 bool RoverMecanum::init()
@@ -54,117 +53,266 @@ void RoverMecanum::updateParams()
 {
 	ModuleParams::updateParams();

-	if (_param_ro_accel_limit.get() > FLT_EPSILON && _param_ro_max_thr_speed.get() > FLT_EPSILON) {
-		_throttle_body_x_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get());
-		_throttle_body_y_setpoint.setSlewRate(_param_ro_accel_limit.get() / _param_ro_max_thr_speed.get());
-	}
+	_max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F;
 }

 void RoverMecanum::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+		return;
+	}
+
+	updateSubscriptions();
+
+	// Generate and publish attitude and velocity setpoints
+	hrt_abstime timestamp = hrt_absolute_time();
+
+	switch (_nav_state) {
+	case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
+			manual_control_setpoint_s manual_control_setpoint{};
+			rover_mecanum_setpoint_s rover_mecanum_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_mecanum_setpoint.timestamp = timestamp;
+				rover_mecanum_setpoint.forward_speed_setpoint = NAN;
+				rover_mecanum_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
+				rover_mecanum_setpoint.lateral_speed_setpoint = NAN;
+				rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll;
+				rover_mecanum_setpoint.yaw_rate_setpoint = NAN;
+
+				if (_max_yaw_rate > FLT_EPSILON && _param_rm_max_thr_yaw_r.get() > FLT_EPSILON) {
+					const float scaled_yaw_rate_input = math::interpolate<float>(manual_control_setpoint.yaw,
+									    -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+					const float speed_diff = scaled_yaw_rate_input * _param_rm_wheel_track.get() / 2.f;
+					rover_mecanum_setpoint.speed_diff_setpoint_normalized = math::interpolate<float>(speed_diff,
+							-_param_rm_max_thr_yaw_r.get(), _param_rm_max_thr_yaw_r.get(), -1.f, 1.f);
+
+				} else {
+					rover_mecanum_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.yaw;
+
+				}
+
+				rover_mecanum_setpoint.yaw_setpoint = NAN;
+				_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
+
+			}
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_ACRO: {
+			manual_control_setpoint_s manual_control_setpoint{};
+			rover_mecanum_setpoint_s rover_mecanum_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_mecanum_setpoint.timestamp = timestamp;
+				rover_mecanum_setpoint.forward_speed_setpoint = NAN;
+				rover_mecanum_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
+				rover_mecanum_setpoint.lateral_speed_setpoint = NAN;
+				rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll;
+				rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.yaw,
+						-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN;
+				rover_mecanum_setpoint.yaw_setpoint = NAN;
+				_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
+			}
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_STAB: {
+			manual_control_setpoint_s manual_control_setpoint{};
+			rover_mecanum_setpoint_s rover_mecanum_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_mecanum_setpoint.timestamp = timestamp;
+				rover_mecanum_setpoint.forward_speed_setpoint = NAN;
+				rover_mecanum_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
+				rover_mecanum_setpoint.lateral_speed_setpoint = NAN;
+				rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll;
+				rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
+						STICK_DEADZONE),
+						-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN;
+				rover_mecanum_setpoint.yaw_setpoint = NAN;
+
+				if (fabsf(rover_mecanum_setpoint.yaw_rate_setpoint) > FLT_EPSILON
+				    || (fabsf(rover_mecanum_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON
+					&& fabsf(rover_mecanum_setpoint.lateral_speed_setpoint_normalized) < FLT_EPSILON)) { // Closed loop yaw rate control
+					_yaw_ctl = false;
+
+				} else { // Closed loop yaw control
+
+					if (!_yaw_ctl) {
+						_desired_yaw = _vehicle_yaw;
+						_yaw_ctl = true;
+					}
+
+					rover_mecanum_setpoint.yaw_setpoint = _desired_yaw;
+					rover_mecanum_setpoint.yaw_rate_setpoint = NAN;
+				}
+
+				_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
+			}
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_POSCTL: {
+			manual_control_setpoint_s manual_control_setpoint{};
+			rover_mecanum_setpoint_s rover_mecanum_setpoint{};
+
+			if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
+				rover_mecanum_setpoint.timestamp = timestamp;
+				rover_mecanum_setpoint.forward_speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
+						-1.f, 1.f, -_param_rm_max_speed.get(), _param_rm_max_speed.get());;
+				rover_mecanum_setpoint.forward_speed_setpoint_normalized = NAN;
+				rover_mecanum_setpoint.lateral_speed_setpoint = math::interpolate<float>(manual_control_setpoint.roll,
+						-1.f, 1.f, -_param_rm_max_speed.get(), _param_rm_max_speed.get());
+				rover_mecanum_setpoint.lateral_speed_setpoint_normalized = NAN;
+				rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
+						STICK_DEADZONE),
+						-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+				rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN;
+				rover_mecanum_setpoint.yaw_setpoint = NAN;
+
+				// Reset cruise control if the manual input changes
+				if (_yaw_ctl && (!(fabsf(manual_control_setpoint.throttle - _prev_throttle) > STICK_DEADZONE)
+						 || !(fabsf(manual_control_setpoint.roll - _prev_roll) > STICK_DEADZONE))) {
+					_yaw_ctl = false;
+					_prev_throttle = manual_control_setpoint.throttle;
+					_prev_roll = manual_control_setpoint.roll;
+
+				} else if (!_yaw_ctl) {
+					_prev_throttle = manual_control_setpoint.throttle;
+					_prev_roll = manual_control_setpoint.roll;
+				}
+
+
+				if (fabsf(rover_mecanum_setpoint.yaw_rate_setpoint) > FLT_EPSILON
+				    || (fabsf(rover_mecanum_setpoint.forward_speed_setpoint) < FLT_EPSILON
+					&& fabsf(rover_mecanum_setpoint.lateral_speed_setpoint) < FLT_EPSILON)) { // Closed loop yaw rate control
+					rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.yaw,
+							-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
+					rover_mecanum_setpoint.yaw_setpoint = NAN;
+					_yaw_ctl = false;
+
+				} else { // Cruise control
+					const Vector3f velocity = Vector3f(rover_mecanum_setpoint.forward_speed_setpoint,
+									   rover_mecanum_setpoint.lateral_speed_setpoint, 0.f);
+					const float desired_velocity_magnitude = velocity.norm();
+
+					if (!_yaw_ctl) {
+						_desired_yaw = _vehicle_yaw;
+						_yaw_ctl = true;
+						_pos_ctl_start_position_ned = _curr_pos_ned;
+						const Vector3f pos_ctl_course_direction_local = _vehicle_attitude_quaternion.rotateVector(velocity.normalized());
+						_pos_ctl_course_direction = Vector2f(pos_ctl_course_direction_local(0), pos_ctl_course_direction_local(1));
+
+					}
+
+					// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
+					const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
+										2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
+					const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction;
+					const float desired_heading = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned,
+								      _curr_pos_ned, desired_velocity_magnitude);
+					const float heading_error = matrix::wrap_pi(desired_heading - _vehicle_yaw);
+					const Vector2f desired_velocity = desired_velocity_magnitude * Vector2f(cosf(heading_error), sinf(heading_error));
+					rover_mecanum_setpoint.forward_speed_setpoint = desired_velocity(0);
+					rover_mecanum_setpoint.lateral_speed_setpoint = desired_velocity(1);
+					rover_mecanum_setpoint.yaw_setpoint = _desired_yaw;
+					rover_mecanum_setpoint.yaw_rate_setpoint = NAN;
+				}
+
+				_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
+			}
+		} break;
+
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
+	case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
+		_rover_mecanum_guidance.computeGuidance(_vehicle_yaw, _nav_state);
+		break;
+
+	default: // Unimplemented nav states will stop the rover
+		rover_mecanum_setpoint_s rover_mecanum_setpoint{};
+		rover_mecanum_setpoint.timestamp = timestamp;
+		rover_mecanum_setpoint.forward_speed_setpoint = NAN;
+		rover_mecanum_setpoint.forward_speed_setpoint_normalized = 0.f;
+		rover_mecanum_setpoint.lateral_speed_setpoint = NAN;
+		rover_mecanum_setpoint.lateral_speed_setpoint_normalized = 0.f;
+		rover_mecanum_setpoint.yaw_rate_setpoint = NAN;
+		rover_mecanum_setpoint.speed_diff_setpoint_normalized = 0.f;
+		rover_mecanum_setpoint.yaw_setpoint = NAN;
+		_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
+		break;
+	}
+
+	if (!_armed) { // Reset when disarmed
+		_rover_mecanum_control.resetControllers();
+		_yaw_ctl = false;
+	}
+
+	_rover_mecanum_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed,
+			_vehicle_lateral_speed);
+
+}
+
+void RoverMecanum::updateSubscriptions()
 {
 	if (_parameter_update_sub.updated()) {
+		parameter_update_s parameter_update;
+		_parameter_update_sub.copy(&parameter_update);
 		updateParams();
 	}

-	const hrt_abstime timestamp_prev = _timestamp;
-	_timestamp = hrt_absolute_time();
-	_dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+	if (_vehicle_status_sub.updated()) {
+		vehicle_status_s vehicle_status{};
+		_vehicle_status_sub.copy(&vehicle_status);

-	_mecanum_pos_vel_control.updatePosControl();
-	_mecanum_att_control.updateAttControl();
-	_mecanum_rate_control.updateRateControl();
+		if (vehicle_status.nav_state != _nav_state) {
+			_rover_mecanum_control.resetControllers();
+			_yaw_ctl = false;

-	if (_vehicle_control_mode_sub.updated()) {
-		_vehicle_control_mode_sub.copy(&_vehicle_control_mode);
+			if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
+			    || vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
+				_rover_mecanum_guidance.setDesiredYaw(_vehicle_yaw);
+			}
+		}
+
+		_nav_state = vehicle_status.nav_state;
+		_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
 	}

-	const bool full_manual_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
-					      && !_vehicle_control_mode.flag_control_position_enabled && !_vehicle_control_mode.flag_control_attitude_enabled
-					      && !_vehicle_control_mode.flag_control_rates_enabled;
-
-	if (full_manual_mode_enabled) { // Manual mode
-		generateSteeringSetpoint();
+	if (_vehicle_local_position_sub.updated()) {
+		vehicle_local_position_s vehicle_local_position{};
+		_vehicle_local_position_sub.copy(&vehicle_local_position);
+		_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
+		Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
+		Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
+		// Apply threshold to the velocity measurement to cut off measurement noise when standing still
+		_vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f;
+		_vehicle_lateral_speed = fabsf(velocity_in_body_frame(1)) > SPEED_THRESHOLD ? velocity_in_body_frame(1) : 0.f;
 	}

-	if (_vehicle_control_mode.flag_armed) {
-		generateActuatorSetpoint();
+	if (_vehicle_angular_velocity_sub.updated()) {
+		vehicle_angular_velocity_s vehicle_angular_velocity{};
+		_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
+
+		// Apply threshold to the yaw rate measurement if the rover is standing still to avoid stuttering due to closed loop yaw(rate) control
+		if ((fabsf(_vehicle_forward_speed) > FLT_EPSILON && fabsf(_vehicle_lateral_speed) > FLT_EPSILON)
+		    || fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD) {
+			_vehicle_yaw_rate = vehicle_angular_velocity.xyz[2];
+
+		} else {
+			_vehicle_yaw_rate = 0.f;
+		}

 	}

-}
-
-void RoverMecanum::generateSteeringSetpoint()
-{
-	manual_control_setpoint_s manual_control_setpoint{};
-
-	if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
-		rover_steering_setpoint_s rover_steering_setpoint{};
-		rover_steering_setpoint.timestamp = _timestamp;
-		rover_steering_setpoint.normalized_speed_diff = manual_control_setpoint.yaw;
-		_rover_steering_setpoint_pub.publish(rover_steering_setpoint);
-		rover_throttle_setpoint_s rover_throttle_setpoint{};
-		rover_throttle_setpoint.timestamp = _timestamp;
-		rover_throttle_setpoint.throttle_body_x = manual_control_setpoint.throttle;
-		rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
-		_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);
-	}
-}
-
-void RoverMecanum::generateActuatorSetpoint()
-{
-	if (_rover_throttle_setpoint_sub.updated()) {
-		_rover_throttle_setpoint_sub.copy(&_rover_throttle_setpoint);
+	if (_vehicle_attitude_sub.updated()) {
+		vehicle_attitude_s vehicle_attitude{};
+		_vehicle_attitude_sub.copy(&vehicle_attitude);
+		_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
+		_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
 	}

-	if (_actuator_motors_sub.updated()) {
-		actuator_motors_s actuator_motors{};
-		_actuator_motors_sub.copy(&actuator_motors);
-		_current_throttle_body_x = (actuator_motors.control[0] + actuator_motors.control[1]) / 2.f;
-		_current_throttle_body_y = (actuator_motors.control[2] - actuator_motors.control[0]) / 2.f;
-	}
-
-	if (_rover_steering_setpoint_sub.updated()) {
-		_rover_steering_setpoint_sub.copy(&_rover_steering_setpoint);
-	}
-
-	const float throttle_body_x = RoverControl::throttleControl(_throttle_body_x_setpoint,
-				      _rover_throttle_setpoint.throttle_body_x, _current_throttle_body_x, _param_ro_accel_limit.get(),
-				      _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt);
-	const float throttle_body_y = RoverControl::throttleControl(_throttle_body_y_setpoint,
-				      _rover_throttle_setpoint.throttle_body_y, _current_throttle_body_y, _param_ro_accel_limit.get(),
-				      _param_ro_decel_limit.get(), _param_ro_max_thr_speed.get(), _dt);
-	actuator_motors_s actuator_motors{};
-	actuator_motors.reversible_flags = _param_r_rev.get();
-	computeInverseKinematics(throttle_body_x, throttle_body_y,
-				 _rover_steering_setpoint.normalized_speed_diff).copyTo(actuator_motors.control);
-	actuator_motors.timestamp = _timestamp;
-	_actuator_motors_pub.publish(actuator_motors);
-
-
-}
-
-Vector4f RoverMecanum::computeInverseKinematics(float throttle_body_x, float throttle_body_y,
-		const float speed_diff_normalized)
-{
-	const float total_speed = fabsf(throttle_body_x) + fabsf(throttle_body_y) + fabsf(speed_diff_normalized);
-
-	if (total_speed > 1.f) { // Adjust speed setpoints if infeasible
-		const float theta = atan2f(fabsf(throttle_body_y), fabsf(throttle_body_x));
-		const float magnitude = (1.f - fabsf(speed_diff_normalized)) / (sinf(theta) + cosf(theta));
-		const float normalization = 1.f / (sqrtf(powf(throttle_body_x, 2.f) + powf(throttle_body_y, 2.f)));
-		throttle_body_x *= magnitude * normalization;
-		throttle_body_y *= magnitude * normalization;
-
-	}
-
-	// Calculate motor commands
-	const float input_data[3] = {throttle_body_x, throttle_body_y, speed_diff_normalized};
-	const Matrix<float, 3, 1> input(input_data);
-	const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f};
-	const Matrix<float, 4, 3> m(m_data);
-	const Vector4f motor_commands = m * input;
-
-	return motor_commands;
 }

 int RoverMecanum::task_spawn(int argc, char *argv[])
@@ -192,7 +340,7 @@ int RoverMecanum::task_spawn(int argc, char *argv[])

 int RoverMecanum::custom_command(int argc, char *argv[])
 {
-	return print_usage("unknown command");
+	return print_usage("unk_timestampn command");
 }

 int RoverMecanum::print_usage(const char *reason)
@@ -204,7 +352,7 @@ int RoverMecanum::print_usage(const char *reason)
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
 ### Description
-Rover mecanum module.
+Rover Mecanum controller.
 )DESCR_STR");

 	PRINT_MODULE_USAGE_NAME("rover_mecanum", "controller");
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -39,34 +39,41 @@
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
-
-// Libraries
-#include <lib/rover_control/RoverControl.hpp>
-#include <lib/slew_rate/SlewRate.hpp>
+#include <lib/pure_pursuit/PurePursuit.hpp>

 // uORB includes
-#include <uORB/Subscription.hpp>
 #include <uORB/Publication.hpp>
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/actuator_motors.h>
-#include <uORB/topics/rover_steering_setpoint.h>
-#include <uORB/topics/rover_throttle_setpoint.h>
-#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_angular_velocity.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/rover_mecanum_setpoint.h>
+
+// Standard libraries
+#include <lib/pid/PID.hpp>
+#include <matrix/matrix/math.hpp>

 // Local includes
-#include "MecanumRateControl/MecanumRateControl.hpp"
-#include "MecanumAttControl/MecanumAttControl.hpp"
-#include "MecanumPosVelControl/MecanumPosVelControl.hpp"
+#include "RoverMecanumGuidance/RoverMecanumGuidance.hpp"
+#include "RoverMecanumControl/RoverMecanumControl.hpp"
+
+// Constants
+static constexpr float YAW_RATE_THRESHOLD =
+	0.02f; // [rad/s] Threshold for the yaw rate measurement to avoid stuttering when the rover is standing still
+static constexpr float SPEED_THRESHOLD =
+	0.1f; // [m/s] Threshold for the speed measurement to cut off measurement noise when the rover is standing still
+static constexpr float STICK_DEADZONE =
+	0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
+
+using namespace time_literals;

 class RoverMecanum : public ModuleBase<RoverMecanum>, public ModuleParams,
 	public px4::ScheduledWorkItem
 {
 public:
-	/**
-	 * @brief Constructor for RoverMecanum
-	 */
 	RoverMecanum();
 	~RoverMecanum() override = default;

@@ -82,69 +89,54 @@ public:
 	bool init();

 protected:
-	/**
-	 * @brief Update the parameters of the module.
-	 */
 	void updateParams() override;

 private:
 	void Run() override;

 	/**
-	 * @brief Generate and publish roverSteeringSetpoint from manualControlSetpoint (Manual Mode).
+	 * @brief Update uORB subscriptions.
 	 */
-	void generateSteeringSetpoint();
+	void updateSubscriptions();

-	/**
-	 * @brief Generate and publish actuatorMotors setpoints from roverThrottleSetpoint/roverSteeringSetpoint.
-	 */
-	void generateActuatorSetpoint();
-
-	/**
-	 * @brief Compute normalized motor commands based on normalized setpoints.
-	 * @param throttle_body_x Normalized speed in body x direction [-1, 1].
-	 * @param throttle_body_y Normalized speed in body y direction [-1, 1].
-	 * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1].
-	 * @return Motor speeds [-1, 1].
-	 */
-	Vector4f computeInverseKinematics(float throttle_body_x, float throttle_body_y, const float speed_diff_normalized);
-
-	// uORB subscriptions
-	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
-	uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
-	uORB::Subscription _rover_throttle_setpoint_sub{ORB_ID(rover_throttle_setpoint)};
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	// uORB Subscriptions
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
-	vehicle_control_mode_s    _vehicle_control_mode{};
-	rover_steering_setpoint_s _rover_steering_setpoint{};
-	rover_throttle_setpoint_s _rover_throttle_setpoint{};
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
+	uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};

-	// uORB publications
-	uORB::PublicationMulti<actuator_motors_s>    _actuator_motors_pub{ORB_ID(actuator_motors)};
-	uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
-	uORB::Publication<rover_steering_setpoint_s> _rover_steering_setpoint_pub{ORB_ID(rover_steering_setpoint)};
+	// uORB Publications
+	uORB::Publication<rover_mecanum_setpoint_s> _rover_mecanum_setpoint_pub{ORB_ID(rover_mecanum_setpoint)};

-	// Class instances
-	MecanumRateControl   _mecanum_rate_control{this};
-	MecanumAttControl    _mecanum_att_control{this};
-	MecanumPosVelControl _mecanum_pos_vel_control{this};
+	// Instances
+	RoverMecanumGuidance _rover_mecanum_guidance{this};
+	RoverMecanumControl _rover_mecanum_control{this};
+	PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library

 	// Variables
-	hrt_abstime _timestamp{0};
-	float _dt{0.f};
-	float _current_throttle_body_x{0.f};
-	float _current_throttle_body_y{0.f};
+	matrix::Quatf _vehicle_attitude_quaternion{};
+	float _vehicle_yaw_rate{0.f};
+	float _vehicle_forward_speed{0.f};
+	float _vehicle_lateral_speed{0.f};
+	float _vehicle_yaw{0.f};
+	float _max_yaw_rate{0.f};
+	int _nav_state{0};
+	bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in position mode
+	float _desired_yaw{0.f}; // Yaw setpoint for position mode
+	Vector2f _pos_ctl_start_position_ned{};
+	Vector2f _pos_ctl_course_direction{};
+	Vector2f _curr_pos_ned{};
+	float _prev_throttle{0.f};
+	float _prev_roll{0.f};
+	bool _armed{false};

-	// Controllers
-	SlewRate<float> _throttle_body_x_setpoint{0.f};
-	SlewRate<float> _throttle_body_y_setpoint{0.f};
-
-	// Parameters
 	DEFINE_PARAMETERS(
-		(ParamInt<px4::params::CA_R_REV>)           _param_r_rev,
-		(ParamFloat<px4::params::RO_ACCEL_LIM>)     _param_ro_accel_limit,
-		(ParamFloat<px4::params::RO_DECEL_LIM>)     _param_ro_decel_limit,
-		(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
+		(ParamFloat<px4::params::RM_MAX_SPEED>)    _param_rm_max_speed,
+		(ParamFloat<px4::params::RM_WHEEL_TRACK>)   _param_rm_wheel_track,
+		(ParamFloat<px4::params::RM_MAX_THR_YAW_R>) _param_rm_max_thr_yaw_r,
+		(ParamFloat<px4::params::RM_MAX_YAW_RATE>)  _param_rm_max_yaw_rate,
+		(ParamFloat<px4::params::PP_LOOKAHD_MAX>)   _param_pp_lookahd_max
 	)
 };
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,9 +31,9 @@
 #
 ############################################################################

-px4_add_library(MecanumRateControl
-	MecanumRateControl.cpp
+px4_add_library(RoverMecanumControl
+	RoverMecanumControl.cpp
 )

-target_link_libraries(MecanumRateControl PUBLIC PID)
-target_include_directories(MecanumRateControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_link_libraries(RoverMecanumControl PUBLIC PID)
+target_include_directories(RoverMecanumControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -0,0 +1,323 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "RoverMecanumControl.hpp"
+
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+
+RoverMecanumControl::RoverMecanumControl(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+	_rover_mecanum_status_pub.advertise();
+}
+
+void RoverMecanumControl::updateParams()
+{
+	ModuleParams::updateParams();
+
+	_pid_yaw_rate.setGains(_param_rm_yaw_rate_p.get(), _param_rm_yaw_rate_i.get(), 0.f);
+	_pid_yaw_rate.setIntegralLimit(1.f);
+	_pid_yaw_rate.setOutputLimit(1.f);
+
+	_pid_forward_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f);
+	_pid_forward_throttle.setIntegralLimit(1.f);
+	_pid_forward_throttle.setOutputLimit(1.f);
+
+	_pid_lateral_throttle.setGains(_param_rm_p_gain_speed.get(), _param_rm_i_gain_speed.get(), 0.f);
+	_pid_lateral_throttle.setIntegralLimit(1.f);
+	_pid_lateral_throttle.setOutputLimit(1.f);
+
+	_max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F;
+	_max_yaw_accel = _param_rm_max_yaw_accel.get() * M_DEG_TO_RAD_F;
+	_pid_yaw.setGains(_param_rm_p_gain_yaw.get(), _param_rm_i_gain_yaw.get(), 0.f);
+	_pid_yaw.setIntegralLimit(_max_yaw_rate);
+	_pid_yaw.setOutputLimit(_max_yaw_rate);
+
+	// Update slew rates
+	if (_max_yaw_rate > FLT_EPSILON) {
+		_yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate);
+	}
+
+	if (_max_yaw_accel > FLT_EPSILON) {
+		_yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel);
+	}
+}
+
+void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate,
+		const float vehicle_forward_speed, const float vehicle_lateral_speed)
+{
+	// Timestamps
+	hrt_abstime timestamp_prev = _timestamp;
+	_timestamp = hrt_absolute_time();
+	const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
+
+	// Update mecanum setpoint
+	_rover_mecanum_setpoint_sub.update(&_rover_mecanum_setpoint);
+
+	// Closed loop yaw control
+	if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) {
+		_yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint), dt);
+		_rover_mecanum_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState());
+		_pid_yaw.setSetpoint(
+			matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() -
+					vehicle_yaw));  // error as setpoint to take care of wrapping
+		_rover_mecanum_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt);
+		_rover_mecanum_status.clyaw_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint;
+
+	} else {
+		_pid_yaw.resetIntegral();
+		_yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw);
+	}
+
+	// Yaw rate control
+	float speed_diff_normalized{0.f};
+
+	if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control
+		speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate,
+					_param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit,
+					_pid_yaw_rate, false);
+		_rover_mecanum_status.adjusted_yaw_rate_setpoint = _yaw_rate_with_accel_limit.getState();
+
+	} else { // Use normalized setpoint
+		speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.speed_diff_setpoint_normalized,
+					vehicle_yaw_rate,
+					_param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit,
+					_pid_yaw_rate, true);
+	}
+
+	// Speed control
+	float forward_speed_normalized{0.f};
+	float lateral_speed_normalized{0.f};
+
+	if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint)
+	    && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint)) { // Closed loop speed control
+
+		if (_param_rm_max_thr_spd.get() > FLT_EPSILON) {
+
+			forward_speed_normalized = math::interpolate<float>(_rover_mecanum_setpoint.forward_speed_setpoint,
+						   -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f);
+
+			lateral_speed_normalized = math::interpolate<float>(_rover_mecanum_setpoint.lateral_speed_setpoint,
+						   -_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f);
+
+			const float total_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized) + fabsf(
+							  speed_diff_normalized);
+
+			if (total_speed > 1.f) { // Adjust speed setpoints if infeasible
+				const float theta = atan2f(fabsf(lateral_speed_normalized), fabsf(forward_speed_normalized));
+				const float magnitude = (1.f - fabsf(speed_diff_normalized)) / (sinf(theta) + cosf(theta));
+				const float normalization = 1.f / (sqrtf(powf(forward_speed_normalized, 2.f) + powf(lateral_speed_normalized, 2.f)));
+				forward_speed_normalized *= magnitude * normalization;
+				lateral_speed_normalized *= magnitude * normalization;
+				_rover_mecanum_setpoint.forward_speed_setpoint = math::interpolate<float>(forward_speed_normalized, -1.f, 1.f,
+						-_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get());
+				_rover_mecanum_setpoint.lateral_speed_setpoint = math::interpolate<float>(lateral_speed_normalized, -1.f, 1.f,
+						-_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get());
+			}
+		}
+
+		forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint,
+					   vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle,
+					   _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false);
+		lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint,
+					   vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle,
+					   _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false);
+		_rover_mecanum_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState();
+		_rover_mecanum_status.adjusted_lateral_speed_setpoint = _lateral_speed_setpoint_with_accel_limit.getState();
+
+
+	} else if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized)
+		   && PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized)) { // Use normalized setpoint
+		forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint_normalized,
+					   vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle,
+					   _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true);
+		lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized,
+					   vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle,
+					   _param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true);
+
+	}
+
+	// Publish rover mecanum status (logging)
+	_rover_mecanum_status.timestamp = _timestamp;
+	_rover_mecanum_status.measured_forward_speed = vehicle_forward_speed;
+	_rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed;
+	_rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate;
+	_rover_mecanum_status.measured_yaw = vehicle_yaw;
+	_rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral();
+	_rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral();
+	_rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral();
+	_rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral();
+	_rover_mecanum_status_pub.publish(_rover_mecanum_status);
+
+	// Publish to motors
+	actuator_motors_s actuator_motors{};
+	actuator_motors.reversible_flags = _param_r_rev.get();
+	computeInverseKinematics(forward_speed_normalized, lateral_speed_normalized,
+				 speed_diff_normalized).copyTo(actuator_motors.control);
+	actuator_motors.timestamp = _timestamp;
+	_actuator_motors_pub.publish(actuator_motors);
+
+}
+
+float RoverMecanumControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate,
+		const float max_thr_yaw_r,
+		const float max_yaw_accel, const float wheel_track, const float dt, SlewRate<float> &yaw_rate_with_accel_limit,
+		PID &pid_yaw_rate, const bool normalized)
+{
+	float slew_rate_normalization{1.f};
+
+	if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized
+		slew_rate_normalization = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f;
+	}
+
+	if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
+		yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization);
+		yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt);
+
+	} else {
+		yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint);
+	}
+
+	// Transform yaw rate into speed difference
+	float speed_diff_normalized{0.f};
+
+	if (normalized) {
+		speed_diff_normalized = yaw_rate_with_accel_limit.getState();
+
+	} else {
+		if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward
+			const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track /
+						 2.f;
+			speed_diff_normalized = math::interpolate<float>(speed_diff, -max_thr_yaw_r,
+						max_thr_yaw_r, -1.f, 1.f);
+		}
+
+		_pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState());
+		speed_diff_normalized = math::constrain(speed_diff_normalized +
+							_pid_yaw_rate.update(vehicle_yaw_rate, dt),
+							-1.f, 1.f); // Feedback
+
+
+	}
+
+	return math::constrain(speed_diff_normalized, -1.f, 1.f);
+
+}
+
+float RoverMecanumControl::calcNormalizedSpeedSetpoint(const float speed_setpoint,
+		const float vehicle_speed, const float max_thr_spd, SlewRate<float> &speed_setpoint_with_accel_limit,
+		PID &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized)
+{
+	float slew_rate_normalization{1.f};
+
+	if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized
+		slew_rate_normalization = max_thr_spd > FLT_EPSILON ? max_thr_spd : 0.f;
+	}
+
+	// Apply acceleration and deceleration limit
+	if (fabsf(speed_setpoint) >= fabsf(speed_setpoint_with_accel_limit.getState())) {
+		if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
+			speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization);
+			speed_setpoint_with_accel_limit.update(speed_setpoint, dt);
+
+		} else {
+			speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint);
+
+		}
+
+	} else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
+		speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization);
+		speed_setpoint_with_accel_limit.update(speed_setpoint, dt);
+
+	} else {
+		speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint);
+	}
+
+	// Calculate normalized forward speed setpoint
+	float forward_speed_normalized{0.f};
+
+	if (normalized) {
+		forward_speed_normalized = speed_setpoint_with_accel_limit.getState();
+
+	} else { // Closed loop speed control
+
+		if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
+			forward_speed_normalized = math::interpolate<float>(speed_setpoint_with_accel_limit.getState(),
+						   -max_thr_spd, max_thr_spd,
+						   -1.f, 1.f);
+		}
+
+		pid_throttle.setSetpoint(speed_setpoint_with_accel_limit.getState());
+		forward_speed_normalized += pid_throttle.update(vehicle_speed, dt); // Feedback
+
+	}
+
+	return math::constrain(forward_speed_normalized, -1.f, 1.f);
+
+}
+
+matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_speed_normalized,
+		float lateral_speed_normalized,
+		float speed_diff)
+{
+	const float total_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized) + fabsf(speed_diff);
+
+	if (total_speed > 1.f) { // Adjust speed setpoints if infeasible
+		const float theta = atan2f(fabsf(lateral_speed_normalized), fabsf(forward_speed_normalized));
+		const float magnitude = (1.f - fabsf(speed_diff)) / (sinf(theta) + cosf(theta));
+		const float normalization = 1.f / (sqrtf(powf(forward_speed_normalized, 2.f) + powf(lateral_speed_normalized, 2.f)));
+		forward_speed_normalized *= magnitude * normalization;
+		lateral_speed_normalized *= magnitude * normalization;
+
+	}
+
+	// Calculate motor commands
+	const float input_data[3] = {forward_speed_normalized, lateral_speed_normalized, speed_diff};
+	const Matrix<float, 3, 1> input(input_data);
+	const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f};
+	const Matrix<float, 4, 3> m(m_data);
+	const Vector4f motor_commands = m * input;
+
+	return motor_commands;
+}
+
+void RoverMecanumControl::resetControllers()
+{
+	_pid_forward_throttle.resetIntegral();
+	_pid_lateral_throttle.resetIntegral();
+	_pid_yaw_rate.resetIntegral();
+	_pid_yaw.resetIntegral();
+}
@@ -0,0 +1,177 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+// PX4 includes
+#include <px4_platform_common/module_params.h>
+
+// uORB includes
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/rover_mecanum_setpoint.h>
+#include <uORB/topics/rover_mecanum_status.h>
+#include <uORB/topics/actuator_motors.h>
+#include <lib/slew_rate/SlewRate.hpp>
+#include <lib/slew_rate/SlewRateYaw.hpp>
+
+// Standard libraries
+#include <lib/pid/PID.hpp>
+#include <matrix/matrix/math.hpp>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <math.h>
+
+using namespace matrix;
+
+/**
+ * @brief Class for mecanum rover guidance.
+ */
+class RoverMecanumControl : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for RoverMecanumControl.
+	 * @param parent The parent ModuleParams object.
+	 */
+	RoverMecanumControl(ModuleParams *parent);
+	~RoverMecanumControl() = default;
+
+	/**
+	 * @brief Compute motor commands based on setpoints
+	 * @param vehicle_yaw Yaw of the vehicle [rad]
+	 * @param vehicle_yaw_rate Yaw rate of the vehicle [rad/s]
+	 * @param vehicle_forward_speed Forward speed of the vehicle [m/s]
+	 * @param vehicle_lateral_speed Lateral speed of the vehicle [m/s]
+	 */
+	void computeMotorCommands(float vehicle_yaw, float vehicle_yaw_rate, float vehicle_forward_speed,
+				  float vehicle_lateral_speed);
+
+	/**
+	 * @brief Reset PID controllers
+	 */
+	void resetControllers();
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	/**
+	 * @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates.
+	 * @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]].
+	 * @param vehicle_yaw_rate Measured yaw rate [rad/s].
+	 * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s].
+	 * @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2].
+	 * @param wheel_track Wheel track [m].
+	 * @param dt Time since last update [s].
+	 * @param yaw_rate_with_accel_limit Yaw rate slew rate.
+	 * @param pid_yaw_rate Yaw rate PID
+	 * @param normalized Indicates if the forward speed setpoint is already normalized.
+	 * @return Normalized speed differece setpoint with applied slew rates [-1, 1].
+	 */
+	float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel,
+				      float wheel_track, float dt, SlewRate<float> &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized);
+	/**
+	 * @brief Compute normalized speed setpoint and apply slew rates.
+	 * to the speed setpoint and doing closed loop speed control if enabled.
+	 * @param speed_setpoint Speed setpoint [m/s].
+	 * @param vehicle_speed Actual speed [m/s].
+	 * @param max_thr_spd Speed the rover drives at maximum throttle [m/s].
+	 * @param speed_setpoint_with_accel_limit Speed slew rate.
+	 * @param pid_throttle Throttle PID
+	 * @param max_accel Maximum acceleration [m/s^2]
+	 * @param max_decel Maximum deceleration [m/s^2]
+	 * @param dt Time since last update [s].
+	 * @param normalized Indicates if the speed setpoint is already normalized.
+	 * @return Normalized speed setpoint with applied slew rates [-1, 1].
+	 */
+	float calcNormalizedSpeedSetpoint(float speed_setpoint, float vehicle_forward_speed, float max_thr_spd,
+					  SlewRate<float> &speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel,
+					  float dt, bool normalized);
+
+	/**
+	 * @brief Turn normalized speed setpoints into normalized motor commands.
+	 *
+	 * @param forward_speed Normalized linear speed in body forward direction [-1, 1].
+	 * @param lateral_speed Normalized linear speed in body lateral direction [-1, 1].
+	 * @param speed_diff Normalized speed difference between left and right wheels [-1, 1].
+	 * @return matrix::Vector4f: Normalized motor inputs [-1, 1]
+	 */
+	matrix::Vector4f computeInverseKinematics(float forward_speed, float lateral_speed, float speed_diff);
+
+	// uORB subscriptions
+	uORB::Subscription _rover_mecanum_setpoint_sub{ORB_ID(rover_mecanum_setpoint)};
+
+	// uORB publications
+	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+	uORB::Publication<rover_mecanum_status_s> _rover_mecanum_status_pub{ORB_ID(rover_mecanum_status)};
+	rover_mecanum_status_s _rover_mecanum_status{};
+
+	// Variables
+	rover_mecanum_setpoint_s _rover_mecanum_setpoint{};
+	hrt_abstime _timestamp{0};
+	float _max_yaw_rate{0.f};
+	float _max_yaw_accel{0.f};
+
+	// Controllers
+	PID _pid_forward_throttle; // PID for the closed loop forward speed control
+	PID _pid_lateral_throttle; // PID for the closed loop lateral speed control
+	PID _pid_yaw; // PID for the closed loop yaw control
+	PID _pid_yaw_rate; // PID for the closed loop yaw rate control
+	SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};
+	SlewRate<float> _lateral_speed_setpoint_with_accel_limit{0.f};
+	SlewRate<float> _yaw_rate_with_accel_limit{0.f};
+	SlewRateYaw<float> _yaw_setpoint_with_yaw_rate_limit;
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RM_WHEEL_TRACK>)   _param_rm_wheel_track,
+		(ParamFloat<px4::params::RM_MAX_THR_SPD>)   _param_rm_max_thr_spd,
+		(ParamFloat<px4::params::RM_MAX_ACCEL>)     _param_rm_max_accel,
+		(ParamFloat<px4::params::RM_MAX_DECEL>)     _param_rm_max_decel,
+		(ParamFloat<px4::params::RM_MAX_THR_YAW_R>) _param_rm_max_thr_yaw_r,
+		(ParamFloat<px4::params::RM_MAX_YAW_RATE>)  _param_rm_max_yaw_rate,
+		(ParamFloat<px4::params::RM_MAX_YAW_ACCEL>) _param_rm_max_yaw_accel,
+		(ParamFloat<px4::params::RM_YAW_RATE_P>)    _param_rm_yaw_rate_p,
+		(ParamFloat<px4::params::RM_YAW_RATE_I>)    _param_rm_yaw_rate_i,
+		(ParamFloat<px4::params::RM_SPEED_P>)       _param_rm_p_gain_speed,
+		(ParamFloat<px4::params::RM_SPEED_I>)       _param_rm_i_gain_speed,
+		(ParamFloat<px4::params::RM_YAW_P>)         _param_rm_p_gain_yaw,
+		(ParamFloat<px4::params::RM_YAW_I>)         _param_rm_i_gain_yaw,
+		(ParamInt<px4::params::CA_R_REV>)           _param_r_rev
+	)
+};
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2025 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2024 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -31,6 +31,8 @@
 #
 ############################################################################

-px4_add_romfs_files(
-	rcS
+px4_add_library(RoverMecanumGuidance
+	RoverMecanumGuidance.cpp
 )
+
+target_include_directories(RoverMecanumGuidance PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@@ -0,0 +1,206 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023-2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include "RoverMecanumGuidance.hpp"
+
+#include <mathlib/math/Limits.hpp>
+
+using namespace matrix;
+using namespace time_literals;
+
+RoverMecanumGuidance::RoverMecanumGuidance(ModuleParams *parent) : ModuleParams(parent)
+{
+	updateParams();
+	_max_velocity_magnitude = _param_rm_max_speed.get();
+	_rover_mecanum_guidance_status_pub.advertise();
+}
+
+void RoverMecanumGuidance::updateParams()
+{
+	ModuleParams::updateParams();
+}
+
+void RoverMecanumGuidance::computeGuidance(const float yaw, const int nav_state)
+{
+	updateSubscriptions();
+
+	// Calculate desired velocity magnitude
+	float desired_velocity_magnitude{_max_velocity_magnitude};
+	const float distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
+					  _curr_wp(0), _curr_wp(1));
+
+	if (_param_rm_max_jerk.get() > FLT_EPSILON && _param_rm_max_accel.get() > FLT_EPSILON
+	    && _param_rm_miss_vel_gain.get() > FLT_EPSILON) {
+		const float speed_reduction = math::constrain(_param_rm_miss_vel_gain.get() * math::interpolate(
+						      M_PI_F - _waypoint_transition_angle, 0.f,
+						      M_PI_F, 0.f, 1.f), 0.f, 1.f);
+		desired_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(_param_rm_max_jerk.get(),
+					     _param_rm_max_accel.get(), distance_to_curr_wp, _max_velocity_magnitude * (1.f - speed_reduction));
+		desired_velocity_magnitude = math::constrain(desired_velocity_magnitude, -_max_velocity_magnitude,
+					     _max_velocity_magnitude);
+	}
+
+
+	// Calculate heading error
+	const float desired_heading = _pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
+				      desired_velocity_magnitude);
+	const float heading_error = matrix::wrap_pi(desired_heading - yaw);
+
+	// Catch return to launch
+	const float distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
+					  _next_wp(0), _next_wp(1));
+
+	if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
+		_mission_finished = distance_to_next_wp < _param_nav_acc_rad.get();
+	}
+
+	// Guidance logic
+	Vector2f desired_velocity(0.f, 0.f);
+
+	if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) {
+		desired_velocity = desired_velocity_magnitude * Vector2f(cosf(heading_error), sinf(heading_error));
+	}
+
+	// Timestamp
+	hrt_abstime timestamp = hrt_absolute_time();
+
+	// Publish mecanum controller status (logging)
+	rover_mecanum_guidance_status_s rover_mecanum_guidance_status{};
+	rover_mecanum_guidance_status.timestamp = timestamp;
+	rover_mecanum_guidance_status.lookahead_distance = _pure_pursuit.getLookaheadDistance();
+	rover_mecanum_guidance_status.heading_error = heading_error;
+	rover_mecanum_guidance_status.desired_speed = desired_velocity_magnitude;
+	_rover_mecanum_guidance_status_pub.publish(rover_mecanum_guidance_status);
+
+	// Publish setpoints
+	rover_mecanum_setpoint_s rover_mecanum_setpoint{};
+	rover_mecanum_setpoint.timestamp = timestamp;
+	rover_mecanum_setpoint.forward_speed_setpoint = desired_velocity(0);
+	rover_mecanum_setpoint.forward_speed_setpoint_normalized = NAN;
+	rover_mecanum_setpoint.lateral_speed_setpoint = desired_velocity(1);
+	rover_mecanum_setpoint.lateral_speed_setpoint_normalized = NAN;
+	rover_mecanum_setpoint.yaw_rate_setpoint = NAN;
+	rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN;
+	rover_mecanum_setpoint.yaw_setpoint = _desired_yaw;
+	_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
+}
+
+void RoverMecanumGuidance::updateSubscriptions()
+{
+	if (_vehicle_global_position_sub.updated()) {
+		vehicle_global_position_s vehicle_global_position{};
+		_vehicle_global_position_sub.copy(&vehicle_global_position);
+		_curr_pos = Vector2d(vehicle_global_position.lat, vehicle_global_position.lon);
+	}
+
+	if (_local_position_sub.updated()) {
+		vehicle_local_position_s local_position{};
+		_local_position_sub.copy(&local_position);
+
+		if (!_global_ned_proj_ref.isInitialized()
+		    || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) {
+			_global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp);
+		}
+
+		_curr_pos_ned = Vector2f(local_position.x, local_position.y);
+	}
+
+	if (_position_setpoint_triplet_sub.updated()) {
+		updateWaypoints();
+	}
+
+	if (_mission_result_sub.updated()) {
+		mission_result_s mission_result{};
+		_mission_result_sub.copy(&mission_result);
+		_mission_finished = mission_result.finished;
+	}
+
+	if (_home_position_sub.updated()) {
+		home_position_s home_position{};
+		_home_position_sub.copy(&home_position);
+		_home_position = Vector2d(home_position.lat, home_position.lon);
+	}
+}
+
+void RoverMecanumGuidance::updateWaypoints()
+{
+	position_setpoint_triplet_s position_setpoint_triplet{};
+	_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
+
+	// Global waypoint coordinates
+	if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
+	    && PX4_ISFINITE(position_setpoint_triplet.current.lon)) {
+		_curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon);
+
+	} else {
+		_curr_wp = Vector2d(0, 0);
+	}
+
+	if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat)
+	    && PX4_ISFINITE(position_setpoint_triplet.previous.lon)) {
+		_prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon);
+
+	} else {
+		_prev_wp = _curr_pos;
+	}
+
+	if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat)
+	    && PX4_ISFINITE(position_setpoint_triplet.next.lon)) {
+		_next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
+
+	} else {
+		_next_wp = _home_position;
+	}
+
+	// NED waypoint coordinates
+	_curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1));
+	_prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1));
+	_next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1));
+
+	// Distances
+	const Vector2f curr_to_next_wp_ned = _next_wp_ned - _curr_wp_ned;
+	const Vector2f curr_to_prev_wp_ned = _prev_wp_ned - _curr_wp_ned;
+	float cosin = curr_to_prev_wp_ned.unit_or_zero() * curr_to_next_wp_ned.unit_or_zero();
+	cosin = math::constrain<float>(cosin, -1.f, 1.f); // Protect against float precision problem
+	_waypoint_transition_angle = acosf(cosin);
+
+	// Waypoint cruising speed
+	_max_velocity_magnitude = position_setpoint_triplet.current.cruising_speed > FLT_EPSILON ? math::constrain(
+					  position_setpoint_triplet.current.cruising_speed, 0.f,
+					  _param_rm_max_speed.get()) : _param_rm_max_speed.get();
+
+	// Waypoint yaw setpoint
+	if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) {
+		_desired_yaw = position_setpoint_triplet.current.yaw;
+	}
+}
@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2024 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+// PX4 includes
+#include <px4_platform_common/module_params.h>
+#include <lib/pure_pursuit/PurePursuit.hpp>
+
+// uORB includes
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/mission_result.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/rover_mecanum_guidance_status.h>
+#include <uORB/topics/rover_mecanum_setpoint.h>
+
+// Standard libraries
+#include <matrix/matrix/math.hpp>
+#include <matrix/math.hpp>
+#include <mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+#include <math.h>
+
+using namespace matrix;
+
+/**
+ * @brief Class for mecanum rover guidance.
+ */
+class RoverMecanumGuidance : public ModuleParams
+{
+public:
+	/**
+	 * @brief Constructor for RoverMecanumGuidance.
+	 * @param parent The parent ModuleParams object.
+	 */
+	RoverMecanumGuidance(ModuleParams *parent);
+	~RoverMecanumGuidance() = default;
+
+	/**
+	 * @brief Compute guidance for the vehicle.
+	 * @param yaw The yaw orientation of the vehicle in radians.
+	 * @param nav_state Navigation state of the rover.
+	 */
+	void computeGuidance(float yaw, int nav_state);
+
+	void setDesiredYaw(float desired_yaw) { _desired_yaw = desired_yaw; };
+
+protected:
+	/**
+	 * @brief Update the parameters of the module.
+	 */
+	void updateParams() override;
+
+private:
+	/**
+	 * @brief Update uORB subscriptions
+	 */
+	void updateSubscriptions();
+
+	/**
+	 * @brief Update global/ned waypoint coordinates
+	 */
+	void updateWaypoints();
+
+	// uORB subscriptions
+	uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
+	uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
+	uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _home_position_sub{ORB_ID(home_position)};
+
+	// uORB publications
+	uORB::Publication<rover_mecanum_guidance_status_s> _rover_mecanum_guidance_status_pub{ORB_ID(rover_mecanum_guidance_status)};
+	uORB::Publication<rover_mecanum_setpoint_s> _rover_mecanum_setpoint_pub{ORB_ID(rover_mecanum_setpoint)};
+
+	// Variables
+	MapProjection _global_ned_proj_ref{}; // Transform global to ned coordinates.
+	PurePursuit _pure_pursuit{this}; // Pure pursuit library
+	bool _mission_finished{false};
+
+	// Waypoints
+	Vector2d _curr_pos{};
+	Vector2f _curr_pos_ned{};
+	Vector2d _prev_wp{};
+	Vector2f _prev_wp_ned{};
+	Vector2d _curr_wp{};
+	Vector2f _curr_wp_ned{};
+	Vector2d _next_wp{};
+	Vector2f _next_wp_ned{};
+	Vector2d _home_position{};
+	float _max_velocity_magnitude{0.f};
+	float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
+	float _desired_yaw{0.f};
+
+	// Parameters
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RM_MAX_SPEED>) _param_rm_max_speed,
+		(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
+		(ParamFloat<px4::params::RM_MAX_JERK>) _param_rm_max_jerk,
+		(ParamFloat<px4::params::RM_MAX_ACCEL>) _param_rm_max_accel,
+		(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate,
+		(ParamFloat<px4::params::RM_MISS_VEL_GAIN>) _param_rm_miss_vel_gain
+	)
+};
@@ -3,17 +3,47 @@ module_name: Rover Mecanum
 parameters:
  - group: Rover Mecanum
    definitions:
+
      RM_WHEEL_TRACK:
-          description:
-            short: Wheel track
-            long: Distance from the center of the right wheel to the center of the left wheel.
-          type: float
-          unit: m
-          min: 0
-          max: 100
-          increment: 0.001
-          decimal: 3
-          default: 0
+        description:
+          short: Wheel track
+          long: Distance from the center of the right wheels to the center of the left wheels.
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.5
+
+      RM_MAX_YAW_RATE:
+        description:
+          short: Maximum allowed yaw rate for the rover.
+          long: |
+            This parameter is used to cap desired yaw rates and map controller inputs to desired yaw rates in acro mode.
+        type: float
+        unit: deg/s
+        min: 0.01
+        max: 1000
+        increment: 0.01
+        decimal: 2
+        default: 90
+
+      RM_MAX_YAW_ACCEL:
+        description:
+          short: Maximum allowed yaw acceleration for the rover
+          long: |
+            This parameter is used to cap desired yaw acceleration. This is used to adjust incoming yaw rate setpoints
+            to a feasible yaw rate setpoint based on the physical limitation on how fast the yaw rate can change.
+            This leads to a smooth setpoint trajectory for the closed loop yaw rate controller to track.
+            Set to -1 to disable.
+        type: float
+        unit: deg/s^2
+        min: -1
+        max: 1000
+        increment: 0.01
+        decimal: 2
+        default: -1

      RM_MAX_THR_YAW_R:
        description:
@@ -22,7 +52,7 @@ parameters:
            This parameter is used to calculate the feedforward term of the closed loop yaw rate control.
            The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate.
            This desired speed difference is then linearly mapped to normalized motor commands.
-            A good starting point is half the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)).
+            A good starting point is twice the speed the rover drives at maximum throttle (RM_MAX_THRTL_SPD)).
            Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower.
        type: float
        unit: m/s
@@ -30,37 +60,149 @@ parameters:
        max: 100
        increment: 0.01
        decimal: 2
+        default: 2
+
+      RM_YAW_RATE_P:
+        description:
+          short: Proportional gain for the closed-loop yaw rate controller.
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RM_YAW_RATE_I:
+        description:
+          short: Integral gain for the closed-loop yaw rate controller.
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
        default: 0

-      RM_MISS_SPD_GAIN:
+      RM_YAW_P:
        description:
-          short: Tuning parameter for the speed reduction during waypoint transition
-          long: |
-            The waypoint transition speed is calculated as:
-            Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN)
-            The normalized transition angle is the angle between the line segment from prev-curr waypoint and
-            curr-next waypoint interpolated from [0, 180] -> [0, 1].
-            Higher value -> More speed reduction during waypoint transitions.
-            Set to -1 to disable any speed reduction during waypoint transition.
+          short: Proportional gain for closed loop yaw controller
        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RM_YAW_I:
+        description:
+            short: Integral gain for closed loop yaw controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.1
+
+      RM_MAX_SPEED:
+        description:
+          short: Maximum speed the rover is allowed to drive
+          long: |
+            This parameter is used cap the maximum speed the rover is allowed to drive
+            and to map stick inputs to desired speeds in position mode.
+        type: float
+        unit: m/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 7
+
+      RM_MAX_THR_SPD:
+        description:
+          short: Speed the rover drives at maximum throttle
+          long: |
+            This parameter is used to calculate the feedforward term of the closed loop speed control which linearly
+            maps desired speeds to normalized motor commands [-1. 1].
+            A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode.
+            Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower.
+        type: float
+        unit: m/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 1
+
+      RM_SPEED_P:
+        description:
+          short: Proportional gain for speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 3
+        default: 1
+
+      RM_SPEED_I:
+        description:
+          short: Integral gain for ground speed controller
+        type: float
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 3
+        default: 0
+
+      RM_MAX_JERK:
+        description:
+          short: Maximum jerk
+          long: Limit for forwards acc/deceleration change.
+        type: float
+        unit: m/s^3
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.5
+
+      RM_MAX_ACCEL:
+        description:
+          short: Maximum acceleration
+          long: Maximum acceleration is used to limit the acceleration of the rover
+        type: float
+        unit: m/s^2
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 0.5
+
+      RM_MAX_DECEL:
+        description:
+          short: Maximum deceleration
+          long: |
+            Maximum decelaration is used to limit the deceleration of the rover.
+            Set to -1 to disable, causing the rover to decelerate as fast as possible.
+            Caution: Disabling the deceleration limit also disables the slow down effect in auto modes.
+        type: float
+        unit: m/s^2
        min: -1
        max: 100
        increment: 0.01
        decimal: 2
        default: -1

-      RM_COURSE_CTL_TH:
+      RM_MISS_VEL_GAIN:
        description:
-          short: Threshold to update course control in manual position mode
+          short: Tuning parameter for the velocity reduction during waypoint transition
          long: |
-            Threshold for the angle between the active cruise direction and the cruise direction given
-            by the stick inputs.
-            This can be understood as a deadzone for the combined stick inputs for forward/backwards
-            and lateral speed which defines a course direction.
+            The waypoint transition speed is calculated as:
+            Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN)
+            The normalized transition angle is the angle between the line segment from prev-curr WP and curr-next WP
+            interpolated from [0, 180] -> [0, 1].
+            Higher value -> More velocity reduction during cornering.
        type: float
-        unit: rad
-        min: 0
-        max: 3.14
+        min: 0.05
+        max: 100
        increment: 0.01
        decimal: 2
-        default: 0.17
+        default: 1
@@ -172,6 +172,11 @@ void Standard::update_transition_state()

 	VtolType::update_transition_state();

+	const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d));
+	float roll_body = attitude_setpoint_euler.phi();
+	float pitch_body = attitude_setpoint_euler.theta();
+	float yaw_body = attitude_setpoint_euler.psi();
+
 	// 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) {
@@ -181,6 +186,7 @@ void Standard::update_transition_state()
 		}

 		memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+		roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi();

 	} else {
 		// we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active)
@@ -192,16 +198,6 @@ void Standard::update_transition_state()
 		_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
 	}

-
-	const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d));
-	float roll_body = attitude_setpoint_euler.phi();
-	float pitch_body = attitude_setpoint_euler.theta();
-	float yaw_body = attitude_setpoint_euler.psi();
-
-	if (_v_control_mode->flag_control_climb_rate_enabled) {
-		roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi();
-	}
-
 	if (_vtol_mode == vtol_mode::TRANSITION_TO_FW) {
 		if (_param_vt_psher_slew.get() <= FLT_EPSILON) {
 			// just set the final target throttle value
@@ -203,6 +203,11 @@ void Tiltrotor::update_transition_state()

 	const hrt_abstime now = hrt_absolute_time();

+	const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d));
+	float roll_body = attitude_setpoint_euler.phi();
+	float pitch_body = attitude_setpoint_euler.theta();
+	float yaw_body = attitude_setpoint_euler.psi();
+
 	// 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) {
@@ -212,6 +217,7 @@ void Tiltrotor::update_transition_state()
 		}

 		memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+		roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi();
 		_thrust_transition = -_mc_virtual_att_sp->thrust_body[2];

 	} else {
@@ -225,15 +231,6 @@ void Tiltrotor::update_transition_state()
 	}


-	const Eulerf attitude_setpoint_euler(Quatf(_v_att_sp->q_d));
-	float roll_body = attitude_setpoint_euler.phi();
-	float pitch_body = attitude_setpoint_euler.theta();
-	float yaw_body = attitude_setpoint_euler.psi();
-
-	if (_v_control_mode->flag_control_climb_rate_enabled) {
-		roll_body = Eulerf(Quatf(_fw_virtual_att_sp->q_d)).phi();
-	}
-
 	if (_vtol_mode == vtol_mode::TRANSITION_FRONT_P1) {
 		// for the first part of the transition all rotors are enabled

@@ -1,39 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-px4_add_module(
-	MODULE systemcmds__mft_cfg
-	MAIN mft_cfg
-	COMPILE_FLAGS
-	SRCS
-		mft_cfg.cpp
-	)
@@ -1,5 +0,0 @@
-menuconfig SYSTEMCMDS_MFT_CFG
-	bool "mft_cfg"
-	default n
-	---help---
-		Enable support for mft_cfg
@@ -1,239 +0,0 @@
-/****************************************************************************
-*
-* 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.
-*
-****************************************************************************/
-
-/**
-* @file mft_cfg.c
-*/
-
-#include <board_config.h>
-#include <px4_platform_common/board_common.h>
-#include <px4_platform/board_hw_eeprom_rev_ver.h>
-#include <px4_platform_common/module.h>
-#include <px4_platform_common/getopt.h>
-#include <px4_platform_common/px4_manifest.h>
-#include <stdio.h>
-#include <stdbool.h>
-#include <string.h>
-#include <fcntl.h>
-
-
-static void usage(const char *reason)
-{
-	if (reason != nullptr) {
-		printf("%s\n\n", reason);
-	}
-
-	PRINT_MODULE_DESCRIPTION("Tool to set and get manifest configuration");
-
-	PRINT_MODULE_USAGE_NAME("mft_cfg", "command");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("get", "get manifest configuration");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("set", "set manifest configuration");
-	PRINT_MODULE_USAGE_COMMAND_DESCR("reset", "reset manifest configuration");
-	PRINT_MODULE_USAGE_ARG("hwver|hwrev", "Select type: MTD_MTF_VER|MTD_MTF_REV", false);
-	PRINT_MODULE_USAGE_PARAM_INT('i', 0x10, 0x10, 0xFFFF,
-				     "argument to set extended hardware id (id == version for <hwver>, id == revision for <hwrev> )", false);
-
-}
-
-
-static int print_extended_id(const char *type)
-{
-	mtd_mft_v0_t mtd_mft;
-	mtd_mft.version.id = MTD_MFT_v0;
-	mtd_mft.hw_extended_id = -1;
-
-	const char *path = nullptr;
-	int ret_val = px4_mtd_query(type, NULL, &path);
-
-	if (ret_val != PX4_OK) {
-		PX4_ERR("Can't get mtd query (%s, %i)", type, ret_val);
-
-	} else {
-
-		ret_val = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);
-
-		if (ret_val == PX4_OK) {
-			PX4_INFO("%s, hw_extended_id = %#x", type, mtd_mft.hw_extended_id);
-
-		} else  {
-
-			if (ret_val == -EPROTO) {
-				PX4_ERR("Manifest data may not exist for %s", path);
-
-			} else {
-				PX4_ERR("Can't read hw_extended_id from EEPROM (%s, %i)", type, ret_val);
-			}
-		}
-	}
-
-	return ret_val;
-}
-
-extern "C" __EXPORT int mft_cfg_main(int argc, char *argv[])
-{
-	if ((argc == 2) && (!strcmp(argv[1], "get"))) {
-
-		char type_ver[] = "MTD_MFT_VER";
-		char type_rev[] = "MTD_MFT_REV";
-
-		print_extended_id(type_ver);
-		print_extended_id(type_rev);
-
-		return 0;
-
-	} else if (argc >= 3) {
-
-		int ret_val = -1;
-		const char *path = nullptr;
-		bool arg_exist = false;
-
-		for (int i = 2; i < argc; ++i) {
-			if (strcmp("hwver", argv[i]) == 0) {
-				ret_val = px4_mtd_query("MTD_MFT_VER", NULL, &path);
-				arg_exist = true;
-				break;
-			}
-
-			if (strcmp("hwrev", argv[i]) == 0) {
-				ret_val = px4_mtd_query("MTD_MFT_REV", NULL, &path);
-				arg_exist = true;
-				break;
-			}
-		}
-
-		if (!arg_exist) {
-			PX4_ERR("Missing <hwver> or <hwrev> arguments'");
-			return 1;
-		}
-
-		if (ret_val != PX4_OK) {
-			PX4_ERR("Can't get mtd query (%i)", ret_val);
-			return 1;
-		}
-
-		mtd_mft_v0_t mtd_mft;
-		mtd_mft.version.id = MTD_MFT_v0;
-		mtd_mft.hw_extended_id = -1;
-
-		if (!strcmp(argv[1], "set")) {
-			if (argc == 5) {
-
-				const char *myoptarg = NULL;
-				int ch = 0;
-				int myoptind = 1;
-				int hw_extended_id = -1;
-
-				while ((ch = px4_getopt(argc, argv, "i:", &myoptind, &myoptarg)) != EOF) {
-					switch (ch) {
-					case 'i':
-						hw_extended_id = strtol(myoptarg, NULL, 0);
-						break;
-
-					default:
-						PX4_ERR("To set id use '-i x'");
-						break;
-					}
-				}
-
-				if (hw_extended_id != -1) {
-
-					mtd_mft.hw_extended_id = (uint16_t)hw_extended_id;
-
-					ret_val = board_set_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);
-
-					if (ret_val != PX4_OK) {
-						PX4_ERR("Can't write to EEPROM (%i)", ret_val);
-
-					} else {
-						board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);
-						PX4_INFO("New hw_extended_id = %#x", mtd_mft.hw_extended_id);
-					}
-				}
-
-			} else {
-				PX4_ERR("Not enough arguments, try 'mft_cfg set hwver -i x'");
-				return 1;
-			}
-
-			return 0;
-		}
-
-		if (!strcmp(argv[1], "get")) {
-
-			ret_val = board_get_eeprom_hw_info(path, (mtd_mft_t *)&mtd_mft);
-
-			if (ret_val == PX4_OK) {
-				PX4_INFO("hw_extended_id = %#x", mtd_mft.hw_extended_id);
-
-			} else {
-
-				if (ret_val == -EPROTO) {
-					PX4_ERR("Manifest data may not exist for %s", path);
-
-				} else {
-					PX4_ERR("Can't read from EEPROM (%s, %i)", path, ret_val);
-				}
-
-				return 1;
-			}
-
-			return 0;
-		}
-
-		if (!strcmp(argv[1], "reset")) {
-
-			uint8_t buffer[64];
-			memset(buffer, 0xFF, sizeof(buffer));
-
-			int fd = open(path, O_WRONLY);
-
-			if (fd == -1) {
-				PX4_ERR("Failed to open partition %s", path);
-				return 1;
-			}
-
-			while (write(fd, buffer, sizeof(buffer)) == sizeof(buffer)) {}
-
-			PX4_INFO("Reset for %s completed. To remove manifest data from RAM, a reboot is required.", path);
-			close(fd);
-
-			return 0;
-		}
-
-	} else {
-		usage("Error, not enough parameters.");
-		return 1;
-	}
-
-	return 0;
-}
Author	SHA1	Message	Date
Matthias Grob	05851a89d1	ActuatorEffectivenessHelicopter: clarfification suggestions for servo linearization feature	2025-02-20 19:50:08 +01:00
TedObrien	642ea5b7f7	reduce CA_MAX_SVO_THROW short description length to stop test failure	2025-02-20 14:50:46 +01:00
TedObrien	d70a6726f4	fix formatting	2025-02-20 14:50:46 +01:00
TedObrien	8445795935	fix spelling mistake	2025-02-20 14:50:46 +01:00
TedObrien	c1d655e7f9	added comment and fixed spelling mistake	2025-02-20 14:50:46 +01:00
TedObrien	5d69bfc31e	remove CA_MAX_SVO_THROW from actuators tab to avoid confusion during standard swashplate setup.	2025-02-20 14:50:46 +01:00
TedObrien	0ee772e5e8	remove CA_LIN_SERVO and enable feature if CA_MAX_SERVO_THROW > 0 plus Update param description.	2025-02-20 14:50:40 +01:00
TedObrien	6ba8d73ebe	removed whitespace	2025-02-20 14:47:18 +01:00
TedObrien	3a8151e7e0	removed param unnecessary param	2025-02-20 14:47:18 +01:00
Ted	3c5b881482	add missing indentation Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>	2025-02-20 14:47:18 +01:00
TedObrien	80902cfe5e	fix variable referenced before assignment	2025-02-20 14:47:18 +01:00
TedObrien	ea42110a30	add missing semi-colon	2025-02-20 14:47:18 +01:00
TedObrien	6e1e2f1a2c	update variable name	2025-02-20 14:47:18 +01:00
Ted	a70c5e7e35	update description of CA_LIN_SERVO parameter	2025-02-20 14:47:18 +01:00
Ted	49fbcb2153	Apply suggestions from code review Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com>	2025-02-20 14:47:18 +01:00
TedObrien	f13718d482	control_allocator: Added linearization feature for heli swashplates to help prevent servo binding	2025-02-20 14:47:13 +01:00