Add topic remap plugin

ModeCompleted.msg: document nav_state (#22544 )
[control_allocation] small function comment typo fix (#22607 )
2026-05-24 23:47:35 +08:00 · 2024-01-09 10:27:42 +01:00 · 2024-01-08 11:32:41 +01:00 · 2024-01-08 11:12:06 +01:00 · 2024-01-06 04:41:00 -05:00 · 2024-01-06 04:41:00 -05:00
128 changed files with 1561 additions and 1132 deletions
@@ -1 +0,0 @@
--ignore-dir=Documentation
@@ -1,33 +0,0 @@
-# How to install:
-#   gem install github_changelog_generator
-# How to run:
-#   github_changelog_generator -u PX4 -p Firmware
-# Description:
-#   The following params are sensible defaults for the PX4 project,
-#   if you want to do a changelog before a release you need to update since-tag and future-releases,
-
-# Params:
-#   github_changelog_generator --help for all options
-
-# max-issues
-#   max threshold for github api queries
-#   make sure you set your CHANGELOG_GITHUB_TOKEN before
-#   running
-max-issues=1500
-
-# exclude-tags-regex
-#   excludes release candidates
-exclude-tags-regex=rc[0-9]{1,}|beta[0-9]{1,}
-
-# since-tag
-#   version of last stable release
-#   you need to change this depending on what you need
-#   if you want a changelog between versions this is the lowest version
-since-tag=1.6.5
-
-# future-release
-#   version you are about to release
-#   if you want a changelog between a version and all unreleased changes grouped as a release
-#     eg: v1.6.5 to v1.7.0
-future-release=v1.7.0
-
@@ -1,35 +0,0 @@
-language: cpp
-
-git:
-  depth: 100
-  submodules: false
-
-matrix:
-  fast_finish: true
-  include:
-    - os: linux
-      dist: xenial
-      # In order to stay under the coverity rate limit, we only run this weekly
-      # and not on push which is configured in travis-ci settings.
-      if: branch = main
-
-before_install:
-    - echo -n | openssl s_client -connect scan.coverity.com:443 | sed -ne '/-BEGIN CERTIFICATE-/,/-END CERTIFICATE-/p' | sudo tee -a /etc/ssl/certs/ca-
-
-install:
-    - export PATH=$HOME/.local/bin:$PATH
-    - pip install --user --upgrade pip
-    - pip install --user -r Tools/setup/requirements.txt
-
-script:
-    - make
-
-addons:
-  coverity_scan:
-    project:
-      name: "PX4/Firmware"
-      description: "Build submitted via Travis CI"
-    notification_email: ci@px4.io
-    build_command_prepend: "make distclean"
-    build_command: "make px4_sitl_default"
-    branch_pattern: coverity_scan
@@ -1,172 +0,0 @@
-# This file is NOT licensed under the GPLv3, which is the license for the rest
-# of YouCompleteMe.
-#
-# Here's the license text for this file:
-#
-# This is free and unencumbered software released into the public domain.
-#
-# Anyone is free to copy, modify, publish, use, compile, sell, or
-# distribute this software, either in source code form or as a compiled
-# binary, for any purpose, commercial or non-commercial, and by any
-# means.
-#
-# In jurisdictions that recognize copyright laws, the author or authors
-# of this software dedicate any and all copyright interest in the
-# software to the public domain. We make this dedication for the benefit
-# of the public at large and to the detriment of our heirs and
-# successors. We intend this dedication to be an overt act of
-# relinquishment in perpetuity of all present and future rights to this
-# software under copyright law.
-#
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
-# EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
-# MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
-# IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
-# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
-# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
-# OTHER DEALINGS IN THE SOFTWARE.
-#
-# For more information, please refer to <http://unlicense.org/>
-
-import os
-import ycm_core
-
-# These are the compilation flags that will be used in case there's no
-# compilation database set (by default, one is not set).
-# CHANGE THIS LIST OF FLAGS. YES, THIS IS THE DROID YOU HAVE BEEN LOOKING FOR.
-flags = [
-'-Wall',
-'-Wextra',
-'-Werror',
-#'-Wc++98-compat',
-'-Wno-long-long',
-'-Wno-variadic-macros',
-'-fexceptions',
-'-DNDEBUG',
-# You 100% do NOT need -DUSE_CLANG_COMPLETER in your flags; only the YCM
-# source code needs it.
-#'-DUSE_CLANG_COMPLETER',
-# THIS IS IMPORTANT! Without a "-std=<something>" flag, clang won't know which
-# language to use when compiling headers. So it will guess. Badly. So C++
-# headers will be compiled as C headers. You don't want that so ALWAYS specify
-# a "-std=<something>".
-# For a C project, you would set this to something like 'c99' instead of
-# 'c++14'.
-'-std=c++14',
-# ...and the same thing goes for the magic -x option which specifies the
-# language that the files to be compiled are written in. This is mostly
-# relevant for c++ headers.
-# For a C project, you would set this to 'c' instead of 'c++'.
-'-x',
-'c++',
-'-undef', # get rid of standard definitions to allow us to include arm math header
-'-I', os.path.join(os.path.expanduser("~"),'gcc-arm-none-eabi-4_7-2013q3/arm-none-eabi/include'),
-'-I', 'Build/px4_io-v2_default.build/nuttx-export/include/',
-'-I', './NuttX/nuttx/arch/arm/include',
-'-include', './src/include/visibility.h',
-'-I', './src',
-'-I', './src/modules',
-'-I', './src/include',
-'-I', './src/lib',
-'-I', './NuttX',
-]
-
-
-# Set this to the absolute path to the folder (NOT the file!) containing the
-# compile_commands.json file to use that instead of 'flags'. See here for
-# more details: http://clang.llvm.org/docs/JSONCompilationDatabase.html
-#
-# Most projects will NOT need to set this to anything; you can just change the
-# 'flags' list of compilation flags. Notice that YCM itself uses that approach.
-compilation_database_folder = ''
-
-if os.path.exists( compilation_database_folder ):
-  database = ycm_core.CompilationDatabase( compilation_database_folder )
-else:
-  database = None
-
-SOURCE_EXTENSIONS = [ '.cpp', '.cxx', '.cc', '.c', '.m', '.mm' ]
-
-def DirectoryOfThisScript():
-  return os.path.dirname( os.path.abspath( __file__ ) )
-
-
-def MakeRelativePathsInFlagsAbsolute( flags, working_directory ):
-  if not working_directory:
-    return list( flags )
-  new_flags = []
-  make_next_absolute = False
-  path_flags = [ '-isystem', '-I', '-iquote', '--sysroot=' ]
-  for flag in flags:
-    new_flag = flag
-
-    if make_next_absolute:
-      make_next_absolute = False
-      if not flag.startswith( '/' ):
-        new_flag = os.path.join( working_directory, flag )
-
-    for path_flag in path_flags:
-      if flag == path_flag:
-        make_next_absolute = True
-        break
-
-      if flag.startswith( path_flag ):
-        path = flag[ len( path_flag ): ]
-        new_flag = path_flag + os.path.join( working_directory, path )
-        break
-
-    if new_flag:
-      new_flags.append( new_flag )
-  return new_flags
-
-
-def IsHeaderFile( filename ):
-  extension = os.path.splitext( filename )[ 1 ]
-  return extension in [ '.h', '.hxx', '.hpp', '.hh' ]
-
-
-def GetCompilationInfoForFile( filename ):
-  # The compilation_commands.json file generated by CMake does not have entries
-  # for header files. So we do our best by asking the db for flags for a
-  # corresponding source file, if any. If one exists, the flags for that file
-  # should be good enough.
-  if IsHeaderFile( filename ):
-    basename = os.path.splitext( filename )[ 0 ]
-    for extension in SOURCE_EXTENSIONS:
-      replacement_file = basename + extension
-      if os.path.exists( replacement_file ):
-        compilation_info = database.GetCompilationInfoForFile(
-          replacement_file )
-        if compilation_info.compiler_flags_:
-          return compilation_info
-    return None
-  return database.GetCompilationInfoForFile( filename )
-
-
-def FlagsForFile( filename, **kwargs ):
-  if database:
-    # Bear in mind that compilation_info.compiler_flags_ does NOT return a
-    # python list, but a "list-like" StringVec object
-    compilation_info = GetCompilationInfoForFile( filename )
-    if not compilation_info:
-      return None
-
-    final_flags = MakeRelativePathsInFlagsAbsolute(
-      compilation_info.compiler_flags_,
-      compilation_info.compiler_working_dir_ )
-
-    # NOTE: This is just for YouCompleteMe; it's highly likely that your project
-    # does NOT need to remove the stdlib flag. DO NOT USE THIS IN YOUR
-    # ycm_extra_conf IF YOU'RE NOT 100% SURE YOU NEED IT.
-    #try:
-    #  final_flags.remove( '-stdlib=libc++' )
-    #except ValueError:
-    #  pass
-  else:
-    relative_to = DirectoryOfThisScript()
-    final_flags = MakeRelativePathsInFlagsAbsolute( flags, relative_to )
-
-  return {
-    'flags': final_flags,
-    'do_cache': True
-  }
@@ -1,83 +0,0 @@
-{
-	"folders":
-	[
-		{
-			"path": ".",
-			"file_exclude_patterns":
-			[
-				"*.o",
-				"*.a",
-				"*.d",
-				".built",
-				".context",
-				".depend",
-				".config",
-				".version",
-				"Make.dep",
-				".configured",
-				"*.sublime-project",
-				"*.sublime-workspace",
-				".project",
-				".cproject",
-				"cscope.out"
-			],
-			"folder_exclude_patterns":
-			[
-				".settings",
-				"nuttx/arch/arm/src/board",
-				"nuttx/arch/arm/src/chip",
-				"build_*"
-			]
-		}
-	],
-	"settings":
-	{
-		"tab_size": 8,
-		"translate_tabs_to_spaces": false,
-		"highlight_line": true,
-		"AStyleFormatter":
-		{
-			"options_c":
-			{
-				"use_only_additional_options": true,
-				"additional_options_file": "${project_path}/Tools/astyle/astylerc"
-			},
-			"options_c++":
-			{
-				"use_only_additional_options": true,
-				"additional_options_file": "${project_path}/Tools/astyle/astylerc"
-			}
-		}
-	},
-	"build_systems":
-	[
-		{
-			"name": "PX4: make all",
-			"working_dir": "${project_path}",
-			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
-			"cmd": ["make"],
-			"shell": true
-		},
-		{
-			"name": "PX4: make and upload",
-			"working_dir": "${project_path}",
-			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
-			"cmd": ["make upload px4_fmu-v2_default -j8"],
-			"shell": true
-		},
-		{
-			"name": "PX4: make posix",
-			"working_dir": "${project_path}",
-			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
-			"cmd": ["make posix"],
-			"shell": true
-		},
-		{
-			"name": "MindPX_V2: make and upload",
-			"working_dir": "${project_path}",
-			"file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$",
-			"cmd": ["make upload mindpx-v2_default -j8"],
-			"shell": true
-		}
-	]
-}
@@ -4,31 +4,9 @@
 # @type Rover
 # @class Rover

-. ${R}etc/init.d/rc.rover_defaults
+. ${R}etc/init.d/rc.rover_differential_defaults

-param set-default GND_L1_DIST 5
-param set-default GND_SP_CTRL_MODE 1
-param set-default GND_SPEED_D 3
-param set-default GND_SPEED_I 0.001
-param set-default GND_SPEED_IMAX 0.125
-param set-default GND_SPEED_P 0.25
-param set-default GND_SPEED_THR_SC 1
-param set-default GND_SPEED_TRIM 4
-param set-default GND_THR_CRUISE 0.3
-param set-default GND_THR_MAX 0.5
-param set-default GND_THR_MIN 0
-
-param set-default NAV_ACC_RAD 0.5
-param set-default NAV_LOITER_RAD 2
-
-param set-default GND_MAX_ANG 0.6
-param set-default GND_WHEEL_BASE 2.0
-
-param set-default CA_AIRFRAME 6
-
-param set-default CA_R_REV 3
 param set-default PWM_MAIN_FUNC1 101
 param set-default PWM_MAIN_FUNC2 101
 param set-default PWM_MAIN_FUNC6 102
 param set-default PWM_MAIN_FUNC7 102
-
@@ -66,7 +66,6 @@ param set-default SIM_GZ_SV_FUNC2 202
 param set-default SIM_GZ_SV_FUNC3 203

 param set-default COM_RC_IN_MODE 1
-param set-default ASPD_PRIMARY 1

 param set-default CA_SV_CS_COUNT 3
 param set-default CA_SV_CS0_TYPE 1
@@ -0,0 +1,31 @@
+#!/bin/sh
+# @name Aion Robotics R1 Rover
+# @type Rover
+# @class Rover
+
+. ${R}etc/init.d/rc.rover_differential_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+# Simulated sensors
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
+# Actuator mapping
+param set-default SIM_GZ_WH_FUNC1 101 # right wheel
+param set-default SIM_GZ_WH_MIN1 0
+param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_DIS1 100
+
+param set-default SIM_GZ_WH_FUNC2 102 # left wheel
+param set-default SIM_GZ_WH_MIN2 0
+param set-default SIM_GZ_WH_MAX2 200
+param set-default SIM_GZ_WH_DIS2 100
+
+param set-default SIM_GZ_WH_REV 1 # reverse right wheel
@@ -80,6 +80,7 @@ px4_add_romfs_files(
 	4005_gz_x500_vision
 	4006_gz_px4vision
 	4008_gz_advanced_plane
+	4009_gz_r1_rover
 	4010_gz_x500_mono_cam

 	6011_gazebo-classic_typhoon_h480
@@ -153,7 +153,6 @@ fi

 param set-default BAT1_N_CELLS 4

-param set-default CBRK_AIRSPD_CHK 0
 param set-default CBRK_SUPPLY_CHK 894281

 # disable check, no CPU load reported on posix yet
@@ -51,6 +51,8 @@ px4_add_romfs_files(
 	rc.thermal_cal
 	rc.rover_apps
 	rc.rover_defaults
+	rc.rover_differential_apps
+	rc.rover_differential_defaults
 	rc.uuv_apps
 	rc.uuv_defaults
 	rc.vehicle_setup
@@ -28,13 +28,13 @@ param set-default BAT1_CAPACITY 23000
 param set-default BAT1_N_CELLS 4
 param set-default BAT1_R_INTERNAL 0.0025

-param set-default CBRK_AIRSPD_CHK 162128
+param set-default SYS_HAS_NUM_ASPD 0
 param set-default CBRK_IO_SAFETY 22027

 param set-default EKF2_GPS_POS_X -0.12
 param set-default EKF2_IMU_POS_X -0.12

-param set-default FW_ARSP_MODE 1
+param set-default FW_USE_AIRSPD 0
 param set-default NPFG_PERIOD 25
 param set-default FW_PR_FF 0.7
 param set-default FW_PR_I 0.18
@@ -11,52 +11,15 @@
 # @board bitcraze_crazyflie exclude
 #

-. ${R}etc/init.d/rc.rover_defaults
-
-
+. ${R}etc/init.d/rc.rover_differential_defaults

 param set-default BAT1_N_CELLS 4

 param set-default EKF2_GBIAS_INIT 0.01
 param set-default EKF2_ANGERR_INIT 0.01
-param set-default EKF2_MAG_TYPE 1
-
-param set-default FW_AIRSPD_MIN 0
-param set-default FW_AIRSPD_TRIM 1
-param set-default FW_AIRSPD_MAX 3
-
-param set-default GND_SP_CTRL_MODE 1
-param set-default GND_L1_DIST 5
-param set-default GND_L1_PERIOD 3
-param set-default GND_THR_CRUISE 0.7
-param set-default GND_THR_MAX 0.5
-
-# Because this is differential drive, it can make a turn with radius 0.
-# This corresponds to a turn angle of pi radians.
-# If a special case is made for differential-drive, this will need to change.
-param set-default GND_MAX_ANG 3.142
-param set-default GND_WHEEL_BASE 0.3
-
-# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
-# to support negative throttle.
-param set-default GND_THR_MIN 0
-param set-default GND_SPEED_P 0.25
-param set-default GND_SPEED_I 3
-param set-default GND_SPEED_D 0.001
-param set-default GND_SPEED_IMAX 0.125
-param set-default GND_SPEED_THR_SC 1
-
-param set-default NAV_ACC_RAD 0.5
-
-# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
-param set-default GND_MAX_ANG 3.1415

 # Set geometry & output configration
-param set-default CA_AIRFRAME 6
-param set-default CA_R_REV 3
-
-
 param set-default RBCLW_ADDRESS 128
 param set-default RBCLW_FUNC1 101
 param set-default RBCLW_FUNC2 102
-param set-default RBCLW_REV 1
+param set-default RBCLW_REV 1 # reverse right wheels
@@ -54,3 +54,5 @@ param set-default MIS_TKO_LAND_REQ 2
 # FW takeoff acceleration can easily exceed ublox GPS 2G default.
 #
 param set-default GPS_UBX_DYNMODEL 8
+
+param set-default SYS_HAS_NUM_ASPD 1 # by default require an airspeed sensor
@@ -9,8 +9,6 @@
 # Start the attitude and position estimator.
 #
 ekf2 start &
-#attitude_estimator_q start
-#local_position_estimator start

 #
 # Start Control Allocator
@@ -22,7 +20,6 @@ control_allocator start
 #
 rover_pos_control start

-
 #
 # Start Land Detector.
 #
@@ -10,9 +10,6 @@ set VEHICLE_TYPE rover
 # MAV_TYPE_GROUND_ROVER 10
 param set-default MAV_TYPE 10

-# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor
-param set-default CBRK_AIRSPD_CHK 162128
-
 param set-default MIS_TAKEOFF_ALT 0.01

 param set-default NAV_ACC_RAD 2
@@ -0,0 +1,11 @@
+#!/bin/sh
+# Standard apps for a differential drive rover.
+
+# Start the attitude and position estimator.
+ekf2 start &
+
+# Start rover differential drive controller.
+differential_drive_control start
+
+# Start Land Detector.
+land_detector start rover
@@ -0,0 +1,11 @@
+#!/bin/sh
+# Differential rover parameters.
+
+set VEHICLE_TYPE rover_differential
+
+param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
+
+param set-default CA_AIRFRAME 6 # Rover (Differential)
+param set-default CA_R_REV 3 # Right and left motors reversible
+
+param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying
@@ -9,6 +9,3 @@ set VEHICLE_TYPE uuv

 # MAV_TYPE_SUBMARINE 12
 param set-default MAV_TYPE 12
-
-# UUV don't have an airspeed sensor, so disable checks around it
-param set-default CBRK_AIRSPD_CHK 162128
@@ -32,6 +32,15 @@ then
 	. ${R}etc/init.d/rc.rover_apps
 fi

+#
+# Differential Rover setup.
+#
+if [ $VEHICLE_TYPE = rover_differential ]
+then
+	# Start differential drive rover apps.
+	. ${R}etc/init.d/rc.rover_differential_apps
+fi
+
 #
 # VTOL setup.
 #
@@ -42,4 +42,6 @@ param set-default MPC_MAN_Y_MAX 90

 param set-default RTL_TYPE 1

+param set-default SYS_HAS_NUM_ASPD 1 # by default require an airspeed sensor
+
 param set-default WV_EN 1
@@ -1,47 +0,0 @@
-# Build version
-version: "{build}"
-# do not shallow clone because we want to infer the version from the last tag
-
-branches:
-  only:
-  - master
-  - beta
-  - stable
-
-# Build worker image (build VM template)
-image: Visual Studio 2017
-
-environment:
-  matrix:
-    - PX4_CONFIG: tests # this builds posix in px4_sitl_test folder and runs tests
-    - PX4_CONFIG: px4_fmu-v5_default
-
-install:
-# if the toolchain wasn't restored from build cache download and install it
- ps: >-
-    if (-not (Test-Path C:\PX4)) {
-        Invoke-WebRequest https://s3-us-west-2.amazonaws.com/px4-tools/PX4+Windows+Cygwin+Toolchain/PX4+Windows+Cygwin+Toolchain+0.9.msi -OutFile C:\Toolchain.msi
-        Start-Process -Wait msiexec -ArgumentList '/I C:\Toolchain.msi /quiet /qn /norestart /log C:\install.log'
-    }
-
-# Note: using Start-Process -Wait is important
-# because otherwise the install begins but non-blocking and the result cannot be used just after
-
-build_script:
-# FIXME Temporary we need to create the home folder because it's not contained in installer 0.5 and CI fails if it doesn't exist
- if not exist "C:\PX4\home" mkdir C:\PX4\home
-# setup the environmental variables to work within the installed cygwin toolchain
- call C:\PX4\toolchain\scripts\setup-environment.bat x
-# safe the repopath for switching to it in cygwin bash
- for /f %%i in ('cygpath -u %%CD%%') do set repopath=%%i
-# build the make target
- call bash --login -c "cd $repopath && make $PX4_CONFIG"
-
-# Note: using bash --login is important
-# because otherwise certain things (like python; import numpy) do not work
-
-cache:
-# cache the entire toolchain installation folder to avoid
-# downloading it from AWS S3 and installing the MSI each time
-# it's ~1.8GB > 1GB free limit for build caches
- C:\PX4 -> appveyor.yml
@@ -53,6 +53,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
 CONFIG_MODULES_EVENTS=y
@@ -16,8 +16,6 @@ param set-default SENS_EN_INA238 0
 param set-default SENS_EN_INA228 0
 param set-default SENS_EN_INA226 1

-param set-default SYS_USE_IO 1
-
 safety_button start

 if param greater -s UAVCAN_ENABLE 0
@@ -18,7 +18,7 @@

 set HAVE_PM2 yes

-if ver hwtypecmp V5X005000 V5X005001 V5X005002
+if ver hwtypecmp V6XRT005000
 then
 	set HAVE_PM2 no
 fi
@@ -61,8 +61,8 @@ then
 	fi
 fi

-# Internal SPI bus ICM42688p (hard-mounted)
-icm42688p -R 12 -b 1 -s start
+# Internal SPI bus ICM42686p (hard-mounted)
+icm42688p -6 -R 12 -b 1 -s start

 # Internal on IMU SPI BMI088
 bmi088 -A -R 4 -s start
@@ -84,6 +84,8 @@ ist8310 -X -b 1 -R 10 start
 if param compare SENS_INT_BARO_EN 1
 then
 	bmp388 -I -b 3 -a 0x77 start
-	bmp388 -X -b 2 start
 fi
+
+bmp388 -X -b 2 start
+
 unset HAVE_PM2
@@ -1,4 +1,66 @@
+/* Static */
+*(.text.arm_ack_irq)
+*(.text.arm_doirq)
+*(.text.arm_svcall)
+*(.text.arm_switchcontext)
+*(.text.board_autoled_on)
+*(.text.clock_timer)
+*(.text.exception_common)
 *(.text.hrt_absolute_time)
+*(.text.hrt_tim_isr)
+*(.text.imxrt_configwaitints)
+*(.text.imxrt_dma_callback)
+*(.text.imxrt_dmach_interrupt)
+*(.text.imxrt_dmaterminate)
+*(.text.imxrt_edma_interrupt)
+*(.text.imxrt_endwait)
+*(.text.imxrt_gpio3_16_31_interrupt)
+*(.text.imxrt_interrupt)
+*(.text.imxrt_lpi2c_isr)
+*(.text.imxrt_recvdma)
+*(.text.imxrt_tcd_free)
+*(.text.imxrt_timerisr)
+*(.text.imxrt_usbinterrupt)
+*(.text.irq_dispatch)
+*(.text.memcpy)
+*(.text.nxsched_add_blocked)
+*(.text.nxsched_add_prioritized)
+*(.text.nxsched_add_readytorun)
+*(.text.nxsched_get_files)
+*(.text.nxsched_get_tcb)
+*(.text.nxsched_merge_pending)
+*(.text.nxsched_process_timer)
+*(.text.nxsched_remove_blocked)
+*(.text.nxsched_remove_readytorun)
+*(.text.nxsched_resume_scheduler)
+*(.text.nxsched_suspend_scheduler)
+*(.text.nxsem_add_holder)
+*(.text.nxsem_add_holder_tcb)
+*(.text.nxsem_clockwait)
+*(.text.nxsem_foreachholder)
+*(.text.nxsem_freecount0holder)
+*(.text.nxsem_freeholder)
+*(.text.nxsem_post)
+*(.text.nxsem_release_holder)
+*(.text.nxsem_restore_baseprio)
+*(.text.nxsem_tickwait)
+*(.text.nxsem_timeout)
+*(.text.nxsem_trywait)
+*(.text.nxsem_wait)
+*(.text.nxsem_wait_uninterruptible)
+*(.text.nxsig_timedwait)
+*(.text.sched_lock)
+*(.text.sched_note_resume)
+*(.text.sched_note_suspend)
+*(.text.sched_unlock)
+*(.text.sq_addafter)
+*(.text.sq_addlast)
+*(.text.sq_rem)
+*(.text.sq_remafter)
+*(.text.sq_remfirst)
+*(.text.uart_connected)
+*(.text.wd_timer)
+/* Auto-generated */
 *(.text._ZN4uORB7Manager27orb_add_internal_subscriberE6ORB_IDhPj)
 *(.text._ZN13MavlinkStream6updateERKy)
 *(.text._ZN7Mavlink16update_rate_multEv)
@@ -12,20 +74,16 @@
 *(.text._ZN4uORB12Subscription9subscribeEv.part.0)
 *(.text._ZN4uORB7Manager13orb_data_copyEPvS1_Rjb)
 *(.text._ZN4uORB10DeviceNode5writeEP4filePKcj)
-*(.text.exception_common)
 *(.text.strcmp)
 *(.text._ZN4uORB10DeviceNode7publishEPK12orb_metadataPvPKv)
 *(.text._ZN4uORB12DeviceMaster19getDeviceNodeLockedEPK12orb_metadatah)
 *(.text._Z12get_orb_meta6ORB_ID)
-*(.text.nxsem_wait)
 *(.text._ZN9ICM42688P12ProcessAccelERKyPKN20InvenSense_ICM42688P4FIFO4DATAEh)
-*(.text.nxsem_post)
 *(.text._ZN3px49WorkQueue3RunEv)
 *(.text._ZN9ICM42688P11ProcessGyroERKyPKN20InvenSense_ICM42688P4FIFO4DATAEh)
 *(.text._ZN4EKF23RunEv)
 *(.text.imxrt_lpspi_exchange)
 *(.text.imxrt_dmach_xfrsetup)
-*(.text.arm_doirq)
 *(.text._ZN7sensors10VehicleIMU7PublishEv)
 *(.text._ZN4math17WelfordMeanVectorIfLj3EE6updateERKN6matrix6VectorIfLj3EEE)
 *(.text._ZN7sensors10VehicleIMU10UpdateGyroEv)
@@ -40,16 +98,11 @@
 *(.text.perf_set_elapsed.part.0)
 *(.text._ZN4uORB12Subscription6updateEPv)
 *(.text._ZN12PX4Gyroscope10updateFIFOER18sensor_gyro_fifo_s)
-*(.text.hrt_tim_isr)
-*(.text.nxsig_timedwait)
-*(.text.nxsem_foreachholder)
 *(.text._ZN7sensors10VehicleIMU3RunEv)
 *(.text.up_unblock_task)
 *(.text.__aeabi_l2f)
 *(.text._ZN39ControlAllocationSequentialDesaturation23computeDesaturationGainERKN6matrix6VectorIfLj16EEES4_)
-*(.text.sched_unlock)
 *(.text.pthread_mutex_timedlock)
-*(.text.nxsem_restore_baseprio)
 *(.text._ZN7sensors22VehicleAngularVelocity21FilterAngularVelocityEiPfi)
 *(.text._ZN26MulticopterAttitudeControl3RunEv.part.0)
 *(.text._ZN6device3SPI9_transferEPhS1_j)
@@ -59,12 +112,10 @@
 *(.text.fs_getfilep)
 *(.text.MEM_DataCopy0_1)
 *(.text._ZN7sensors19VehicleAcceleration3RunEv)
-*(.text.sched_note_resume)
 *(.text.uart_ioctl)
 *(.text._ZN26MulticopterPositionControl3RunEv.part.0)
 *(.text.pthread_mutex_take)
 *(.text._ZN14ImuDownSampler6updateERKN9estimator9imuSampleE)
-*(.text.irq_dispatch)
 *(.text._ZN39ControlAllocationSequentialDesaturation6mixYawEv)
 *(.text._ZN16ControlAllocator25publish_actuator_controlsEv.part.0)
 *(.text._ZN9ICM42688P7RunImplEv)
@@ -74,39 +125,27 @@
 *(.text._ZN7sensors22VehicleAngularVelocity21SensorSelectionUpdateERKyb)
 *(.text._ZN3px49WorkQueue3AddEPNS_8WorkItemE)
 *(.text.wd_start)
-*(.text.sq_rem)
-*(.text.nxsem_add_holder_tcb)
-*(.text.imxrt_dmaterminate)
 *(.text.hrt_call_enter)
 *(.text._ZN4EKF220PublishLocalPositionERKy)
 *(.text._mav_finalize_message_chan_send)
-*(.text.nxsched_add_blocked)
-*(.text.arm_switchcontext)
 *(.text._ZN3Ekf19fixCovarianceErrorsEb)
-*(.text.nxsched_add_prioritized)
 *(.text._ZN7sensors22VehicleAngularVelocity16ParametersUpdateEb)
 *(.text.ioctl)
 *(.text._ZN6events12SendProtocol6updateERKy)
-*(.text.imxrt_dmach_interrupt)
-*(.text.sched_lock)
 *(.text._ZN6device3SPI8transferEPhS1_j)
 *(.text._ZN27MavlinkStreamDistanceSensor4sendEv)
 *(.text.hrt_call_internal)
-*(.text.arm_svcall)
 *(.text._ZN39ControlAllocationSequentialDesaturation18mixAirmodeDisabledEv)
 *(.text._ZN7Mavlink15get_free_tx_bufEv)
 *(.text.nx_poll)
-*(.text.sched_note_suspend)
 *(.text._ZN15MavlinkReceiver3runEv)
 *(.text._ZN9ICM42688P18ProcessTemperatureEPKN20InvenSense_ICM42688P4FIFO4DATAEh)
 *(.text._ZN15OutputPredictor19correctOutputStatesEyRKN6matrix10QuaternionIfEERKNS0_7Vector3IfEES8_S8_S8_)
 *(.text._ZN3Ekf12predictStateERKN9estimator9imuSampleE)
 *(.text._ZN3px46logger6Logger3runEv)
-*(.text.nxsem_freecount0holder)
 *(.text._ZN4uORB20SubscriptionInterval7updatedEv)
 *(.text._ZN24MavlinkStreamCommandLong4sendEv)
 *(.text._ZN9Commander3runEv)
-*(.text.nxsem_release_holder)
 *(.text._ZN3Ekf17predictCovarianceERKN9estimator9imuSampleE)
 *(.text.wd_cancel)
 *(.text._ZN7Sensors3RunEv)
@@ -123,16 +162,13 @@
 *(.text._ZN16ControlAllocator32publish_control_allocator_statusEi)
 *(.text.__ieee754_atan2f)
 *(.text._ZNK18DynamicSparseLayer3getEt)
-*(.text.nxsched_remove_readytorun)
 *(.text.__udivmoddi4)
 *(.text._ZN8Failsafe17checkStateAndModeERKyRKN12FailsafeBase5StateERK16failsafe_flags_s)
 *(.text._ZN29MavlinkStreamHygrometerSensor4sendEv)
-*(.text.nxsched_remove_blocked)
 *(.text.pthread_mutex_give)
 *(.text._ZN3Ekf18controlFusionModesERKN9estimator9imuSampleE)
 *(.text._ZN4cdev4CDev11poll_notifyEm)
 *(.text.file_vioctl)
-*(.text.wd_timer)
 *(.text._ZN7sensors18VotedSensorsUpdate11sensorsPollER17sensor_combined_s)
 *(.text.nxsig_nanosleep)
 *(.text.imxrt_lpspi1select)
@@ -148,7 +184,6 @@
 *(.text.cdcuart_ioctl)
 *(.text.cdcacm_sndpacket)
 *(.text._ZN7sensors22VehicleAngularVelocity16SensorBiasUpdateEb)
-*(.text.nxsched_merge_pending)
 *(.text._ZN13MavlinkStream11update_dataEv)
 *(.text._ZN7sensors18VotedSensorsUpdate21calcGyroInconsistencyEv)
 *(.text.param_set_used)
@@ -162,18 +197,14 @@
 *(.text._ZN22MavlinkStreamCollision4sendEv)
 *(.text.imxrt_lpi2c_transfer)
 *(.text.uart_putxmitchar)
-*(.text.nxsem_tickwait)
 *(.text.clock_nanosleep)
-*(.text.memcpy)
 *(.text.up_release_pending)
 *(.text.MEM_DataCopy0)
 *(.text._ZN22MavlinkStreamGPSRawInt4sendEv)
 *(.text.dq_rem)
 *(.text._ZN15GyroCalibration3RunEv.part.0)
-*(.text.imxrt_edma_interrupt)
 *(.text._ZN7sensors18VotedSensorsUpdate22calcAccelInconsistencyEv)
 *(.text._ZN24MavlinkStreamADSBVehicle4sendEv)
-*(.text.nxsched_process_timer)
 *(.text.sinf)
 *(.text.hrt_call_after)
 *(.text._ZN39ControlAllocationSequentialDesaturation8allocateEv)
@@ -184,8 +215,6 @@
 *(.text._ZN20MavlinkStreamESCInfo4sendEv)
 *(.text.sem_post)
 *(.text._ZN3px417ScheduledWorkItem15ScheduleDelayedEm)
-*(.text.nxsched_resume_scheduler)
-*(.text.nxsched_add_readytorun)
 *(.text._ZN10FlightTaskC1Ev)
 *(.text.usleep)
 *(.text._ZN14FlightTaskAutoC1Ev)
@@ -194,7 +223,6 @@
 *(.text.imxrt_gpio_write)
 *(.text._ZN3Ekf6updateEv)
 *(.text.__ieee754_acosf)
-*(.text.nxsem_wait_uninterruptible)
 *(.text._ZN3Ekf20updateIMUBiasInhibitERKN9estimator9imuSampleE)
 *(.text._ZN9Commander13dataLinkCheckEv)
 *(.text._ZN17FlightModeManager10switchTaskE15FlightTaskIndex)
@@ -204,10 +232,8 @@
 *(.text._ZN18MavlinkStreamDebug4sendEv)
 *(.text._ZN27MavlinkStreamServoOutputRawILi0EE4sendEv)
 *(.text.asinf)
-*(.text.nxsem_freeholder)
 *(.text._ZN6matrix5EulerIfEC1ERKNS_3DcmIfEE)
 *(.text._ZN4EKF227PublishInnovationTestRatiosERKy)
-*(.text.imxrt_gpio3_16_31_interrupt)
 *(.text._ZN4EKF213PublishStatusERKy)
 *(.text._ZN4EKF226PublishInnovationVariancesERKy)
 *(.text._ZN13land_detector23MulticopterLandDetector25_get_ground_contact_stateEv)
@@ -222,7 +248,6 @@
 *(.text._ZNK10ConstLayer3getEt)
 *(.text.__aeabi_uldivmod)
 *(.text.up_udelay)
-*(.text.imxrt_usbinterrupt)
 *(.text.up_idle)
 *(.text._ZN20MavlinkStreamGPS2Raw4sendEv)
 *(.text._ZN4EKF217UpdateCalibrationERKyRNS_19InFlightCalibrationERKN6matrix7Vector3IfEES8_fbb)
@@ -268,11 +293,9 @@
 *(.text._ZN36MavlinkStreamPositionTargetGlobalInt4sendEv)
 *(.text._ZN4uORB12Subscription4copyEPv)
 *(.text._ZN7sensors19VehicleAcceleration21SensorSelectionUpdateEb)
-*(.text.nxsem_add_holder)
 *(.text.crc_accumulate)
 *(.text._ZN3px46logger6Logger13update_paramsEv)
 *(.text._ZN11calibration14DeviceExternalEm)
-*(.text.sq_addafter)
 *(.text._ZN25MavlinkStreamHomePosition8get_sizeEv)
 *(.text.imxrt_lpspi_modifyreg32)
 *(.text._ZN7sensors19VehicleAcceleration16SensorBiasUpdateEb)
@@ -280,7 +303,6 @@
 *(.text._ZNK6matrix6MatrixIfLj3ELj1EEmlEf)
 *(.text._ZN6matrix5EulerIfEC1ERKNS_10QuaternionIfEE)
 *(.text.imxrt_queuedtd)
-*(.text.nxsched_suspend_scheduler)
 *(.text._ZN27MavlinkStreamDistanceSensor8get_sizeEv)
 *(.text._ZN3Ekf16fuseVelPosHeightEffi)
 *(.text._ZN3Ekf23controlBaroHeightFusionEv)
@@ -294,7 +316,6 @@
 *(.text._ZN7sensors14VehicleAirData3RunEv)
 *(.text.perf_count)
 *(.text._ZN3Ekf16controlMagFusionEv)
-*(.text.nxsem_clockwait)
 *(.text.pthread_sem_give)
 *(.text._ZN7sensors10VehicleIMU16ParametersUpdateEb)
 *(.text._ZN30MavlinkStreamUTMGlobalPosition4sendEv)
@@ -302,13 +323,11 @@
 *(.text._ZN12I2CSPIDriverI9ICM42688PE3RunEv)
 *(.text._ZN17ObstacleAvoidanceC1EP12ModuleParams)
 *(.text.imxrt_epcomplete.constprop.0)
-*(.text.imxrt_tcd_free)
 *(.text._ZNK6matrix6MatrixIfLj3ELj1EEmiERKS1_)
 *(.text._ZN9Commander30handleModeIntentionAndFailsafeEv)
 *(.text.perf_event_count)
 *(.text._ZN4EKF215PublishAttitudeERKy)
 *(.text._ZN19MavlinkStreamRawRpm8get_sizeEv)
-*(.text.nxsem_trywait)
 *(.text._ZNK3px46atomicIbE4loadEv)
 *(.text._ZN29MavlinkStreamHygrometerSensor8get_sizeEv)
 *(.text.pthread_mutex_add)
@@ -332,7 +351,6 @@
 *(.text._ZN3Ekf31checkVerticalAccelerationHealthERKN9estimator9imuSampleE)
 *(.text._ZN6matrix6MatrixIfLj3ELj1EEC1ERKS1_)
 *(.text.udp_pollsetup)
-*(.text.nxsem_timeout)
 *(.text._ZL14timer_callbackPv)
 *(.text._ZN3Ekf4fuseERKN6matrix6VectorIfLj23EEEf)
 *(.text._ZN13land_detector23MulticopterLandDetector22_set_hysteresis_factorEi)
@@ -396,7 +414,6 @@
 *(.text._ZN17MavlinkLogHandler4sendEv)
 *(.text._ZN7control10SuperBlock5setDtEf)
 *(.text._ZN29MavlinkStreamMountOrientation8get_sizeEv)
-*(.text.board_autoled_on)
 *(.text._ZN5PX4IO13io_get_statusEv)
 *(.text._ZN26MulticopterAttitudeControl3RunEv)
 *(.text._ZThn16_N31ActuatorEffectivenessMultirotor22getEffectivenessMatrixERN21ActuatorEffectiveness13ConfigurationE25EffectivenessUpdateReason)
@@ -432,7 +449,6 @@
 *(.text._ZN36MavlinkStreamGimbalDeviceSetAttitude4sendEv)
 *(.text._ZN16PreFlightChecker6updateEfRK23estimator_innovations_s)
 *(.text._ZN4math13expo_deadzoneIfEEKT_RS2_S3_S3_.isra.0)
-*(.text.nxsched_get_tcb)
 *(.text._ZN19StickAccelerationXYC1EP12ModuleParams)
 *(.text.imxrt_epsubmit)
 *(.text._ZN15PositionControl6updateEf)
@@ -455,7 +471,6 @@
 *(.text._ZN6Sticks25checkAndUpdateStickInputsEv)
 *(.text.atan2f)
 *(.text._ZN23MavlinkStreamRCChannels4sendEv)
-*(.text.sq_remfirst)
 *(.text._ZN4EKF221UpdateExtVisionSampleER17ekf2_timestamps_s)
 *(.text.imxrt_dmach_stop)
 *(.text._ZN9Commander16handleAutoDisarmEv)
@@ -488,7 +503,6 @@
 *(.text._ZN33FlightTaskManualAltitudeSmoothVelC1Ev)
 *(.text.powf)
 *(.text._ZN4EKF217PublishEventFlagsERKy)
-*(.text.sq_remafter)
 *(.text._ZN17FlightTaskDescend6updateEv)
 *(.text.imxrt_iomux_configure)
 *(.text.hrt_store_absolute_time)
@@ -593,7 +607,6 @@
 *(.text._ZN20MavlinkStreamESCInfo8get_sizeEv)
 *(.text._ZNK6matrix6VectorIfLj2EE4normEv)
 *(.text._Z15arm_auth_updateyb)
-*(.text.imxrt_lpi2c_isr)
 *(.text._ZN3LED5ioctlEP4fileim)
 *(.text._ZNK3px46logger9LogWriter20had_file_write_errorEv)
 *(.text._ZN29MavlinkStreamLocalPositionNED4sendEv)
@@ -627,13 +640,11 @@
 *(.text._ZN4EKF216PublishEvPosBiasERKy)
 *(.text._ZN21MavlinkStreamAttitude8get_sizeEv)
 *(.text._ZThn16_N7sensors19VehicleAcceleration3RunEv)
-*(.text.imxrt_timerisr)
 *(.text._ZN3Ekf24controlRangeHeightFusionEv)
 *(.text._ZN33MavlinkStreamTimeEstimateToTarget4sendEv)
 *(.text._ZN6matrix6MatrixIfLj3ELj1EE6setAllEf)
 *(.text._ZN12ModuleParamsD1Ev)
 *(.text._ZN3Ekf20controlFakeHgtFusionEv)
-*(.text.sq_addlast)
 *(.text.imxrt_reqcomplete)
 *(.text._ZNK6matrix7Vector3IfEmlEf)
 *(.text._ZN18ZeroVelocityUpdate6updateER3EkfRKN9estimator9imuSampleE)
@@ -653,7 +664,6 @@
 *(.text._ZN13BatteryChecks14checkAndReportERK7ContextR6Report)
 *(.text._ZN18DataValidatorGroup16get_sensor_stateEj)
 *(.text.uart_xmitchars_done)
-*(.text.nxsched_get_files)
 *(.text._ZN4EKF225PublishYawEstimatorStatusERKy)
 *(.text.sin)
 *(.text._ZN16PreFlightChecker27preFlightCheckVertVelFailedERK23estimator_innovations_sf)
@@ -702,7 +712,6 @@
 *(.text._ZThn8_N3ADC3RunEv)
 *(.text._ZN11StickTiltXYC1EP12ModuleParams)
 *(.text._ZN12SafetyButton3RunEv)
-*(.text.arm_ack_irq)
 *(.text._ZN6BMP38811set_op_modeEh)
 *(.text._ZN3GPS8callbackE15GPSCallbackTypePviS1_)
 *(.text._ZN13AnalogBattery19get_current_channelEv)
@@ -36,7 +36,6 @@ if("${PX4_BOARD_LABEL}" STREQUAL  "bootloader")
 		bootloader_main.c
 		init.c
 		usb.c
-		imxrt_romapi.c
 		imxrt_flexspi_nor_boot.c
 		imxrt_flexspi_nor_flash.c
 		imxrt_clockconfig.c
@@ -48,6 +47,7 @@ if("${PX4_BOARD_LABEL}" STREQUAL  "bootloader")
 			nuttx_drivers # sdio
 			px4_layer #gpio
 			arch_io_pins # iotimer
+			arch_board_romapi
 			bootloader
 	)
 	target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/bootloader/common)
@@ -72,7 +72,6 @@ else()
 		spi.cpp
 		timer_config.cpp
 		usb.c
-		imxrt_romapi.c
 		imxrt_flexspi_fram.c
 		imxrt_flexspi_nor_boot.c
 		imxrt_flexspi_nor_flash.c
@@ -83,6 +82,7 @@ else()
 	target_link_libraries(drivers_board
 		PRIVATE
 			arch_board_hw_info
+			arch_board_romapi
 			arch_spi
 			drivers__led # drv_led_start
 			nuttx_arch # sdio
@@ -297,6 +297,7 @@
 #define GPIO_HW_VER_SENSE     /* GPIO_AD_23 GPIO9 Pin 22 */  ADC_GPIO(5, 22)
 #define HW_INFO_INIT_PREFIX   "V6XRT"
 #define V6XRT_00             HW_VER_REV(0x0,0x0) // First Release
+#define V6XRT_50             HW_VER_REV(0x5,0x0) // HB Mini Rev 0

 #define BOARD_I2C_LATEINIT 1 /* See Note about SE550 Eanable */

@@ -539,6 +540,8 @@

 /* This board provides the board_on_reset interface */

+#define BOARD_HAS_ISP_BOOTLOADER 1
+
 #define BOARD_HAS_ON_RESET 1

 #define PX4_GPIO_INIT_LIST { \
@@ -22,7 +22,7 @@
 * Included Files
 ****************************************************************************/

-#include "imxrt_flexspi_nor_flash.h"
+#include <px4_arch/imxrt_flexspi_nor_flash.h>

 /****************************************************************************
 * Public Data
@@ -66,8 +66,7 @@

 #include "arm_internal.h"
 #include "imxrt_flexspi_nor_boot.h"
-#include "imxrt_flexspi_nor_flash.h"
-#include "imxrt_romapi.h"
+#include <px4_arch/imxrt_flexspi_nor_flash.h>
 #include "imxrt_iomuxc.h"
 #include "imxrt_flexcan.h"
 #include "imxrt_enet.h"
@@ -79,10 +78,12 @@

 #include <arch/board/board.h>

+
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_board_led.h>
 #include <systemlib/px4_macros.h>
 #include <px4_arch/io_timer.h>
+#include <px4_arch/imxrt_romapi.h>
 #include <px4_platform_common/init.h>
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_determine_hw_info.h>
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2018, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018, 2021, 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
@@ -46,12 +46,14 @@
 * Included Files
 ****************************************************************************/

-#include <px4_platform_common/px4_config.h>
+#include <nuttx/config.h>
+#include <board_config.h>
+
+#include <inttypes.h>
 #include <stdbool.h>
 #include <syslog.h>

 #include "systemlib/px4_macros.h"
-#include "px4_log.h"

 /****************************************************************************
 * Pre-Processor Definitions
@@ -98,8 +100,30 @@ static const px4_hw_mft_item_t hw_mft_list_V00[] = {
 	},
 };

+static const px4_hw_mft_item_t hw_mft_list_V50[] = {
+	{
+		//  PX4_MFT_PX4IO
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_unknown,
+	},
+	{
+		// PX4_MFT_USB
+		.present     = 1,
+		.mandatory   = 1,
+		.connection  = px4_hw_con_onboard,
+	},
+	{
+		// PX4_MFT_CAN2
+		.present     = 0,
+		.mandatory   = 0,
+		.connection  = px4_hw_con_unknown,
+	},
+};
+
 static px4_hw_mft_list_entry_t mft_lists[] = {
 	{V6XRT_00, hw_mft_list_V00, arraySize(hw_mft_list_V00)},
+	{V6XRT_50, hw_mft_list_V50, arraySize(hw_mft_list_V50)}, // HB Mini
 };

 /************************************************************************************
@@ -122,7 +146,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 	static px4_hw_mft_list_entry boards_manifest = px4_hw_mft_list_uninitialized;

 	if (boards_manifest == px4_hw_mft_list_uninitialized) {
-		uint32_t ver_rev = board_get_hw_version() << 8;
+		uint32_t ver_rev = board_get_hw_version() << 16;
 		ver_rev |= board_get_hw_revision();

 		for (unsigned i = 0; i < arraySize(mft_lists); i++) {
@@ -133,7 +157,7 @@ __EXPORT px4_hw_mft_item board_query_manifest(px4_hw_mft_item_id_t id)
 		}

 		if (boards_manifest == px4_hw_mft_list_uninitialized) {
-			PX4_ERR("Board %4" PRIx32 " is not supported!", ver_rev);
+			syslog(LOG_ERR, "[boot] Board %08" PRIx32 " is not supported!\n", ver_rev);
 		}
 	}

@@ -65,7 +65,7 @@

 constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::LPSPI1, {
-		initSPIDevice(DRV_IMU_DEVTYPE_ICM42688P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */
+		initSPIDevice(DRV_IMU_DEVTYPE_ICM42686P, SPI::CS{GPIO::Port2, GPIO::Pin11}, SPI::DRDY{GPIO::Port3, GPIO::Pin19}), /* GPIO_EMC_B2_01 GPIO2_IO11, GPIO_AD_20, GPIO3_IO19 */
 	}, {GPIO::Port2, GPIO::Pin1}), // Power GPIO_EMC_B1_33  GPIO2_IO01

 	initSPIBus(SPI::Bus::LPSPI2, {
@@ -12,6 +12,7 @@ CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
+CONFIG_MODULES_DIFFERENTIAL_DRIVE_CONTROL=y
 CONFIG_MODULES_EKF2=y
 CONFIG_EKF2_AUX_GLOBAL_POSITION=y
 CONFIG_MODULES_EVENTS=y
@@ -1,205 +0,0 @@
-<?xml version="1.0" encoding="UTF-8" standalone="no"?>
-<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
-	<storageModule moduleId="org.eclipse.cdt.core.settings">
-		<cconfiguration id="cdt.managedbuild.toolchain.gnu.cross.base.432866957">
-			<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.cross.base.432866957" moduleId="org.eclipse.cdt.core.settings" name="Default">
-				<externalSettings/>
-				<extensions>
-					<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
-					<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-					<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
-				</extensions>
-			</storageModule>
-			<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-				<configuration artifactName="PX4-Firmware" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.cross.base.432866957" name="Default" parent="org.eclipse.cdt.build.core.emptycfg">
-					<folderInfo id="cdt.managedbuild.toolchain.gnu.cross.base.432866957.1182904611" name="/" resourcePath="">
-						<toolChain id="cdt.managedbuild.toolchain.gnu.base.716090342" name="Linux GCC" superClass="cdt.managedbuild.toolchain.gnu.base">
-							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="cdt.managedbuild.target.gnu.platform.base.328447809" name="Debug Platform" osList="linux,hpux,aix,qnx" superClass="cdt.managedbuild.target.gnu.platform.base"/>
-							<builder id="cdt.managedbuild.target.gnu.builder.base.977864740" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" superClass="cdt.managedbuild.target.gnu.builder.base">
-								<outputEntries>
-									<entry excluding="build*" flags="VALUE_WORKSPACE_PATH" kind="outputPath" name=""/>
-								</outputEntries>
-							</builder>
-							<tool id="cdt.managedbuild.tool.gnu.archiver.base.1653222719" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.base.331692139" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.base">
-								<option id="gnu.cpp.compiler.option.include.paths.1654158476" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" valueType="includePath">
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/uORB}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/lib/ecl}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/modules/systemlib}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/lib/geo}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/platforms}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/include}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/uORB/topics}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${git_work_tree}/build/px4_sitl_default/&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${git_work_tree}/build/px4_sitl_default/src/modules/systemlib/param&quot;"/>
-								</option>
-								<option id="gnu.cpp.compiler.option.preprocessor.def.1050909243" name="Defined symbols (-D)" superClass="gnu.cpp.compiler.option.preprocessor.def" valueType="definedSymbols">
-									<listOptionValue builtIn="false" value="__PX4_POSIX=1"/>
-									<listOptionValue builtIn="false" value="__PX4_LINUX=1"/>
-									<listOptionValue builtIn="false" value="__cplusplus=1"/>
-									<listOptionValue builtIn="false" value="__EXPORT"/>
-								</option>
-								<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.322788461" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
-							</tool>
-							<tool id="cdt.managedbuild.tool.gnu.c.compiler.base.1055476396" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.base">
-								<option id="gnu.c.compiler.option.include.paths.1309473256" name="Include paths (-I)" superClass="gnu.c.compiler.option.include.paths" valueType="includePath">
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/uORB}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/lib/ecl}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/modules/systemlib}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/lib/geo}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/platforms}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/include}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/uORB/topics}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${git_work_tree}/build/px4_sitl_default/&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${git_work_tree}/build/px4_sitl_default/src/modules/systemlib/param&quot;"/>
-								</option>
-								<option id="gnu.c.compiler.option.preprocessor.def.symbols.734675451" name="Defined symbols (-D)" superClass="gnu.c.compiler.option.preprocessor.def.symbols" valueType="definedSymbols">
-									<listOptionValue builtIn="false" value="__PX4_POSIX=1"/>
-									<listOptionValue builtIn="false" value="__PX4_LINUX=1"/>
-									<listOptionValue builtIn="false" value="__cplusplus=1"/>
-									<listOptionValue builtIn="false" value="__EXPORT"/>
-								</option>
-								<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1650107366" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
-							</tool>
-							<tool id="cdt.managedbuild.tool.gnu.c.linker.base.1577413388" name="GCC C Linker" superClass="cdt.managedbuild.tool.gnu.c.linker.base"/>
-							<tool id="cdt.managedbuild.tool.gnu.cpp.linker.base.1471249362" name="GCC C++ Linker" superClass="cdt.managedbuild.tool.gnu.cpp.linker.base">
-								<inputType id="cdt.managedbuild.tool.gnu.cpp.linker.input.1062268162" superClass="cdt.managedbuild.tool.gnu.cpp.linker.input">
-									<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
-									<additionalInput kind="additionalinput" paths="$(LIBS)"/>
-								</inputType>
-							</tool>
-							<tool id="cdt.managedbuild.tool.gnu.assembler.base.900091175" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.base">
-								<option id="gnu.both.asm.option.include.paths.2025449581" name="Include paths (-I)" superClass="gnu.both.asm.option.include.paths" valueType="includePath">
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/uORB}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/lib/ecl}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/modules/systemlib}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/lib/geo}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/platforms}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/src/include}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware/uORB/topics}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${workspace_loc:/PX4-Firmware}&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${git_work_tree}/build/px4_sitl_default/&quot;"/>
-									<listOptionValue builtIn="false" value="&quot;${git_work_tree}/build/px4_sitl_default/src/modules/systemlib/param&quot;"/>
-								</option>
-								<inputType id="cdt.managedbuild.tool.gnu.assembler.input.674673703" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
-							</tool>
-						</toolChain>
-					</folderInfo>
-					<sourceEntries>
-						<entry flags="VALUE_WORKSPACE_PATH" kind="sourcePath" name="src"/>
-					</sourceEntries>
-				</configuration>
-			</storageModule>
-			<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
-		</cconfiguration>
-	</storageModule>
-	<storageModule moduleId="cdtBuildSystem" version="4.0.0">
-		<project id="PX4Firmware.null.2007659608" name="PX4Firmware"/>
-	</storageModule>
-	<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
-	<storageModule moduleId="refreshScope" versionNumber="2">
-		<configuration configurationName="Default">
-			<resource resourceType="PROJECT" workspacePath="/PX4-Firmware"/>
-		</configuration>
-	</storageModule>
-	<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
-	<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
-		<buildTargets>
-			<target name="px4_fmu-v2_default" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>px4_fmu-v2_default</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="px4_fmu-v4_default" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments/>
-				<buildTarget>px4_fmu-v4_default</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="all" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="px4_sitl_default" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments/>
-				<buildTarget>px4_sitl_default</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments/>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="distclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>distclean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="tests" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments/>
-				<buildTarget>tests</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="submodulesclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildTarget>submodulesclean</buildTarget>
-				<stopOnError>false</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="quick_check" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments/>
-				<buildTarget>quick_check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-		</buildTargets>
-	</storageModule>
-	<storageModule moduleId="scannerConfiguration">
-		<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.cross.base.432866957;cdt.managedbuild.toolchain.gnu.cross.base.432866957.1182904611;cdt.managedbuild.tool.gnu.cross.cpp.compiler.1747549746;cdt.managedbuild.tool.gnu.cpp.compiler.input.576083683">
-			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.cross.base.432866957;cdt.managedbuild.toolchain.gnu.cross.base.432866957.1182904611;cdt.managedbuild.tool.gnu.cpp.compiler.base.331692139;cdt.managedbuild.tool.gnu.cpp.compiler.input.322788461">
-			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.cross.base.432866957;cdt.managedbuild.toolchain.gnu.cross.base.432866957.1182904611;cdt.managedbuild.tool.gnu.c.compiler.base.1055476396;cdt.managedbuild.tool.gnu.c.compiler.input.1650107366">
-			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		</scannerConfigBuildInfo>
-		<scannerConfigBuildInfo instanceId="cdt.managedbuild.toolchain.gnu.cross.base.432866957;cdt.managedbuild.toolchain.gnu.cross.base.432866957.1182904611;cdt.managedbuild.tool.gnu.cross.c.compiler.595284732;cdt.managedbuild.tool.gnu.c.compiler.input.2047539569">
-			<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
-		</scannerConfigBuildInfo>
-	</storageModule>
-</cproject>
@@ -1,69 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<projectDescription>
-	<name>PX4-Firmware</name>
-	<comment></comment>
-	<projects>
-	</projects>
-	<buildSpec>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
-			<triggers>clean,full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-		<buildCommand>
-			<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
-			<triggers>full,incremental,</triggers>
-			<arguments>
-			</arguments>
-		</buildCommand>
-	</buildSpec>
-	<natures>
-		<nature>org.eclipse.cdt.core.cnature</nature>
-		<nature>org.eclipse.cdt.core.ccnature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
-		<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
-	</natures>
-	<linkedResources>
-		<link>
-			<name>topics_sources</name>
-			<type>2</type>
-			<locationURI>uORB_SRC</locationURI>
-		</link>
-		<link>
-			<name>uORB</name>
-			<type>2</type>
-			<locationURI>uORB_LOC</locationURI>
-		</link>
-	</linkedResources>
-	<filteredResources>
-		<filter>
-			<id>1457837186676</id>
-			<name></name>
-			<type>26</type>
-			<matcher>
-				<id>org.eclipse.ui.ide.multiFilter</id>
-				<arguments>1.0-name-matches-false-false-.git</arguments>
-			</matcher>
-		</filter>
-		<filter>
-			<id>1457837186687</id>
-			<name></name>
-			<type>10</type>
-			<matcher>
-				<id>org.eclipse.ui.ide.multiFilter</id>
-				<arguments>1.0-name-matches-true-false-build*</arguments>
-			</matcher>
-		</filter>
-	</filteredResources>
-	<variableList>
-		<variable>
-			<name>uORB_LOC</name>
-			<value>$%7BPROJECT_LOC%7D/build/px4_sitl_default/uORB</value>
-		</variable>
-		<variable>
-			<name>uORB_SRC</name>
-			<value>$%7BPROJECT_LOC%7D/build/px4_sitl_default/msg/topics_sources</value>
-		</variable>
-	</variableList>
-</projectDescription>
@@ -69,6 +69,7 @@ set(msg_files
 	DebugKeyValue.msg
 	DebugValue.msg
 	DebugVect.msg
+	DifferentialDriveSetpoint.msg
 	DifferentialPressure.msg
 	DistanceSensor.msg
 	Ekf2Timestamps.msg
@@ -0,0 +1,4 @@
+uint64 timestamp # time since system start (microseconds)
+
+float32 speed # [m/s] collective roll-off speed in body x-axis
+float32 yaw_rate # [rad/s] yaw rate
@@ -1,4 +1,5 @@
 # Mode completion result, published by an active mode.
+# The possible values of nav_state are defined in the VehicleStatus msg.
 # Note that this is not always published (e.g. when a user switches modes or on
 # failsafe activation)
 uint64 timestamp				 # time since system start (microseconds)
@@ -10,5 +11,5 @@ uint8 RESULT_FAILURE_OTHER = 100 # Mode failed (generic error)

 uint8 result                     # One of RESULT_*

-uint8 nav_state                  # Source mode
+uint8 nav_state                  # Source mode (values in VehicleStatus)

@@ -22,11 +22,7 @@ float32 vz			# local velocity setpoint in m/s in NED
 float64 lat			# latitude, in deg
 float64 lon			# longitude, in deg
 float32 alt			# altitude AMSL, in m
-float32 yaw			# yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw
-bool yaw_valid			# true if yaw setpoint valid
-
-float32 yawspeed		# yawspeed (only for multirotors, in rad/s)
-bool yawspeed_valid		# true if yawspeed setpoint valid
+float32 yaw			# yaw (only in hover), in rad [-PI..PI), NaN = leave to flight task

 float32 loiter_radius		# loiter major axis radius in m
 float32 loiter_minor_radius	# loiter minor axis radius (used for non-circular loiter shapes) in m
@@ -39,5 +35,3 @@ float32 acceptance_radius   # navigation acceptance_radius if we're doing waypoi
 float32 cruising_speed		# the generally desired cruising speed (not a hard constraint)
 bool gliding_enabled		# commands the vehicle to glide if the capability is available (fixed wing only)
 float32 cruising_throttle	# the generally desired cruising throttle (not a hard constraint), only has an effect for rover
-
-bool disable_weather_vane   # VTOL: disable (in auto mode) the weather vane feature that turns the nose into the wind
@@ -91,7 +91,7 @@ extern pthread_mutex_t px4_modules_mutex;
 *      static int custom_command(int argc, char *argv[])
 *      {
 *              // support for custom commands
- *              // it none are supported, just do:
+ *              // if none are supported, just do:
 *              return print_usage("unrecognized command");
 *      }
 *
@@ -70,6 +70,12 @@ __EXPORT int px4_register_shutdown_hook(shutdown_hook_t hook);
 */
 __EXPORT int px4_unregister_shutdown_hook(shutdown_hook_t hook);

+/** Types of reboot requests for PX4 */
+typedef enum {
+	REBOOT_REQUEST = 0,          ///< Normal reboot
+	REBOOT_TO_BOOTLOADER = 1,    ///< Reboot to PX4 bootloader
+	REBOOT_TO_ISP = 2,           ///< Reboot to ISP bootloader
+} reboot_request_t;

 /**
 * Request the system to reboot.
@@ -83,7 +89,7 @@ __EXPORT int px4_unregister_shutdown_hook(shutdown_hook_t hook);
 * @return 0 on success, <0 on error
 */
 #if defined(CONFIG_BOARDCTL_RESET)
-__EXPORT int px4_reboot_request(bool to_bootloader = false, uint32_t delay_us = 0);
+__EXPORT int px4_reboot_request(reboot_request_t request = REBOOT_REQUEST, uint32_t delay_us = 0);
 #endif // CONFIG_BOARDCTL_RESET


@@ -108,6 +108,7 @@ static uint16_t shutdown_counter = 0; ///< count how many times the shutdown wor
 #define SHUTDOWN_ARG_IN_PROGRESS (1<<0)
 #define SHUTDOWN_ARG_REBOOT (1<<1)
 #define SHUTDOWN_ARG_TO_BOOTLOADER (1<<2)
+#define SHUTDOWN_ARG_TO_ISP (1<<3)
 static uint8_t shutdown_args = 0;

 static constexpr int max_shutdown_hooks = 1;
@@ -175,7 +176,17 @@ static void shutdown_worker(void *arg)
 		if (shutdown_args & SHUTDOWN_ARG_REBOOT) {
 #if defined(CONFIG_BOARDCTL_RESET)
 			PX4_INFO_RAW("Reboot NOW.");
-			boardctl(BOARDIOC_RESET, (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) ? 1 : 0);
+
+			if (shutdown_args & SHUTDOWN_ARG_TO_BOOTLOADER) {
+				boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_TO_BOOTLOADER);
+
+			} else if (shutdown_args & SHUTDOWN_ARG_TO_ISP) {
+				boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_TO_ISP);
+
+			} else {
+				boardctl(BOARDIOC_RESET, (uintptr_t)REBOOT_REQUEST);
+			}
+
 #else
 			PX4_PANIC("board reset not available");
 #endif
@@ -206,7 +217,7 @@ static void shutdown_worker(void *arg)
 }

 #if defined(CONFIG_BOARDCTL_RESET)
-int px4_reboot_request(bool to_bootloader, uint32_t delay_us)
+int px4_reboot_request(reboot_request_t request, uint32_t delay_us)
 {
 	pthread_mutex_lock(&shutdown_mutex);

@@ -217,8 +228,11 @@ int px4_reboot_request(bool to_bootloader, uint32_t delay_us)

 	shutdown_args |= SHUTDOWN_ARG_REBOOT;

-	if (to_bootloader) {
+	if (request == REBOOT_TO_BOOTLOADER) {
 		shutdown_args |= SHUTDOWN_ARG_TO_BOOTLOADER;
+
+	} else if (request == REBOOT_TO_ISP) {
+		shutdown_args |= SHUTDOWN_ARG_TO_ISP;
 	}

 	shutdown_time_us = hrt_absolute_time();
@@ -7,8 +7,8 @@
 #include <px4_defines.h>

 #include "hw_config.h"
-#include "imxrt_flexspi_nor_flash.h"
-#include "imxrt_romapi.h"
+#include <px4_arch/imxrt_flexspi_nor_flash.h>
+#include <px4_arch/imxrt_romapi.h>
 #include <hardware/rt117x/imxrt117x_ocotp.h>
 #include <hardware/rt117x/imxrt117x_anadig.h>
 #include <hardware/rt117x/imxrt117x_snvs.h>
@@ -187,7 +187,7 @@ static void mavlink_usb_check(void *arg)

 										if (param1 == 1) {
 											// 1: Reboot autopilot
-											px4_reboot_request(false, 0);
+											px4_reboot_request(REBOOT_REQUEST, 0);

 										} else if (param1 == 2) {
 											// 2: Shutdown autopilot
@@ -197,7 +197,7 @@ static void mavlink_usb_check(void *arg)

 										} else if (param1 == 3) {
 											// 3: Reboot autopilot and keep it in the bootloader until upgraded.
-											px4_reboot_request(true, 0);
+											px4_reboot_request(REBOOT_TO_BOOTLOADER, 0);
 										}
 									}
 								}
@@ -35,6 +35,8 @@ px4_add_library(arch_board_reset
 	board_reset.cpp
 )

+target_link_libraries(arch_board_reset PRIVATE arch_board_romapi)
+
 # up_systemreset
 if (NOT DEFINED CONFIG_BUILD_FLAT)
 	target_link_libraries(arch_board_reset PRIVATE nuttx_karch)
@@ -38,11 +38,16 @@
 */

 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/shutdown.h>
 #include <errno.h>
 #include <nuttx/board.h>
 #include <arm_internal.h>
 #include <hardware/rt117x/imxrt117x_snvs.h>

+
+#include <px4_arch/imxrt_flexspi_nor_flash.h>
+#include <px4_arch/imxrt_romapi.h>
+
 #define BOOT_RTC_SIGNATURE                0xb007b007
 #define PX4_IMXRT_RTC_REBOOT_REG          3
 #define PX4_IMXRT_RTC_REBOOT_REG_ADDRESS  IMXRT_SNVS_LPGPR3
@@ -61,8 +66,13 @@ static int board_reset_enter_bootloader()

 int board_reset(int status)
 {
-	if (status == 1) {
+	if (status == REBOOT_TO_BOOTLOADER) {
 		board_reset_enter_bootloader();
+
+	} else if (status == REBOOT_TO_ISP) {
+		uint32_t arg = 0xeb100000;
+		ROM_API_Init();
+		ROM_RunBootloader(&arg);
 	}

 #if defined(BOARD_HAS_ON_RESET)
@@ -1,5 +1,5 @@
 /****************************************************************************
- * boards/px4/fmu-v6xrt/src/imxrt_flexspi_nor_flash.h
+ * platforms/nuttx/src/px4/nxp/imrt/include/px4_arch/imxrt_flexspi_nor_flash.h
 *
 * Licensed to the Apache Software Foundation (ASF) under one or more
 * contributor license agreements.  See the NOTICE file distributed with
@@ -18,8 +18,8 @@
 *
 ****************************************************************************/

-#ifndef __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_FLEXSPI_NOR_FLASH_H
-#define __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_FLEXSPI_NOR_FLASH_H
+#ifndef __PX4_ARCH_IMXRT_FLEXSPI_NOR_FLASH_H
+#define __PX4_ARCH_IMXRT_FLEXSPI_NOR_FLASH_H

 /****************************************************************************
 * Included Files
@@ -349,4 +349,4 @@ extern const struct flexspi_nor_config_s g_flash_config;
 extern const struct flexspi_nor_config_s g_flash_fast_config;


-#endif /* __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_FLEXSPI_NOR_FLASH_H */
+#endif /* __PX4_ARCH_IMXRT_FLEXSPI_NOR_FLASH_H */
@@ -1,14 +1,14 @@
 /****************************************************************************
- * boards/px4/fmu-v6xrt/src/imxrt_romapi.c
+ * platforms/nuttx/src/px4/nxp/imrt/include/px4_arch/imxrt_romapi.h
 *
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2024 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 *
 ****************************************************************************/
-#ifndef __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_ROMAPI_H
-#define __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_ROMAPI_H
+#ifndef __PX4_ARCH_IMXRT_ROMAPI_H
+#define __PX4_ARCH_IMXRT_ROMAPI_H

 /****************************************************************************
 *
@@ -370,4 +370,8 @@ void ROM_FLEXSPI_NorFlash_ClearCache(uint32_t instance);

 /*@}*/

-#endif /* __BOARDS_PX4_FMU_V6XRT_SRC_IMXRT_ROMAPI_H */
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __PX4_ARCH_IMXRT_ROMAPI_H */
@@ -0,0 +1,36 @@
+############################################################################
+#
+#   Copyright (c) 2019 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(arch_board_romapi
+	imxrt_romapi.c
+)
@@ -1,7 +1,7 @@
 /****************************************************************************
- * boards/px4/fmu-v6xrt/src/imxrt_romapi.c
+ * platforms/nuttx/src/px4/nxp/imrt/romapi/imxrt_romapi.c
 *
- * Copyright 2017-2020 NXP
+ * Copyright 2017-2024 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
@@ -22,8 +22,8 @@

 #include "arm_internal.h"

-#include "imxrt_flexspi_nor_flash.h"
-#include "imxrt_romapi.h"
+#include <px4_arch/imxrt_flexspi_nor_flash.h>
+#include <px4_arch/imxrt_romapi.h>

 #include <hardware/rt117x/imxrt117x_anadig.h>

@@ -38,6 +38,7 @@
 */

 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/shutdown.h>
 #include <errno.h>
 #include <nuttx/board.h>

@@ -72,7 +73,7 @@ static int board_reset_enter_bootloader()

 int board_reset(int status)
 {
-	if (status == 1) {
+	if (status == REBOOT_TO_BOOTLOADER) {
 		board_reset_enter_bootloader();
 	}

@@ -36,7 +36,7 @@ add_subdirectory(adc)
 add_subdirectory(../imxrt/board_critmon board_critmon)
 add_subdirectory(../imxrt/board_hw_info board_hw_info)
 add_subdirectory(../imxrt/board_reset board_reset)
-#add_subdirectory(../imxrt/dshot dshot)
+add_subdirectory(../imxrt/romapi romapi)
 add_subdirectory(../imxrt/hrt hrt)
 add_subdirectory(../imxrt/led_pwm led_pwm)
 add_subdirectory(../imxrt/io_pins io_pins)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021 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
@@ -30,24 +30,7 @@
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
-
 #pragma once

-#include "ActuatorEffectiveness.hpp"

-class ActuatorEffectivenessRoverDifferential: public ActuatorEffectiveness
-{
-public:
-	ActuatorEffectivenessRoverDifferential() = default;
-	virtual ~ActuatorEffectivenessRoverDifferential() = default;
-
-	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
-
-	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
-			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
-
-	const char *name() const override { return "Rover (Differential)"; }
-private:
-	uint32_t _motors_mask{};
-};
+#include "../../../imxrt/include/px4_arch/imxrt_flexspi_nor_flash.h"
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2021 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
@@ -30,30 +30,7 @@
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
+#pragma once

-#include "ActuatorEffectivenessRoverDifferential.hpp"
-#include <ControlAllocation/ControlAllocation.hpp>
-
-using namespace matrix;
-
-bool
-ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &configuration,
-		EffectivenessUpdateReason external_update)
-{
-	if (external_update == EffectivenessUpdateReason::NO_EXTERNAL_UPDATE) {
-		return false;
-	}
-
-	configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, 0.5f}, Vector3f{0.5f, 0.f, 0.f});
-	configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, -0.5f}, Vector3f{0.5f, 0.f, 0.f});
-	_motors_mask = (1u << 0) | (1u << 1);
-	return true;
-}
-
-void ActuatorEffectivenessRoverDifferential::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
-		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
-		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
-{
-	stopMaskedMotorsWithZeroThrust(_motors_mask, actuator_sp);
-}

+#include "../../../imxrt/include/px4_arch/imxrt_romapi.h"
@@ -39,6 +39,7 @@
 */

 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/shutdown.h>
 #include <systemlib/px4_macros.h>
 #include <errno.h>
 #include <nuttx/board.h>
@@ -96,7 +97,7 @@ int board_configure_reset(reset_mode_e mode, uint32_t arg)

 int board_reset(int status)
 {
-	if (status == 1) {
+	if (status == REBOOT_TO_BOOTLOADER) {
 		board_reset_enter_bootloader();
 	}

@@ -39,6 +39,7 @@
 */

 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/shutdown.h>
 #include <errno.h>
 #include <nuttx/board.h>
 #include <hardware/s32k3xx_mc_me.h>
@@ -38,6 +38,7 @@
 */

 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/shutdown.h>
 #include <systemlib/px4_macros.h>
 #include <errno.h>

@@ -119,7 +120,7 @@

 int board_reset(int status)
 {
-	if (status == 1) {
+	if (status == REBOOT_TO_BOOTLOADER) {
 		// board_configure_reset(BOARD_RESET_MODE_BOOT_TO_BL, 0);
 	}

@@ -38,6 +38,7 @@
 */

 #include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/shutdown.h>
 #include <systemlib/px4_macros.h>
 #include <errno.h>
 #include <stm32_pwr.h>
@@ -117,7 +118,7 @@ int board_configure_reset(reset_mode_e mode, uint32_t arg)

 int board_reset(int status)
 {
-	if (status == 1) {
+	if (status == REBOOT_TO_BOOTLOADER) {
 		board_configure_reset(BOARD_RESET_MODE_BOOT_TO_BL, 0);
 	}

@@ -75,7 +75,7 @@
 *      static int custom_command(int argc, char *argv[])
 *      {
 *              // support for custom commands
- *              // it none are supported, just do:
+ *              // if none are supported, just do:
 *              return print_usage("unrecognized command");
 *      }
 *
@@ -126,8 +126,9 @@ int ICM42688P::probe()
 {
 	for (int i = 0; i < 3; i++) {
 		uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
+		uint8_t expected_whoami = isICM686 ? WHOAMI686 : WHOAMI;

-		if (whoami == WHOAMI || (isICM686 && whoami == WHOAMI686)) {
+		if (whoami == expected_whoami) {
 			return PX4_OK;

 		} else {
@@ -53,7 +53,6 @@
 #define CBRK_BUZZER_KEY         782097
 #define CBRK_SUPPLY_CHK_KEY	894281
 #define CBRK_IO_SAFETY_KEY	22027
-#define CBRK_AIRSPD_CHK_KEY	162128
 #define CBRK_FLIGHTTERM_KEY	121212
 #define CBRK_USB_CHK_KEY	197848
 #define CBRK_VTOLARMING_KEY	159753
@@ -69,21 +69,6 @@ PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
 */
 PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 22027);

-/**
- * Circuit breaker for airspeed sensor
- *
- * Setting this parameter to 162128 will disable the check for an airspeed sensor.
- * The sensor driver will not be started and it cannot be calibrated.
- * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
- *
- * @reboot_required true
- * @min 0
- * @max 162128
- * @category Developer
- * @group Circuit Breaker
- */
-PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
-
 /**
 * Circuit breaker for flight termination
 *
@@ -35,6 +35,8 @@ public:
 		assert(y0 + Q <= N);
 	}

+	Slice(const Slice<Type, P, Q, M, N> &other) = default;
+
 	const Type &operator()(size_t i, size_t j) const
 	{
 		assert(i < P);
@@ -52,6 +54,12 @@ public:
 		return (*_data)(_x0 + i, _y0 + j);
 	}

+	// Separate function needed otherwise the default copy constructor matches before the deep copy implementation
+	Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, M, N> &other)
+	{
+		return this->operator=<M, N>(other);
+	}
+
 	template<size_t MM, size_t NN>
 	Slice<Type, P, Q, M, N> &operator=(const Slice<Type, P, Q, MM, NN> &other)
 	{
@@ -262,3 +262,12 @@ TEST(MatrixSliceTest, Slice)
 	float O_check_data_12 [4] = {2.5, 3, 4, 5};
 	EXPECT_EQ(res_12, (SquareMatrix<float, 2>(O_check_data_12)));
 }
+
+TEST(MatrixSliceTest, XYAssignmentTest)
+{
+	Vector3f a(1, 2, 3);
+	Vector3f b(4, 5, 6);
+	// Assign first two elements from b to first two slot of a
+	a.xy() = b.xy();
+	EXPECT_EQ(a, Vector3f(4, 5, 3));
+}
@@ -85,7 +85,7 @@ public:

 	float canRun(const vehicle_local_position_s &local_pos, bool is_wind_valid) const;
 	/*
-	 * Computes the lateral acceleration and airspeed references necessary to track
+	 * Computes the lateral acceleration and true airspeed references necessary to track
 	 * a path in wind (including excess wind conditions).
 	 *
 	 * @param[in] curr_pos_local Current horizontal vehicle position in local coordinates [m]
@@ -150,12 +150,12 @@ public:
 	void setMaxTrackKeepingMinGroundSpeed(float min_gsp) { min_gsp_track_keeping_max_ = math::max(min_gsp, 0.0f); }

 	/*
-	 * Set the nominal airspeed reference [m/s].
+	 * Set the nominal airspeed reference (true airspeed) [m/s].
 	 */
 	void setAirspeedNom(float airsp) { airspeed_nom_ = math::max(airsp, 0.1f); }

 	/*
-	 * Set the maximum airspeed reference [m/s].
+	 * Set the maximum airspeed reference (true airspeed) [m/s].
 	 */
 	void setAirspeedMax(float airsp) { airspeed_max_ = math::max(airsp, 0.1f); }

@@ -306,8 +306,8 @@ private:
 	// ^disabling this parameter disables all other excess wind handling options, using only the nominal airspeed for reference

 	// guidance settings
-	float airspeed_nom_{15.0f}; // nominal (desired) airspeed reference (generally equivalent to cruise optimized airspeed) [m/s]
-	float airspeed_max_{20.0f}; // maximum airspeed reference - the maximum achievable/allowed airspeed reference [m/s]
+	float airspeed_nom_{15.0f}; // nominal (desired) true airspeed reference (generally equivalent to cruise optimized airspeed) [m/s]
+	float airspeed_max_{20.0f}; // maximum true airspeed reference - the maximum achievable/allowed airspeed reference [m/s]
 	float roll_time_const_{0.0f}; // autopilot roll response time constant [s]
 	float min_gsp_desired_{0.0f}; // user defined miminum desired forward ground speed [m/s]
 	float min_gsp_track_keeping_max_{5.0f}; // maximum, minimum forward ground speed demand from track keeping logic [m/s]
@@ -341,8 +341,8 @@ private:
 	 * guidance outputs
 	 */

-	float airspeed_ref_{15.0f}; // airspeed reference [m/s]
-	matrix::Vector2f air_vel_ref_{matrix::Vector2f{15.0f, 0.0f}}; // air velocity reference vector [m/s]
+	float airspeed_ref_{15.0f}; // true airspeed reference [m/s]
+	matrix::Vector2f air_vel_ref_{matrix::Vector2f{15.0f, 0.0f}}; // true air velocity reference vector [m/s]
 	float lateral_accel_{0.0f}; // lateral acceleration reference [m/s^2]
 	float lateral_accel_ff_{0.0f}; // lateral acceleration demand to maintain path curvature [m/s^2]

@@ -358,7 +358,7 @@ private:
 	 * condition, path properties, and stability bounds.
 	 *
 	 * @param[in] ground_speed Vehicle ground speed [m/s]
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @param[in] wind_speed Wind speed [m/s]
 	 * @param[in] track_error Track error (magnitude) [m]
 	 * @param[in] path_curvature Path curvature at closest point on track [m^-1]
@@ -384,7 +384,7 @@ private:
 	/*
 	 * Cacluates an approximation of the wind factor (see [TODO: include citation]).
 	 *
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @param[in] wind_speed Wind speed [m/s]
 	 * @return Non-dimensional wind factor approximation
 	 */
@@ -395,7 +395,7 @@ private:
 	 * track keeping stability.
 	 *
 	 * @param[in] air_turn_rate The turn rate required to track the current path
-	 *            curvature at the current airspeed, in a no-wind condition [rad/s]
+	 *            curvature at the current true airspeed, in a no-wind condition [rad/s]
 	 * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation])
 	 * @return Period upper bound [s]
 	 */
@@ -408,7 +408,7 @@ private:
 	 * and a safety factor should be applied in addition to the returned value.
 	 *
 	 * @param[in] air_turn_rate The turn rate required to track the current path
-	 *            curvature at the current airspeed, in a no-wind condition [rad/s]
+	 *            curvature at the current true airspeed, in a no-wind condition [rad/s]
 	 * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation])
 	 * @return Period lower bound [s]
 	 */
@@ -493,8 +493,8 @@ private:

 	/*
 	 * Determines a reference air velocity *without curvature compensation, but
-	 * including "optimal" airspeed reference compensation in excess wind conditions.
-	 * Nominal and maximum airspeed member variables must be set before using this method.
+	 * including "optimal" true airspeed reference compensation in excess wind conditions.
+	 * Nominal and maximum true airspeed member variables must be set before using this method.
 	 *
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 * @param[in] bearing_vec Bearing vector
@@ -512,7 +512,7 @@ private:
 	 * Projection of the air velocity vector onto the bearing line considering
 	 * a connected wind triangle.
 	 *
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
 	 * @return Projection of air velocity vector on bearing vector [m/s]
 	 */
@@ -523,7 +523,7 @@ private:
 	 *
 	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
 	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @param[in] wind_speed Wind speed [m/s]
 	 * @return Binary bearing feasibility: 1 if feasible, 0 if infeasible
 	 */
@@ -549,7 +549,7 @@ private:
 	 * @param[in] wind_vel Wind velocity vector [m/s]
 	 * @param[in] bearing_vec Bearing vector
 	 * @param[in] wind_speed Wind speed [m/s]
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @return Air velocity vector [m/s]
 	 */
 	matrix::Vector2f infeasibleAirVelRef(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec,
@@ -562,7 +562,7 @@ private:
 	 *
 	 * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s]
 	 * @param[in] wind_dot_bearing 2D dot product of wind velocity and bearing vector [m/s]
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @param[in] wind_speed Wind speed [m/s]
 	 * @return bearing feasibility
 	 */
@@ -577,7 +577,7 @@ private:
 	 *            in direction of path
 	 * @param[in] ground_vel Vehicle ground velocity vector [m/s]
 	 * @param[in] wind_vel Wind velocity vector [m/s]
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @param[in] wind_speed Wind speed [m/s]
 	 * @param[in] signed_track_error Signed error to track at closest point (sign
 	 *             determined by path normal direction) [m]
@@ -594,7 +594,7 @@ private:
 	 *
 	 * @param[in] air_vel Vechile air velocity vector [m/s]
 	 * @param[in] air_vel_ref Reference air velocity vector [m/s]
-	 * @param[in] airspeed Vehicle airspeed [m/s]
+	 * @param[in] airspeed Vehicle true airspeed [m/s]
 	 * @return Lateral acceleration demand [m/s^2]
 	 */
 	float lateralAccel(const matrix::Vector2f &air_vel, const matrix::Vector2f &air_vel_ref,
@@ -42,7 +42,36 @@

 bool param_modify_on_import(bson_node_t node)
 {
+	// 2023-12-06: translate and invert FW_ARSP_MODE-> FW_USE_AIRSPD
+	{
+		if (strcmp("FW_ARSP_MODE", node->name) == 0) {
+			if (node->i32 == 0) {
+				node->i32 = 1;

+			} else {
+				node->i32 = 0;
+			}
+
+			strcpy(node->name, "FW_USE_AIRSPD");
+			PX4_INFO("copying and inverting %s -> %s", "FW_ARSP_MODE", "FW_USE_AIRSPD");
+			return true;
+		}
+	}
+
+	// 2023-12-06: translate CBRK_AIRSPD_CHK-> SYS_HAS_NUM_ASPD
+	{
+		if (strcmp("CBRK_AIRSPD_CHK", node->name) == 0) {
+			if (node->i32 == 162128) {
+				node->i32 = 0;
+
+				strcpy(node->name, "SYS_HAS_NUM_ASPD");
+				PX4_INFO("copying %s -> %s", "CBRK_AIRSPD_CHK", "SYS_HAS_NUM_ASPD");
+
+			}
+
+			return true;
+		}
+	}

 	return false;
 }
@@ -216,6 +216,19 @@ PARAM_DEFINE_INT32(SYS_HAS_MAG, 1);
 */
 PARAM_DEFINE_INT32(SYS_HAS_BARO, 1);

+/**
+ * Control if the vehicle has an airspeed sensor
+ *
+ * Set this to 0 if the board has no airspeed sensor.
+ * If set to 0, the preflight checks will not check for the presence of an
+ * airspeed sensor.
+ *
+ * @group System
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(SYS_HAS_NUM_ASPD, 0);
+
 /**
 * Number of distance sensors to check being available
 *
@@ -600,9 +600,8 @@ void AirspeedModule::select_airspeed_and_publish()
 	}

 	// check if airspeed based on ground-wind speed is valid and can be published
-	if (_param_airspeed_primary_index.get() > airspeed_index::DISABLED_INDEX &&
-	    (_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
-	     || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX)) {
+	if (_valid_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX
+	    || _param_airspeed_primary_index.get() == airspeed_index::GROUND_MINUS_WIND_INDEX) {

 		// _vehicle_local_position_valid determines if ground-wind estimate is valid
 		if (_vehicle_local_position_valid &&
@@ -132,7 +132,6 @@ PARAM_DEFINE_FLOAT(ASPD_SCALE_3, 1.0f);
 /**
 * Index or primary airspeed measurement source
 *
- * @value -1 Disabled
 * @value 0 Groundspeed minus windspeed
 * @value 1 First airspeed sensor
 * @value 2 Second airspeed sensor
@@ -1201,7 +1201,7 @@ Commander::handle_command(const vehicle_command_s &cmd)

 #if defined(CONFIG_BOARDCTL_RESET)

-			} else if ((param1 == 1) && !isArmed() && (px4_reboot_request(false, 400_ms) == 0)) {
+			} else if ((param1 == 1) && !isArmed() && (px4_reboot_request(REBOOT_REQUEST, 400_ms) == 0)) {
 				// 1: Reboot autopilot
 				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);

@@ -1221,7 +1221,7 @@ Commander::handle_command(const vehicle_command_s &cmd)

 #if defined(CONFIG_BOARDCTL_RESET)

-			} else if ((param1 == 3) && !isArmed() && (px4_reboot_request(true, 400_ms) == 0)) {
+			} else if ((param1 == 3) && !isArmed() && (px4_reboot_request(REBOOT_TO_BOOTLOADER, 400_ms) == 0)) {
 				// 3: Reboot autopilot and keep it in the bootloader until upgraded.
 				answer_command(cmd, vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED);

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2023 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
@@ -37,24 +37,20 @@
 using namespace time_literals;

 AirspeedChecks::AirspeedChecks()
-	: _param_fw_arsp_mode_handle(param_find("FW_ARSP_MODE")), _param_fw_airspd_max_handle(param_find("FW_AIRSPD_MAX"))
+	: _param_fw_airspd_max_handle(param_find("FW_AIRSPD_MAX"))
 {
 }

 void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)
 {
-	if (circuit_breaker_enabled_by_val(_param_cbrk_airspd_chk.get(), CBRK_AIRSPD_CHK_KEY) ||
+	if (_param_sys_has_num_aspd.get() <= 0 ||
 	    (context.status().vehicle_type != vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !context.status().is_vtol)) {
 		return;
 	}

-	int32_t airspeed_mode = 0;
-	param_get(_param_fw_arsp_mode_handle, &airspeed_mode);
-	const bool optional = (airspeed_mode == 1);
-
 	airspeed_validated_s airspeed_validated;

-	if (_airspeed_validated_sub.copy(&airspeed_validated) && hrt_elapsed_time(&airspeed_validated.timestamp) < 1_s) {
+	if (_airspeed_validated_sub.copy(&airspeed_validated) && hrt_elapsed_time(&airspeed_validated.timestamp) < 2_s) {

 		reporter.setIsPresent(health_component_t::differential_pressure);

@@ -100,13 +96,13 @@ void AirspeedChecks::checkAndReport(const Context &context, Report &reporter)
 			}
 		}

-	} else if (!optional) {
+	} else {

 		/* EVENT
 		 * @description
 		 * <profile name="dev">
 		 * Most likely the airspeed selector module is not running.
-		 * This check can be configured via <param>CBRK_AIRSPD_CHK</param> parameter.
+		 * This check can be configured via <param>SYS_HAS_NUM_ASPD</param> parameter.
 		 * </profile>
 		 */
 		reporter.healthFailure(NavModes::All, health_component_t::differential_pressure,
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 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
@@ -49,10 +49,9 @@ public:
 private:
 	uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)};

-	const param_t _param_fw_arsp_mode_handle;
 	const param_t _param_fw_airspd_max_handle;

 	DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
-					(ParamInt<px4::params::CBRK_AIRSPD_CHK>) _param_cbrk_airspd_chk
+					(ParamInt<px4::params::SYS_HAS_NUM_ASPD>) _param_sys_has_num_aspd
 				       )
 };
@@ -177,7 +177,7 @@ void WorkerThread::threadEntry()
 			param_reset_specific(reset_cal, sizeof(reset_cal) / sizeof(reset_cal[0]));
 			_ret_value = param_save_default(true);
 #if defined(CONFIG_BOARDCTL_RESET)
-			px4_reboot_request(false, 400_ms);
+			px4_reboot_request(REBOOT_REQUEST, 400_ms);
 #endif // CONFIG_BOARDCTL_RESET
 			break;
 		}
@@ -62,8 +62,6 @@ px4_add_library(ActuatorEffectiveness
 	ActuatorEffectivenessTailsitterVTOL.hpp
 	ActuatorEffectivenessRoverAckermann.hpp
 	ActuatorEffectivenessRoverAckermann.cpp
-	ActuatorEffectivenessRoverDifferential.hpp
-	ActuatorEffectivenessRoverDifferential.cpp
 )

 target_compile_options(ActuatorEffectiveness PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
@@ -131,9 +131,9 @@ public:
 	void setControlSetpoint(const matrix::Vector<float, NUM_AXES> &control) { _control_sp = control; }

 	/**
-	 * Set the desired control vector
+	 * Get the desired control vector
 	 *
-	 * @param Control vector
+	 * @return Control vector
 	 */
 	const matrix::Vector<float, NUM_AXES> &getControlSetpoint() const { return _control_sp; }

@@ -239,7 +239,7 @@ ControlAllocator::update_effectiveness_source()
 			break;

 		case EffectivenessSource::ROVER_DIFFERENTIAL:
-			tmp = new ActuatorEffectivenessRoverDifferential();
+			// differential_drive_control does allocation and publishes directly to actuator_motors topic
 			break;

 		case EffectivenessSource::FIXED_WING:
@@ -47,7 +47,6 @@
 #include <ActuatorEffectivenessTiltrotorVTOL.hpp>
 #include <ActuatorEffectivenessTailsitterVTOL.hpp>
 #include <ActuatorEffectivenessRoverAckermann.hpp>
-#include <ActuatorEffectivenessRoverDifferential.hpp>
 #include <ActuatorEffectivenessFixedWing.hpp>
 #include <ActuatorEffectivenessMCTilt.hpp>
 #include <ActuatorEffectivenessCustom.hpp>
@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (c) 2023 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(DifferentialDriveKinematics)
+
+px4_add_module(
+	MODULE modules__differential_drive_control
+	MAIN differential_drive_control
+	SRCS
+		DifferentialDriveControl.cpp
+		DifferentialDriveControl.hpp
+	DEPENDS
+		DifferentialDriveKinematics
+		px4_work_queue
+	MODULE_CONFIG
+		module.yaml
+)
@@ -0,0 +1,182 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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 "DifferentialDriveControl.hpp"
+
+using namespace time_literals;
+using namespace matrix;
+namespace differential_drive_control
+{
+
+DifferentialDriveControl::DifferentialDriveControl() :
+	ModuleParams(nullptr),
+	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
+{
+	updateParams();
+}
+
+bool DifferentialDriveControl::init()
+{
+	ScheduleOnInterval(10_ms); // 100 Hz
+	return true;
+}
+
+void DifferentialDriveControl::updateParams()
+{
+	ModuleParams::updateParams();
+	_max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get();
+	_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
+
+	_differential_drive_kinematics.setWheelBase(_param_rdd_wheel_base.get());
+	_differential_drive_kinematics.setMaxSpeed(_max_speed);
+	_differential_drive_kinematics.setMaxAngularVelocity(_max_angular_velocity);
+}
+
+void DifferentialDriveControl::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		exit_and_cleanup();
+	}
+
+	hrt_abstime now = hrt_absolute_time();
+
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		updateParams();
+	}
+
+	if (_vehicle_control_mode_sub.updated()) {
+		vehicle_control_mode_s vehicle_control_mode;
+
+		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
+			_armed = vehicle_control_mode.flag_armed;
+			_manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported
+		}
+	}
+
+	if (_manual_driving) {
+		// Manual mode
+		// directly produce setpoints from the manual control setpoint (joystick)
+		if (_manual_control_setpoint_sub.updated()) {
+			manual_control_setpoint_s manual_control_setpoint{};
+
+			if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
+				_differential_drive_setpoint.timestamp = now;
+				_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed;
+				_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() *
+									_max_angular_velocity;
+				_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
+			}
+		}
+	}
+
+	_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
+
+	// publish data to actuator_motors (output module)
+	// get the wheel speeds from the inverse kinematics class (DifferentialDriveKinematics)
+	Vector2f wheel_speeds = _differential_drive_kinematics.computeInverseKinematics(
+					_differential_drive_setpoint.speed,
+					_differential_drive_setpoint.yaw_rate);
+
+	// Check if max_angular_wheel_speed is zero
+	const bool setpoint_timeout = (_differential_drive_setpoint.timestamp + 100_ms) < now;
+	const bool valid_max_speed = _param_rdd_speed_scale.get() > FLT_EPSILON;
+
+	if (!_armed || setpoint_timeout || !valid_max_speed) {
+		wheel_speeds = {}; // stop
+	}
+
+	wheel_speeds = matrix::constrain(wheel_speeds, -1.f, 1.f);
+
+	actuator_motors_s actuator_motors{};
+	actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
+	wheel_speeds.copyTo(actuator_motors.control);
+	actuator_motors.timestamp = now;
+	_actuator_motors_pub.publish(actuator_motors);
+}
+
+int DifferentialDriveControl::task_spawn(int argc, char *argv[])
+{
+	DifferentialDriveControl *instance = new DifferentialDriveControl();
+
+	if (instance) {
+		_object.store(instance);
+		_task_id = task_id_is_work_queue;
+
+		if (instance->init()) {
+			return PX4_OK;
+		}
+
+	} else {
+		PX4_ERR("alloc failed");
+	}
+
+	delete instance;
+	_object.store(nullptr);
+	_task_id = -1;
+
+	return PX4_ERROR;
+}
+
+int DifferentialDriveControl::custom_command(int argc, char *argv[])
+{
+	return print_usage("unknown command");
+}
+
+int DifferentialDriveControl::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_ERR("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Description
+Rover Differential Drive controller.
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("differential_drive_control", "controller");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+	return 0;
+}
+
+extern "C" __EXPORT int differential_drive_control_main(int argc, char *argv[])
+{
+	return DifferentialDriveControl::main(argc, argv);
+}
+
+} // namespace differential_drive_control
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 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/px4_config.h>
+#include <px4_platform_common/defines.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+
+// uORB includes
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_motors.h>
+#include <uORB/PublicationMulti.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/differential_drive_setpoint.h>
+
+// Standard library includes
+#include <math.h>
+
+// Local includes
+#include <DifferentialDriveKinematics.hpp>
+
+namespace differential_drive_control
+{
+
+class DifferentialDriveControl : public ModuleBase<DifferentialDriveControl>, public ModuleParams,
+	public px4::ScheduledWorkItem
+{
+public:
+	DifferentialDriveControl();
+	~DifferentialDriveControl() override = default;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	bool init();
+
+protected:
+	void updateParams() override;
+
+private:
+	void Run() override;
+
+	uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
+	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
+	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+
+	uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
+	uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
+
+	differential_drive_setpoint_s _differential_drive_setpoint{};
+	DifferentialDriveKinematics _differential_drive_kinematics{};
+
+	bool _armed = false;
+	bool _manual_driving = false;
+	float _max_speed{0.f};
+	float _max_angular_velocity{0.f};
+
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
+		(ParamFloat<px4::params::RDD_ANG_SCALE>) _param_rdd_ang_velocity_scale,
+		(ParamFloat<px4::params::RDD_WHL_SPEED>) _param_rdd_max_wheel_speed,
+		(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
+		(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius,
+		(ParamInt<px4::params::CA_R_REV>) _param_r_rev
+	)
+};
+
+} // namespace differential_drive_control
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2023 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(DifferentialDriveKinematics
+	DifferentialDriveKinematics.cpp
+)
+
+target_include_directories(DifferentialDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+px4_add_unit_gtest(SRC DifferentialDriveKinematicsTest.cpp LINKLIBS DifferentialDriveKinematics)
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 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 "DifferentialDriveKinematics.hpp"
+
+#include <mathlib/mathlib.h>
+
+using namespace matrix;
+
+matrix::Vector2f DifferentialDriveKinematics::computeInverseKinematics(float linear_velocity_x, float yaw_rate)
+{
+	if (_max_speed < FLT_EPSILON) {
+		return Vector2f();
+	}
+
+	linear_velocity_x = math::constrain(linear_velocity_x, -_max_speed, _max_speed);
+	yaw_rate = math::constrain(yaw_rate, -_max_angular_velocity, _max_angular_velocity);
+
+	const float rotational_velocity = (_wheel_base / 2.f) * yaw_rate;
+	float combined_velocity = fabsf(linear_velocity_x) + fabsf(rotational_velocity);
+
+	// Compute an initial gain
+	float gain = 1.0f;
+
+	if (combined_velocity > _max_speed) {
+		float excess_velocity = fabsf(combined_velocity - _max_speed);
+		float adjusted_linear_velocity = fabsf(linear_velocity_x) - excess_velocity;
+		gain = adjusted_linear_velocity / fabsf(linear_velocity_x);
+	}
+
+	// Apply the gain
+	linear_velocity_x *= gain;
+
+	// Calculate the left and right wheel speeds
+	return Vector2f(linear_velocity_x - rotational_velocity,
+			linear_velocity_x + rotational_velocity) / _max_speed;
+}
+
@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <matrix/matrix/math.hpp>
+
+/**
+ * @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot.
+ *
+ * This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
+ * given linear velocity and yaw rate.
+ */
+class DifferentialDriveKinematics
+{
+public:
+	DifferentialDriveKinematics() = default;
+	~DifferentialDriveKinematics() = default;
+
+	/**
+	 * @brief Sets the wheel base of the robot.
+	 *
+	 * @param wheel_base The distance between the centers of the wheels.
+	 */
+	void setWheelBase(const float wheel_base) { _wheel_base = wheel_base; };
+
+	/**
+	 * @brief Sets the maximum speed of the robot.
+	 *
+	 * @param max_speed The maximum speed of the robot.
+	 */
+	void setMaxSpeed(const float max_speed) { _max_speed = max_speed; };
+
+	/**
+	 * @brief Sets the maximum angular speed of the robot.
+	 *
+	 * @param max_angular_speed The maximum angular speed of the robot.
+	 */
+	void setMaxAngularVelocity(const float max_angular_velocity) { _max_angular_velocity = max_angular_velocity; };
+
+	/**
+	 * @brief Computes the inverse kinematics for differential drive.
+	 *
+	 * @param linear_velocity_x Linear velocity along the x-axis.
+	 * @param yaw_rate Yaw rate of the robot.
+	 * @return matrix::Vector2f Motor velocities for the right and left motors.
+	 */
+	matrix::Vector2f computeInverseKinematics(float linear_velocity_x, float yaw_rate);
+
+private:
+	float _wheel_base{0.f};
+	float _max_speed{0.f};
+	float _max_angular_velocity{0.f};
+};
@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2023 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#include <gtest/gtest.h>
+#include "DifferentialDriveKinematics.hpp"
+#include <mathlib/math/Functions.hpp>
+
+using namespace matrix;
+
+TEST(DifferentialDriveKinematicsTest, AllZeroInputCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(10.f);
+	kinematics.setMaxAngularVelocity(10.f);
+
+	// Test with zero linear velocity and zero yaw rate (stationary vehicle)
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 0.f), Vector2f());
+}
+
+
+TEST(DifferentialDriveKinematicsTest, InvalidParameterCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(0.f);
+	kinematics.setMaxSpeed(10.f);
+	kinematics.setMaxAngularVelocity(10.f);
+
+	// Test with invalid parameters (zero wheel base and wheel radius)
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, .1f), Vector2f());
+}
+
+
+TEST(DifferentialDriveKinematicsTest, UnitCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(10.f);
+	kinematics.setMaxAngularVelocity(10.f);
+
+	// Test with unit values for linear velocity and yaw rate
+	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0.05f, 0.15f));
+}
+
+
+TEST(DifferentialDriveKinematicsTest, UnitSaturationCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with unit values for linear velocity and yaw rate, but with max speed that requires saturation
+	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 1.f), Vector2f(0, 1));
+}
+
+
+TEST(DifferentialDriveKinematicsTest, OppositeUnitSaturationCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Negative linear velocity for backward motion and positive yaw rate for turning right
+	EXPECT_EQ(kinematics.computeInverseKinematics(-1.f, 1.f), Vector2f(-1, 0));
+}
+
+TEST(DifferentialDriveKinematicsTest, RandomCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(2.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Negative linear velocity for backward motion and positive yaw rate for turning right
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.5f, 0.7f), Vector2f(-0.4f, 1.0f));
+}
+
+TEST(DifferentialDriveKinematicsTest, RotateInPlaceCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test rotating in place (zero linear velocity, non-zero yaw rate)
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 1.f), Vector2f(-0.5f, 0.5f));
+}
+
+TEST(DifferentialDriveKinematicsTest, StraightMovementCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test moving straight (non-zero linear velocity, zero yaw rate)
+	EXPECT_EQ(kinematics.computeInverseKinematics(1.f, 0.f), Vector2f(1.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MinInputValuesCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(FLT_MIN);
+	kinematics.setMaxSpeed(FLT_MIN);
+	kinematics.setMaxAngularVelocity(FLT_MIN);
+
+	// Test with minimum possible input values
+	EXPECT_EQ(kinematics.computeInverseKinematics(FLT_MIN, FLT_MIN), Vector2f(0.f, 0.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxSpeedLimitCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 10.f), Vector2f(0.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxSpeedForwardsCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(1.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(10.f, 0.f), Vector2f(1.f, 1.f));
+}
+
+TEST(DifferentialDriveKinematicsTest, MaxAngularCase)
+{
+	DifferentialDriveKinematics kinematics;
+	kinematics.setWheelBase(2.f);
+	kinematics.setMaxSpeed(1.f);
+	kinematics.setMaxAngularVelocity(1.f);
+
+	// Test with high linear velocity and yaw rate, expecting speeds to be scaled down to fit the max speed
+	EXPECT_EQ(kinematics.computeInverseKinematics(0.f, 10.f), Vector2f(-1.f, 1.f));
+}
@@ -0,0 +1,6 @@
+menuconfig MODULES_DIFFERENTIAL_DRIVE_CONTROL
+	bool "differential_drive_control"
+	default n
+	depends on MODULES_CONTROL_ALLOCATOR
+	---help---
+		Enable support for control of differential drive rovers
@@ -0,0 +1,55 @@
+module_name: Differential Drive Control
+
+parameters:
+  - group: Rover Differential Drive
+    definitions:
+      RDD_WHEEL_BASE:
+        description:
+          short: Wheel base
+          long: Distance from the center of the right wheel to the center of the left wheel
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.5
+      RDD_WHEEL_RADIUS:
+        description:
+          short: Wheel radius
+          long: Size of the wheel, half the diameter of the wheel
+        type: float
+        unit: m
+        min: 0.001
+        max: 100
+        increment: 0.001
+        decimal: 3
+        default: 0.1
+      RDD_SPEED_SCALE:
+        description:
+          short: Manual speed scale
+        type: float
+        min: 0
+        max: 1.0
+        increment: 0.01
+        decimal: 2
+        default: 1.0
+      RDD_ANG_SCALE:
+        description:
+          short: Manual angular velocity scale
+        type: float
+        min: 0
+        max: 1.0
+        increment: 0.01
+        decimal: 2
+        default: 1.0
+      RDD_WHL_SPEED:
+        description:
+          short: Maximum wheel speed
+        type: float
+        unit: rad/s
+        min: 0
+        max: 100
+        increment: 0.01
+        decimal: 2
+        default: 10
@@ -1,5 +1,5 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.1e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
+10000,1,-0.0094,-0.01,-3.2e-06,0.00023,7.3e-05,-0.011,7.2e-06,2.2e-06,-0.00045,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
 90000,1,-0.0094,-0.011,6.9e-05,-0.00047,0.0026,-0.026,-1.6e-05,0.00011,-0.0023,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00046,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
 190000,1,-0.0094,-0.011,2.8e-05,6.9e-05,0.004,-0.041,-5.9e-06,0.00043,-0.0044,-3e-11,-2.7e-12,5.9e-13,0,0,-2e-09,0,0,0,0,0,0,0,0,0.011,0.011,0.00093,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
 290000,1,-0.0094,-0.011,6.3e-05,0.001,0.0064,-0.053,3.8e-05,0.00036,-0.007,9.1e-10,1e-10,-2.8e-11,0,0,-4.8e-07,0,0,0,0,0,0,0,0,0.012,0.011,0.00077,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0
@@ -219,7 +219,7 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
 21690000,0.77,-0.012,-0.0018,-0.63,0.0057,0.021,0.017,0.0021,0.009,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.0002,0.0088,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00039,0.02,0.025,0.025,0.0084,0.058,0.058,0.038,1.4e-06,1.4e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
 21790000,0.77,-0.012,-0.0017,-0.63,0.0044,0.021,0.015,0.00046,0.01,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00029,0.0086,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00038,0.02,0.022,0.022,0.0082,0.049,0.049,0.038,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
 21890000,0.77,-0.012,-0.0017,-0.63,0.0044,0.021,0.016,0.00091,0.013,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.0003,0.0086,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00039,0.00039,0.02,0.024,0.024,0.0082,0.055,0.055,0.038,1.3e-06,1.3e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
-21990000,0.77,-0.012,-0.0017,-0.63,0.003,0.023,0.016,-0.00047,0.013,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00035,0.0085,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.021,0.021,0.0081,0.047,0.047,0.038,1.2e-06,1.2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
+21990000,0.77,-0.012,-0.0017,-0.63,0.0031,0.023,0.016,-0.00047,0.013,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00035,0.0085,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.021,0.021,0.0081,0.047,0.047,0.038,1.2e-06,1.2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
 22090000,0.77,-0.012,-0.0017,-0.63,0.0035,0.022,0.015,-0.00015,0.016,-3.7e+02,-0.0015,-0.0061,4.4e-05,-0.00036,0.0085,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.023,0.023,0.0081,0.053,0.053,0.038,1.2e-06,1.2e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
 22190000,0.77,-0.012,-0.0017,-0.63,0.0038,0.019,0.015,-0.00015,0.013,-3.7e+02,-0.0015,-0.0061,4.4e-05,-5.3e-05,0.0084,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.021,0.021,0.008,0.046,0.046,0.037,1.1e-06,1.1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
 22290000,0.77,-0.012,-0.0017,-0.63,0.005,0.021,0.015,0.00028,0.015,-3.7e+02,-0.0015,-0.0061,4.4e-05,-6.1e-05,0.0084,-0.13,-0.017,-0.0037,0.57,0,0,0,0,0,0.00038,0.00038,0.02,0.022,0.022,0.008,0.051,0.051,0.037,1.1e-06,1.1e-06,6.3e-06,0.029,0.029,0.0005,0,0,0,0,0,0,0,0
@@ -1,5 +1,5 @@
 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
-10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.8e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
+10000,1,-0.011,-0.01,0.00023,0.00033,-0.00013,-0.01,1e-05,-3.9e-06,-0.00042,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00018,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
 90000,1,-0.011,-0.01,0.00033,-0.001,-0.0031,-0.024,-3.8e-05,-0.00013,-0.0021,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.01,0.01,0.00046,25,25,10,1e+02,1e+02,1e+02,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
 190000,1,-0.012,-0.011,0.00044,-0.0023,-0.003,-0.037,-0.00017,-0.00043,-0.017,4.7e-10,-5e-10,-1.5e-11,0,0,-6.8e-08,0,0,0,0,0,0,0,0,0.011,0.011,0.00094,25,25,10,1e+02,1e+02,1,0.01,0.01,0.01,0.04,0.04,0.04,0,0,0,0,0,0,0,0
 290000,1,-0.012,-0.011,0.00044,-0.0033,-0.0044,-0.046,-0.00024,-0.00025,-0.018,3.8e-09,-5.9e-09,-4.7e-10,0,0,-2.9e-06,0,0,0,0,0,0,0,0,0.011,0.012,0.00077,25,25,9.6,0.37,0.37,0.41,0.01,0.01,0.0052,0.04,0.04,0.04,0,0,0,0,0,0,0,0
@@ -466,7 +466,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 	}

 	// activation/deactivation of weather vane is based on parameter WV_EN and setting of navigator (allow_weather_vane)
-	_weathervane.setNavigatorForceDisabled(_sub_triplet_setpoint.get().current.disable_weather_vane);
+	_weathervane.setNavigatorForceDisabled(PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw));

 	// Calculate the current vehicle state and check if it has updated.
 	State previous_state = _current_state;
@@ -481,7 +481,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 		_obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint,
 				_triplet_next_wp,
 				_sub_triplet_setpoint.get().next.yaw,
-				_sub_triplet_setpoint.get().next.yawspeed_valid ? _sub_triplet_setpoint.get().next.yawspeed : (float)NAN,
+				(float)NAN,
 				_weathervane.isActive(), _sub_triplet_setpoint.get().current.type);
 		_obstacle_avoidance.checkAvoidanceProgress(
 			_position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt));
@@ -509,13 +509,7 @@ bool FlightTaskAuto::_evaluateTriplets()
 			_yaw_setpoint = NAN;
 			_yawspeed_setpoint = 0.f;

-		} else if ((_type != WaypointType::takeoff || _sub_triplet_setpoint.get().current.disable_weather_vane)
-			   && _sub_triplet_setpoint.get().current.yaw_valid) {
-			// Use the yaw computed in Navigator except during takeoff because
-			// Navigator is not handling the yaw reset properly.
-			// But: use if from Navigator during takeoff if disable_weather_vane is true,
-			// because we're then aligning to the transition waypoint.
-			// TODO: fix in navigator
+		} else if (PX4_ISFINITE(_sub_triplet_setpoint.get().current.yaw)) {
 			_yaw_setpoint = _sub_triplet_setpoint.get().current.yaw;
 			_yawspeed_setpoint = NAN;

@@ -150,7 +150,7 @@ float FixedwingAttitudeControl::get_airspeed_constrained()
 	// if no airspeed measurement is available out best guess is to use the trim airspeed
 	float airspeed = _param_fw_airspd_trim.get();

-	if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) {
+	if (_param_fw_use_airspd.get() && airspeed_valid) {
 		/* prevent numerical drama by requiring 0.5 m/s minimal speed */
 		airspeed = math::max(0.5f, _airspeed_validated_sub.get().calibrated_airspeed_m_s);

@@ -135,7 +135,7 @@ private:
 		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
 		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
 		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
-		(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
+		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,

 		(ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
 		(ParamFloat<px4::params::FW_MAN_R_MAX>) _param_fw_man_r_max,
@@ -75,6 +75,7 @@ bool FwAutotuneAttitudeControl::init()

 void FwAutotuneAttitudeControl::reset()
 {
+	_param_fw_at_start.reset();
 }

 void FwAutotuneAttitudeControl::Run()
@@ -59,9 +59,6 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	, _figure_eight(_npfg, _wind_vel, _eas2tas)
 #endif // CONFIG_FIGURE_OF_EIGHT
 {
-	if (vtol) {
-		_param_handle_airspeed_trans = param_find("VT_ARSP_TRANS");
-	}

 	// limit to 50 Hz
 	_local_pos_sub.set_interval_ms(20);
@@ -80,7 +77,6 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
 	/* fetch initial parameter values */
 	parameters_update();

-	_airspeed_slew_rate_controller.setForcedValue(_performance_model.getCalibratedTrimAirspeed());
 	_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));
 	_roll_slew_rate.setForcedValue(0.f);

@@ -111,11 +107,6 @@ FixedwingPositionControl::parameters_update()

 	_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));

-	// VTOL parameter VT_ARSP_TRANS
-	if (_param_handle_airspeed_trans != PARAM_INVALID) {
-		param_get(_param_handle_airspeed_trans, &_param_airspeed_trans);
-	}
-
 	// NPFG parameters
 	_npfg.setPeriod(_param_npfg_period.get());
 	_npfg.setDamping(_param_npfg_damping.get());
@@ -214,20 +205,23 @@ FixedwingPositionControl::airspeed_poll()
 	bool airspeed_valid = _airspeed_valid;
 	airspeed_validated_s airspeed_validated;

-	if ((_param_fw_arsp_mode.get() == 0) && _airspeed_validated_sub.update(&airspeed_validated)) {
+	if (_param_fw_use_airspd.get() && _airspeed_validated_sub.update(&airspeed_validated)) {

 		_eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed

 		if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s)
 		    && PX4_ISFINITE(airspeed_validated.true_airspeed_m_s)
-		    && (airspeed_validated.calibrated_airspeed_m_s > 0.0f)) {
+		    && (airspeed_validated.calibrated_airspeed_m_s > FLT_EPSILON)) {

 			airspeed_valid = true;

 			_time_airspeed_last_valid = airspeed_validated.timestamp;
-			_airspeed = airspeed_validated.calibrated_airspeed_m_s;
+			_airspeed_eas = airspeed_validated.calibrated_airspeed_m_s;

 			_eas2tas = constrain(airspeed_validated.true_airspeed_m_s / airspeed_validated.calibrated_airspeed_m_s, 0.9f, 2.0f);
+
+		} else {
+			airspeed_valid = false;
 		}

 	} else {
@@ -391,8 +385,15 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 	calibrated_airspeed_setpoint = constrain(calibrated_airspeed_setpoint, calibrated_min_airspeed,
 				       _performance_model.getMaximumCalibratedAirspeed());

-	if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())) {
-		_airspeed_slew_rate_controller.setForcedValue(calibrated_airspeed_setpoint);
+	// initialize the airspeed setpoint to the max(current airsped, min airspeed)
+	if (!PX4_ISFINITE(_airspeed_slew_rate_controller.getState())
+	    || !_tecs_is_running) {
+		_airspeed_slew_rate_controller.setForcedValue(math::max(calibrated_min_airspeed, _airspeed_eas));
+	}
+
+	// reset the airspeed setpoint to the min airspeed if the min airspeed changes while in operation
+	if (_airspeed_slew_rate_controller.getState() < calibrated_min_airspeed) {
+		_airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed);
 	}

 	if (control_interval > FLT_EPSILON) {
@@ -400,10 +401,6 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 		_airspeed_slew_rate_controller.update(calibrated_airspeed_setpoint, control_interval);
 	}

-	if (_airspeed_slew_rate_controller.getState() < calibrated_min_airspeed) {
-		_airspeed_slew_rate_controller.setForcedValue(calibrated_min_airspeed);
-	}
-
 	if (_airspeed_slew_rate_controller.getState() > _performance_model.getMaximumCalibratedAirspeed()) {
 		_airspeed_slew_rate_controller.setForcedValue(_performance_model.getMaximumCalibratedAirspeed());
 	}
@@ -648,7 +645,7 @@ void
 FixedwingPositionControl::updateManualTakeoffStatus()
 {
 	if (!_completed_manual_takeoff) {
-		const bool at_controllable_airspeed = _airspeed > _performance_model.getMinimumCalibratedAirspeed(getLoadFactor())
+		const bool at_controllable_airspeed = _airspeed_eas > _performance_model.getMinimumCalibratedAirspeed(getLoadFactor())
 						      || !_airspeed_valid;
 		const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
 					 && _control_mode.flag_armed;
@@ -1372,7 +1369,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
 			_runway_takeoff.forceSetFlyState();
 		}

-		_runway_takeoff.update(now, takeoff_airspeed, _airspeed, _current_altitude - _takeoff_ground_alt,
+		_runway_takeoff.update(now, takeoff_airspeed, _airspeed_eas, _current_altitude - _takeoff_ground_alt,
 				       clearance_altitude_amsl - _takeoff_ground_alt);

 		// yaw control is disabled once in "taking off" state
@@ -2516,48 +2513,14 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		const float desired_max_sinkrate, const float desired_max_climbrate,
 		bool disable_underspeed_detection, float hgt_rate_sp)
 {
-	_tecs_is_running = true;
-
 	// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
-	// (it should also not run during VTOL blending because airspeed is too low still)
-	if (_vehicle_status.is_vtol) {
-		if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _vehicle_status.in_transition_mode) {
-			_tecs_is_running = false;
-		}
-
-		if (_vehicle_status.in_transition_mode) {
-			// we're in transition
-			_was_in_transition = true;
-
-			// set this to transition airspeed to init tecs correctly
-			if (_param_fw_arsp_mode.get() == 1 && PX4_ISFINITE(_param_airspeed_trans)) {
-				// some vtols fly without airspeed sensor
-				_airspeed_after_transition = _param_airspeed_trans;
-
-			} else {
-				_airspeed_after_transition = _airspeed;
-			}
-
-			_airspeed_after_transition = constrain(_airspeed_after_transition,
-							       _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()),
-							       _performance_model.getMaximumCalibratedAirspeed());
-
-		} else if (_was_in_transition) {
-			// after transition we ramp up desired airspeed from the speed we had coming out of the transition
-			_airspeed_after_transition += control_interval * 2.0f; // increase 2m/s
-
-			if (_airspeed_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
-				airspeed_sp = max(_airspeed_after_transition, _airspeed);
-
-			} else {
-				_was_in_transition = false;
-				_airspeed_after_transition = 0.0f;
-			}
-		}
-	}
-
-	if (!_tecs_is_running) {
+	if (_vehicle_status.is_vtol && (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
+					|| _vehicle_status.in_transition_mode)) {
+		_tecs_is_running = false;
 		return;
+
+	} else {
+		_tecs_is_running = true;
 	}

 	/* update TECS vehicle state estimates */
@@ -2575,7 +2538,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		     _current_altitude,
 		     alt_sp,
 		     airspeed_sp,
-		     _airspeed,
+		     _airspeed_eas,
 		     _eas2tas,
 		     throttle_min,
 		     throttle_max,
@@ -360,8 +360,8 @@ private:

 	// AIRSPEED

-	float _airspeed{0.0f};
-	float _eas2tas{1.0f};
+	float _airspeed_eas{0.f};
+	float _eas2tas{1.f};
 	bool _airspeed_valid{false};
 	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};

@@ -384,13 +384,8 @@ private:

 	bool _tecs_is_running{false};
 	// VTOL / TRANSITION
-
-	float _airspeed_after_transition{0.0f};
-	bool _was_in_transition{false};
 	bool _is_vtol_tailsitter{false};
 	matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN};
-	param_t _param_handle_airspeed_trans{PARAM_INVALID};
-	float _param_airspeed_trans{NAN}; // [m/s]

 	// ESTIMATOR RESET COUNTERS

@@ -959,7 +954,7 @@ private:
 		(ParamFloat<px4::params::FW_GPSF_R>) _param_nav_gpsf_r,

 		// external parameters
-		(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
+		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,

 		(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,

@@ -162,7 +162,7 @@ float FixedwingRateControl::get_airspeed_and_update_scaling()
 	// if no airspeed measurement is available out best guess is to use the trim airspeed
 	float airspeed = _param_fw_airspd_trim.get();

-	if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) {
+	if (_param_fw_use_airspd.get() && airspeed_valid) {
 		/* prevent numerical drama by requiring 0.5 m/s minimal speed */
 		airspeed = math::max(0.5f, _airspeed_validated_sub.get().calibrated_airspeed_m_s);

@@ -164,7 +164,7 @@ private:
 		(ParamFloat<px4::params::FW_AIRSPD_MIN>) _param_fw_airspd_min,
 		(ParamFloat<px4::params::FW_AIRSPD_STALL>) _param_fw_airspd_stall,
 		(ParamFloat<px4::params::FW_AIRSPD_TRIM>) _param_fw_airspd_trim,
-		(ParamInt<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
+		(ParamBool<px4::params::FW_USE_AIRSPD>) _param_fw_use_airspd,

 		(ParamInt<px4::params::FW_ARSP_SCALE_EN>) _param_fw_arsp_scale_en,

@@ -41,16 +41,18 @@
 */

 /**
- * Airspeed mode
+ * Use airspeed for control
 *
- * On vehicles without airspeed sensor this parameter can be used to
- * enable flying without an airspeed reading
+ * If set to 1, the airspeed measurement data, if valid, is used in the following controllers:
+ * - Rate controller: output scaling
+ * - Attitude controller: coordinated turn controller
+ * - Position controller: airspeed setpoint tracking, takeoff logic
+ * - VTOL: transition logic
 *
- * @value 0 Use airspeed in controller
- * @value 1 Do not use airspeed in controller
+ * @boolean
 * @group FW Rate Control
 */
-PARAM_DEFINE_INT32(FW_ARSP_MODE, 0);
+PARAM_DEFINE_INT32(FW_USE_AIRSPD, 1);

 /**
 * Pitch rate proportional gain.
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Jaeyoung Lim	bf8840d109	Add topic remap plugin	2024-01-09 10:27:42 +01:00
Federico Ciresola	20129e63fa	ModeCompleted.msg: document nav_state (#22544 )	2024-01-08 11:32:41 +01:00
somebody-once-told-me	3d5c2ef6c4	[control_allocation] small function comment typo fix (#22607 ) Co-authored-by: Master Chief <master-chief@the-void.com>	2024-01-08 11:12:06 +01:00
Peter van der Perk	cf840ff373	Update NuttX	2024-01-06 04:41:00 -05:00
Peter van der Perk	0df7ee423f	fmu-v6xrt: Tune ITCM function mapping	2024-01-06 04:41:00 -05:00
muramura	b24d9bf009	fmu-v6xrt: Move external BMP388 outside of config parameter determination	2024-01-05 09:10:54 -05:00
Peter van der Perk	e79f6b2ac1	v6x-rt: Add reboot to isp support	2024-01-05 08:41:41 -05:00
David Sidrane	5d7cb99204	px4_fmu-v6xrt:Support PX4 VxX Mini baseboard	2024-01-05 01:38:22 -05:00
muramura	959cb41e3a	mavlink: Add a second barometric sensor message	2024-01-04 11:19:41 -05:00
Daniel Agar	6495f40ee4	delete old top level config files (dot files, CI, etc)	2024-01-04 11:18:56 -05:00
muramura	7b32b735e8	fmu-v6xrt: Delete a parameter setting that does not exist	2024-01-04 11:18:18 -05:00
Peter van der Perk	84093a07a2	reboot: Add reboot to ISP option	2024-01-04 11:17:16 -05:00
Peter van der Perk	8ba18a78af	v6x-rt: move romapi to platform	2024-01-04 05:12:34 -05:00
Silvan Fuhrer	6be8cbe439	Navigator: mission_block: reduce enforce eexit course margin to 105% (#22511 ) With this margin it is made sure that if the loiter is not perfectly tracked, (vehicle outside of path setpoint) a wp just at the border of the loiter is still reachable. It should though be as small as necessary, as otherwise, with good loiter tracking, waypoints that are close but not right on the loiter radius are not enforcing the exit course neither. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-21 16:58:22 +01:00
Silvan Fuhrer	7e22b47b85	Navigator/FlightTaskAuto yaw handling improvements/simplifications (#22532 ) * PositionSetpoint: remove yaw_valid field * Navigator: set yaw setpoint to NAN for Takeoff Don't set a yaw setpoint for takeoff, as Navigator doesn't handle the yaw reset. The yaw setpoint generation is handled by FlightTaskAuto. * PositionSetpoint.msg: remove disable_weather_vane and instead only use the yaw field Strictly follow the concept that if the position_setpoint.yaw is set, then follow it the controller, and otherwise let the controller set it as it thinks it's best. * Navigator: remove logic that sets yaw to be accepted in TAKEOFF No longer needed as during Takeoff we anyway don't set a yaw setpoint. * PositionSetpoint.msg: remove yawspeed_valid * PositionSetpoint.msg: remove yawspeed * Navigator: set yaw setpoint to NAN instead of current In set_takeoff and set_land_item, as well as for VTOL transition. The flight tasks then set the yaw corresponding to the current yaw. * Navigator: change get_yaw_acceptance into a bool * PositionSetpoint.msg: improve comment for yaw * MissionBlock: remove unnecessary code from set_vtol_transition_item * Navigator: clean up calculate_breaking_stop(), set yaw to NAN * Navigator: set yaw to NAN in variouls places where not specifc setpoint is desired * Navigator: set yaw to NAN in reset_position_setpoint() --------- Signed-off-by: Silvan Fuhrer <silvan@auterion.com> Co-authored-by: Matthias Grob <maetugr@gmail.com>	2023-12-21 16:50:13 +01:00
Matthias Grob	d872ef87da	differential_drive_control: don't build by default also add dependency on control allocation parameter CA_R_REV	2023-12-21 16:27:53 +01:00
Matthias Grob	3de5c609a4	Differential Rover: PR fixes	2023-12-21 16:27:53 +01:00
PerFrivik	056e41af8c	Differential Rover: Ported R1 to GZ and introduced new GZ wheel interface	2023-12-21 16:27:53 +01:00
PerFrivik	1e7ce32480	Differential Rover: Added logging and dds topics	2023-12-21 16:27:53 +01:00
PerFrivik	3df71d1837	Differential Rover: Differential drive module & library	2023-12-21 16:27:53 +01:00
PerFrivik	e3359ea884	Differential Rover: Update airframe architecture	2023-12-21 16:27:53 +01:00
Peter van der Perk	19d1941758	px4_fmuv6xrt: Switch to icm42686p on SPI1 icm42588p driver don't use a icm42688p when icm42686p is requested	2023-12-21 10:11:20 -05:00
bresch	c1b139dea1	atune: reset param on start This prevents a race condition where autotune cannot start because the param was already set to 1	2023-12-21 13:52:47 +01:00
MaEtUgR	74549e29a5	[AUTO COMMIT] update change indication	2023-12-21 11:42:08 +01:00
Matthias Grob	bcb2b1ad40	matrix: fix slice to slice assignment to do deep copy To fix usage of a.xy() = b.xy() which should copy the first two elements over into a and not act on a copy of a.	2023-12-21 11:42:08 +01:00
Silvan Fuhrer	2afbd09c63	Commander: AirspeedCheck: increase timeout threshold to 2s 1s gives some false positives at boot up, as the airspeed selector only starts publishing 2s after its startup. Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-21 11:34:28 +01:00
Silvan Fuhrer	9d00a3ae4d	AirspeedSelector: remove option to disable airspeed sensor through ASPD_PRIMARY Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-21 11:34:28 +01:00
Silvan Fuhrer	29807a5e50	Replace CBRK_AIRSPD_CHK with SYS_HAS_NUM_ASPD Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-21 11:34:28 +01:00
Silvan Fuhrer	9e0c8fd75e	FW controllers: change param FW_ARSP_MODE to FW_USE_AIRSPD Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-21 11:34:28 +01:00
Silvan Fuhrer	1f2a0bc657	Commander: remove check for FW_ARSP_MODE for airspeed missing reporting Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-21 11:34:28 +01:00
Silvan Fuhrer	5211c358aa	FW Position Controller: change airspeed setpoint init -remove dedicated vtol transition airspeed init logic -init airspeed setpoint on first usage of tecs -init to max of current airspeed and min airspeed Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-21 11:34:28 +01:00
Silvan Fuhrer	27957e1f2f	FW Position Controller: set airspeed_valid flag to false if incoming data is not finite Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-21 11:34:28 +01:00
Silvan Fuhrer	123c06f2e6	NPFG: specify in comments that airspeed reference is for true airspeed Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-21 11:34:28 +01:00
Silvan Fuhrer	589f0f1fc7	FW Position Controller: rename _airspeed to _airspeed_eas Signed-off-by: Silvan Fuhrer <silvan@auterion.com>	2023-12-21 11:34:28 +01:00
bresch	b8c81f6281	gps_blending: fallback to secondary if primary has no fix	2023-12-20 16:35:30 -05:00
bresch	094048ed04	gps_blending: fix selection rapid switching Once a timeout of the primary instance is detected, a fallback is only allowed until the primary receiver is regained.	2023-12-20 16:35:30 -05:00