PX4 ROS2 msg conformity and explicit topics

- .msg files are PascalCase - topics are still either per msg or explicitly listed in TOPICS - compatible structs are still generated (eg struct msg_s), but ROS2 style px4::msg::Msg is also available
uavcan: increase ESC max rate 200->400 Hz
2026-06-03 21:20:04 +08:00 · 2022-03-07 10:36:27 -05:00 · 2022-03-07 10:28:51 -05:00 · 2022-03-04 08:02:11 -05:00 · 2022-03-04 08:02:11 -05:00 · 2022-03-04 08:02:11 -05:00
876 changed files with 20159 additions and 18764 deletions
@@ -28,7 +28,7 @@ pipeline {
          ]

          def base_builds = [
-            target: ["px4_sitl_rtps"],
+            target: ["px4_sitl_default"],
            image: docker_images.base,
            archive: false
          ]
@@ -63,23 +63,17 @@ pipeline {
                     "matek_gnss-m9n-f4_canbootloader",
                     "matek_gnss-m9n-f4_default",
                     "modalai_fc-v1_default",
-                     "modalai_fc-v1_rtps",
                     "modalai_fc-v2_default",
                     "mro_ctrl-zero-f7_default",
                     "mro_ctrl-zero-f7-oem_default",
                     "mro_ctrl-zero-h7_default",
-                     "mro_ctrl-zero-h7_rtps",
                     "mro_ctrl-zero-h7-oem_default",
-                     "mro_ctrl-zero-h7-oem_rtps",
                     "mro_pixracerpro_default",
-                     "mro_pixracerpro_rtps",
                     "mro_x21-777_default",
                     "mro_x21_default",
                     "nxp_fmuk66-e_default",
-                     "nxp_fmuk66-e_rtps",
                     "nxp_fmuk66-e_socketcan",
                     "nxp_fmuk66-v3_default",
-                     "nxp_fmuk66-v3_rtps",
                     "nxp_fmuk66-v3_socketcan",
                     "nxp_fmurt1062-v1_default",
                     "nxp_ucans32k146_canbootloader",
@@ -95,7 +89,6 @@ pipeline {
                     "px4_fmu-v4pro_default",
                     "px4_fmu-v5_debug",
                     "px4_fmu-v5_default",
-                     "px4_fmu-v5_rtps",
                     "px4_fmu-v5_stackcheck",
                     "px4_fmu-v5_uavcanv0periph",
                     "px4_fmu-v5_uavcanv1",
@@ -73,10 +73,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cubepilot_cubeorange_test/cubepilot_cubeorange_test.elf || true'
                }
              }
@@ -145,10 +143,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/cuav_x7pro_test/cuav_x7pro_test.elf || true'
                }
              }
@@ -217,10 +213,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v3_test/px4_fmu-v3_test.elf || true'
                }
              }
@@ -288,10 +282,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4_test/px4_fmu-v4_test.elf || true'
                }
              }
@@ -360,10 +352,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v4pro_test/px4_fmu-v4pro_test.elf || true'
                }
              }
@@ -452,10 +442,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_debug/px4_fmu-v5_debug.elf || true'
                }
              }
@@ -536,10 +524,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_stackcheck/px4_fmu-v5_stackcheck.elf || true'
                }
              }
@@ -608,10 +594,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/px4_fmu-v5_test/px4_fmu-v5_test.elf || true'
                }
              }
@@ -680,10 +664,8 @@ pipeline {
                }
              }
              post {
-                failure {
-                  sh 'cat /tmp/pyserial_spy_file.txt'
-                }
                always {
+                  sh 'cat /tmp/pyserial_spy_file.txt || true'
                  sh './platforms/nuttx/Debug/jlink_gdb_backtrace_simple.sh build/nxp_fmuk66-v3_test/nxp_fmuk66-v3_test.elf || true'
                }
              }
@@ -793,7 +775,7 @@ void checkStatus() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "top once"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uavcan status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ver all"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
  // stop logger
@@ -809,7 +791,7 @@ void resetParameters() {
 void runTests() {

  // test loading a range of airframes
-  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 1000 1001 2100 3000 4001 4018 6001 8001 10016'
+  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 1000 1001 2100 3000 4001 6001 8001 10016'

  resetParameters()

@@ -862,7 +844,7 @@ void runTests() {
 void printTopics() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "ls /obj"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb status"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a" || true'

  // these are for casually inspecting the system, output failure doesn't matter
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener actuator_armed" || true'
@@ -879,7 +861,7 @@ void printTopics() {
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener cpuload" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener distance_sensor" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf2_timestamps" || true'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener ekf_gps_drift" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_gps_status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener esc_status" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_attitude" || true'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "listener estimator_baro_bias" || true'
@@ -23,7 +23,6 @@ jobs:
          "shellcheck_all",
          "NO_NINJA_BUILD=1 px4_fmu-v5_default",
          "NO_NINJA_BUILD=1 px4_sitl_default",
-          "BUILD_MICRORTPS_AGENT=1 px4_sitl_rtps",
          "airframe_metadata",
          "module_documentation",
          "parameters_metadata",
@@ -115,43 +115,3 @@ jobs:
        cd Tools/uorb_graph
        ls -ls *
      # TODO: deploy graph_px4_sitl.json to S3 px4-travis:Firmware/master/
-
-  micrortps_agent:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: microRTPS agent
-      run: |
-        make px4_sitl_rtps
-        git clone https://github.com/PX4/micrortps_agent.git
-        cp -R build/px4_sitl_rtps/src/modules/micrortps_bridge/micrortps_agent/* micrortps_agent
-
-  ROS_msgs:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: PX4 ROS msgs
-      run: |
-        git clone https://github.com/PX4/px4_msgs.git
-        python3 msg/tools/uorb_to_ros_msgs.py msg/ px4_msgs/msg/
-
-  ROS2_bridge:
-    runs-on: ubuntu-latest
-    container: px4io/px4-dev-base-focal:2021-09-08
-    steps:
-    - uses: actions/checkout@v1
-      with:
-        token: ${{ secrets.ACCESS_TOKEN }}
-
-    - name: PX4 ROS2 bridge
-      run: |
-        git clone https://github.com/PX4/px4_ros_com.git
-        ./msg/tools/uorb_to_ros_urtps_topics.py -i msg/tools/urtps_bridge_topics.yaml -o px4_ros_com/templates/urtps_bridge_topics.yaml
@@ -18,10 +18,6 @@
 	path = src/drivers/gps/devices
 	url = https://github.com/PX4/PX4-GPSDrivers.git
 	branch = master
-[submodule "src/modules/micrortps_bridge/micro-CDR"]
-	path = src/modules/micrortps_bridge/micro-CDR
-	url = https://github.com/PX4/Micro-CDR.git
-	branch = master
 [submodule "platforms/nuttx/NuttX/nuttx"]
 	path = platforms/nuttx/NuttX/nuttx
 	url = https://github.com/PX4/NuttX.git
@@ -6,11 +6,6 @@ CONFIG:
      buildType: RelWithDebInfo
      settings:
        CONFIG: px4_sitl_default
-    px4_sitl_rtps:
-      short: px4_sitl_rtps
-      buildType: RelWithDebInfo
-      settings:
-        CONFIG: px4_sitl_rtps
    px4_sitl_asan:
      short: px4_sitl (AddressSanitizer)
      buildType: AddressSanitizer
@@ -31,6 +26,11 @@ CONFIG:
      buildType: RelWithDebInfo
      settings:
        CONFIG: px4_sitl_test
+    px4_ros2_default:
+      short: px4_ros2
+      buildType: RelWithDebInfo
+      settings:
+        CONFIG: px4_ros2_default
    px4_io-v2_default:
      short: px4_io-v2
      buildType: MinSizeRel
@@ -6,7 +6,7 @@
        "C_Cpp.autoAddFileAssociations": false,
        "C_Cpp.clang_format_fallbackStyle": "none",
        "C_Cpp.default.browse.limitSymbolsToIncludedHeaders": true,
-        "C_Cpp.default.cppStandard": "c++14",
+        "C_Cpp.default.cppStandard": "c++17",
        "C_Cpp.default.cStandard": "c11",
        "C_Cpp.formatting": "Disabled",
        "C_Cpp.intelliSenseEngine": "Default",
@@ -121,6 +121,7 @@
                "variant": "cpp",
                "vector": "cpp"
        },
+        "ros.distro": "foxy",
        "search.exclude": {
                "${workspaceFolder}/build": true
        },
@@ -136,5 +137,7 @@
        "yaml.schemas": {
                "${workspaceFolder}/validation/module_schema.yaml": "${workspaceFolder}/src/modules/*/module.yaml"
        },
-        "cortex-debug.openocdPath": "${env:PICO_SDK_PATH}/../openocd/src/openocd"       // Added for rp2040
+        "python.autoComplete.extraPaths": [
+                "/opt/ros/foxy/lib/python3.8/site-packages"
+        ]
 }
@@ -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,6 +1,6 @@
 ############################################################################
 #
-# Copyright (c) 2017 - 2019 PX4 Development Team. All rights reserved.
+# Copyright (c) 2017 - 2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions
@@ -101,6 +101,8 @@

 cmake_minimum_required(VERSION 3.2 FATAL_ERROR)

+cmake_policy(SET CMP0058 NEW)
+
 set(PX4_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}" CACHE FILEPATH "PX4 source directory" FORCE)
 set(PX4_BINARY_DIR "${CMAKE_CURRENT_BINARY_DIR}" CACHE FILEPATH "PX4 binary directory" FORCE)

@@ -138,30 +140,41 @@ define_property(GLOBAL PROPERTY PX4_SRC_FILES
                 BRIEF_DOCS "src files from all PX4 modules & libs"
                 FULL_DOCS "SRC files from px4_add_{module,library}"
                 )
-
+define_property(GLOBAL PROPERTY PX4_PUBLICATIONS
+                 BRIEF_DOCS "PX4 publication topics"
+                 FULL_DOCS "List of topics published by PX4 modules"
+                 )
+define_property(GLOBAL PROPERTY PX4_SUBSCRIPTIONS
+                 BRIEF_DOCS "PX4 subscription topics"
+                 FULL_DOCS "List of topics subscribed by PX4 modules"
+                 )
 #=============================================================================
 # configuration
 #

-set(CONFIG "px4_sitl_default" CACHE STRING "desired configuration")
+# default to px4_ros2_default if building within a ROS2 colcon environment
+if((DEFINED ENV{COLCON_PREFIX_PATH}) AND (DEFINED ENV{ROS_VERSION}))
+	if("$ENV{ROS_VERSION}" MATCHES "2")
+		message(STATUS "ROS_VERSION: $ENV{ROS_VERSION}")
+		set(CONFIG "px4_ros2_default" CACHE STRING "desired configuration") # TODO
+	endif()
+endif()
+
+if(NOT CONFIG)
+	set(CONFIG "px4_sitl_default" CACHE STRING "desired configuration")
+endif()

 include(px4_add_module)
 set(config_module_list)
 set(config_kernel_list)

 # Find Python
-# If using catkin, Python 2 is found since it points
-# to the Python libs installed with the ROS distro
-if (NOT CATKIN_DEVEL_PREFIX)
-	find_package(PythonInterp 3)
-	# We have a custom error message to tell users how to install python3.
-	if (NOT PYTHONINTERP_FOUND)
-		message(FATAL_ERROR "Python 3 not found. Please install Python 3:\n"
-			"    Ubuntu: sudo apt install python3 python3-dev python3-pip\n"
-			"    macOS: brew install python")
-	endif()
-else()
-	find_package(PythonInterp REQUIRED)
+find_package(PythonInterp 3)
+# We have a custom error message to tell users how to install python3.
+if(NOT PYTHONINTERP_FOUND)
+	message(FATAL_ERROR "Python 3 not found. Please install Python 3:\n"
+		"    Ubuntu: sudo apt install python3 python3-dev python3-pip\n"
+		"    macOS: brew install python")
 endif()

 option(PYTHON_COVERAGE "Python code coverage" OFF)
@@ -197,10 +210,6 @@ set_property(GLOBAL PROPERTY PX4_MODULE_CONFIG_FILES)
 include(platforms/${PX4_PLATFORM}/cmake/px4_impl_os.cmake)
 list(APPEND CMAKE_MODULE_PATH ${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake)

-if(EXISTS "${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/init.cmake")
-	include(init)
-endif()
-
 # CMake build type (Debug Release RelWithDebInfo MinSizeRel Coverage)
 if(NOT CMAKE_BUILD_TYPE)
 	if(${PX4_PLATFORM} STREQUAL "nuttx")
@@ -236,20 +245,18 @@ project(px4 CXX C ASM)

 set(package-contact "px4users@googlegroups.com")

-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_C_STANDARD 11)
 set(CMAKE_C_STANDARD_REQUIRED ON)
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)

-# For the catkin build process, unset build of dynamically-linked binaries
-# and do not change CMAKE_RUNTIME_OUTPUT_DIRECTORY
-if (NOT CATKIN_DEVEL_PREFIX)
-	set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PX4_BINARY_DIR})
-	set(CMAKE_RUNTIME_OUTPUT_DIRECTORY_DEBUG ${PX4_BINARY_DIR})
-	set(CMAKE_RUNTIME_OUTPUT_DIRECTORY_RELEASE ${PX4_BINARY_DIR})
-else()
-	SET(BUILD_SHARED_LIBS OFF)
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PX4_BINARY_DIR})
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY_DEBUG ${PX4_BINARY_DIR})
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY_RELEASE ${PX4_BINARY_DIR})
+
+if(EXISTS "${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/init.cmake")
+	include(init)
 endif()

 #=============================================================================
@@ -295,21 +302,6 @@ endif()

 include(ccache)

-#=============================================================================
-# find programs and packages
-#
-
-# see if catkin was invoked to build this
-if (CATKIN_DEVEL_PREFIX)
-	message(STATUS "catkin ENABLED")
-	find_package(catkin REQUIRED)
-	if (catkin_FOUND)
-		catkin_package()
-	else()
-		message(FATAL_ERROR "catkin not found")
-	endif()
-endif()
-
 #=============================================================================
 # get chip and chip manufacturer
 #
@@ -321,13 +313,6 @@ if(NOT PX4_CHIP)
 	message(FATAL_ERROR "px4_os_determine_build_chip() needs to set PX4_CHIP")
 endif()

-#=============================================================================
-# build flags
-#
-include(px4_add_common_flags)
-px4_add_common_flags()
-px4_os_add_flags()
-
 #=============================================================================
 # board cmake init (optional)
 #
@@ -335,6 +320,13 @@ if(EXISTS ${PX4_BOARD_DIR}/cmake/init.cmake)
 	include(${PX4_BOARD_DIR}/cmake/init.cmake)
 endif()

+#=============================================================================
+# build flags
+#
+include(px4_add_common_flags)
+px4_add_common_flags()
+px4_os_add_flags()
+
 #=============================================================================
 # message, and airframe generation
 #
@@ -416,8 +408,13 @@ add_library(parameters_interface INTERFACE)
 include(px4_add_library)
 add_subdirectory(src/lib EXCLUDE_FROM_ALL)

-add_subdirectory(platforms/${PX4_PLATFORM}/src/px4)
-add_subdirectory(platforms EXCLUDE_FROM_ALL)
+add_subdirectory(platforms/${PX4_PLATFORM}/src/px4 EXCLUDE_FROM_ALL)
+
+if(${PX4_PLATFORM} MATCHES "ros2") # TODO: fix
+	add_subdirectory(platforms/common/work_queue EXCLUDE_FROM_ALL)
+else()
+	add_subdirectory(platforms EXCLUDE_FROM_ALL)
+endif()

 if(EXISTS "${PX4_BOARD_DIR}/CMakeLists.txt")
 	add_subdirectory(${PX4_BOARD_DIR})
@@ -439,6 +436,126 @@ target_link_libraries(parameters_interface INTERFACE parameters)
 # firmware added last to generate the builtin for included modules
 add_subdirectory(platforms/${PX4_PLATFORM})

+
+set(PX4_ORB_TOPIC_COUNT 0)
+
+if(${PX4_PLATFORM} MATCHES "ros2") # TODO: fix
+
+	configure_file(${CMAKE_SOURCE_DIR}/build/px4_fmu-v5x_default/uORBTopics.cpp ${CMAKE_BINARY_DIR}/uORBTopics.cpp COPYONLY)
+	configure_file(${CMAKE_SOURCE_DIR}/build/px4_fmu-v5x_default/uORBTopics.hpp ${CMAKE_BINARY_DIR}/uORBTopics.hpp COPYONLY)
+
+	set(PX4_ORB_TOPIC_COUNT 1)
+
+else()
+
+	get_property(publications GLOBAL PROPERTY PX4_PUBLICATIONS)
+	get_property(subscriptions GLOBAL PROPERTY PX4_SUBSCRIPTIONS)
+
+	# TODO: for now combine subsriptions and publications for complete topic list
+	list(APPEND publications ${subscriptions})
+
+	list(SORT publications)
+	list(REMOVE_DUPLICATES publications)
+
+	set(pub_all_topics)
+	set(PX4_MSG_TYPE_ID)
+	set(PX4_MSG_TOPIC_ID)
+	set(PX4_MSG_TOPIC_ID_STRING)
+	set(PX4_MSG_TOPIC_ORB_ID)
+
+	set(PX4_ORB_DECLARE_STR)
+	set(PX4_ORB_DEFINE_STR)
+	set(PX4_ORB_HEADER_INCLUDE_STR)
+
+
+	foreach(pub ${publications})
+		#message(STATUS "pub: ${pub}")
+		string(REPLACE " /" ";" pub ${pub})
+
+		list(GET pub 0 pub_type)
+		list(GET pub 1 pub_topic)
+
+		string(REPLACE "::" ";" pub_type_split ${pub_type})
+		list(GET pub_type_split 2 pub_type_simple_lower)
+
+		# Pascal case to snake case (PubType -> pub_type)
+		string(REGEX REPLACE "(.)([A-Z][a-z]+)" "\\1_\\2" pub_type_simple_lower "${pub_type_simple_lower}")
+		string(REGEX REPLACE "([a-z0-9])([A-Z])" "\\1_\\2" pub_type_simple_lower "${pub_type_simple_lower}")
+		string(TOLOWER "${pub_type_simple_lower}" pub_type_simple_lower)
+
+		#message(STATUS "pub: Type: ${pub_type}, Topic: ${pub_topic} (${pub_type_simple_lower})")
+
+
+
+
+		# pub_type to include path (eg px4::msg::VehicleStatus => px4/msg/vehicle_status.hpp)
+		#  temporary create px4/msg/vehicle_status.hpp which simply includes <uORB/topics/vehicle_status.h>
+
+
+
+
+		list(APPEND pub_all_topics ${pub_topic})
+
+		list(APPEND PX4_MSG_TYPE_ID ${pub_type})
+
+		#list(APPEND PX4_MSG_TOPIC_ID ${pub_topic})
+		set(PX4_MSG_TOPIC_ID "${PX4_MSG_TOPIC_ID}\t${pub_topic},\n")
+		#set(PX4_MSG_TOPIC_ID_STRING "${PX4_MSG_TOPIC_ID_STRING}\t"case ORB_ID::${pub_topic}: return \"${pub_topic}\";"\n")
+		set(PX4_MSG_TOPIC_ORB_ID "${PX4_MSG_TOPIC_ORB_ID}\tORB_ID(${pub_topic}),\n")
+
+		list(APPEND PX4_MSG_TOPIC_ID_STRING
+			"case ORB_ID::${pub_topic}: return \"${pub_topic}\";\n"
+		)
+
+		# PX4_MSG_TYPE_ID
+		# PX4_MSG_TOPIC_ID
+
+		# .h  ORB_DECLARE(actuator_controls_0);
+		# .c  ORB_DEFINE(actuator_controls_0, struct actuator_controls_s, 48, __orb_actuator_controls_fields, static_cast<uint8_t>(ORB_ID::actuator_controls_0));
+
+
+		set(PX4_ORB_DECLARE_STR
+			"${PX4_ORB_DECLARE_STR}ORB_DECLARE(${pub_topic});\n")
+
+		# ORB_DEFINE(actuator_armed, struct actuator_armed_s, 16, __orb_actuator_armed_fields, static_cast<uint8_t>(ORB_ID::actuator_armed));
+		set(PX4_ORB_DEFINE_STR
+			"${PX4_ORB_DEFINE_STR}ORB_DEFINE(${pub_topic}, ${pub_type}, px4_embedded::${pub_type_simple_lower}_s::SIZE_NO_PADDING, px4_embedded::${pub_type_simple_lower}_s::FIELDS, static_cast<uint8_t>(ORB_ID::${pub_topic}));\n")
+
+		set(PX4_ORB_HEADER_INCLUDE_STR
+			"${PX4_ORB_HEADER_INCLUDE_STR}#include <uORB/topics/${pub_type_simple_lower}.h>\n")
+
+		math(EXPR PX4_ORB_TOPIC_COUNT "${PX4_ORB_TOPIC_COUNT}+1")
+
+	endforeach()
+	list(REMOVE_DUPLICATES PX4_MSG_TYPE_ID)
+	#list(REMOVE_DUPLICATES PX4_MSG_TOPIC_ID)
+
+	#message(STATUS "PX4_MSG_TYPE_ID: ${PX4_MSG_TYPE_ID}")
+	#message(STATUS "PX4_MSG_TOPIC_ID: ${PX4_MSG_TOPIC_ID}")
+
+	if(PX4_ORB_TOPIC_COUNT GREATER 0)
+		configure_file(uORBTopics.cpp.in ${CMAKE_BINARY_DIR}/uORBTopics.cpp)
+		configure_file(uORBTopics.hpp.in ${CMAKE_BINARY_DIR}/uORBTopics.hpp)
+	endif()
+
+	# .hpp enum ORB_ID
+	# .cpp struct orb_metadat ORB_ID() array
+
+	foreach(f ${msg_files})
+		#message(STATUS "MSG: ${f}")
+	endforeach()
+endif()
+
+if(PX4_ORB_TOPIC_COUNT GREATER 0)
+	add_library(uorb_msgs ${uorb_headers} ${CMAKE_BINARY_DIR}/uORBTopics.cpp ${CMAKE_BINARY_DIR}/uORBTopics.hpp)
+	add_dependencies(uorb_msgs prebuild_targets uorb_headers)
+endif()
+
+if(${PX4_PLATFORM} MATCHES "ros2") # TODO: fix
+	ament_target_dependencies(uorb_msgs rclcpp std_msgs)
+	rosidl_target_interfaces(uorb_msgs ${PROJECT_NAME} "rosidl_typesupport_cpp")
+endif()
+
 #=============================================================================
 # uORB graph generation: add a custom target 'uorb_graph'
 #
@@ -466,17 +583,13 @@ include(doxygen)
 include(metadata)
 include(package)

-# print size
-add_custom_target(size
-	COMMAND size $<TARGET_FILE:px4>
-	DEPENDS px4
-	WORKING_DIRECTORY ${PX4_BINARY_DIR}
-	USES_TERMINAL
-	)
-
 # install python requirements using configured python
 add_custom_target(install_python_requirements
 	COMMAND ${PYTHON_EXECUTABLE} -m pip install --requirement ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt
 	DEPENDS ${PX4_SOURCE_DIR}/Tools/setup/requirements.txt
 	USES_TERMINAL
 )
+
+if(EXISTS "${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/finalize.cmake")
+	include(finalize)
+endif()
@@ -829,7 +829,6 @@ RECURSIVE              = YES
 # run.

 EXCLUDE                = @CMAKE_SOURCE_DIR@/src/modules/uavcan/libuavcan \
-                         @CMAKE_SOURCE_DIR@/src/modules/micrortps_bridge/micro-CDR \
                         @CMAKE_SOURCE_DIR@/src/examples \
                         @CMAKE_SOURCE_DIR@/src/templates

@@ -12,76 +12,6 @@ pipeline {
        }
      }
      parallel {
-        // stage('Catkin build on ROS workspace') {
-        //   agent {
-        //     docker {
-        //       image 'px4io/px4-dev-ros-melodic:2021-08-18'
-        //       args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
-        //     }
-        //   }
-        //   steps {
-        //     sh 'ls -l'
-        //     sh '''#!/bin/bash -l
-        //       echo $0;
-        //       mkdir -p catkin_ws/src;
-        //       cd catkin_ws;
-        //       git -C ${WORKSPACE}/catkin_ws/src/Firmware submodule update --init --recursive --force Tools/sitl_gazebo
-        //       git clone --recursive ${WORKSPACE}/catkin_ws/src/Firmware/Tools/sitl_gazebo src/mavlink_sitl_gazebo;
-        //       git -C ${WORKSPACE}/catkin_ws/src/Firmware fetch --tags;
-        //       source /opt/ros/melodic/setup.bash;
-        //       export PYTHONPATH=/opt/ros/$ROS_DISTRO/lib/python2.7/dist-packages:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages;
-        //       catkin init;
-        //       catkin build -j$(nproc) -l$(nproc);
-        //     '''
-        //     // test if the binary was correctly installed and runs using 'mavros_posix_silt.launch'
-        //     sh '''#!/bin/bash -l
-        //       echo $0;
-        //       source catkin_ws/devel/setup.bash;
-        //       rostest px4 pub_test.launch;
-        //     '''
-        //   }
-        //   post {
-        //     always {
-        //       sh 'rm -rf catkin_ws'
-        //     }
-        //     failure {
-        //       archiveArtifacts(allowEmptyArchive: false, artifacts: '.ros/**/*.xml, .ros/**/*.log')
-        //     }
-        //   }
-        //   options {
-        //     checkoutToSubdirectory('catkin_ws/src/Firmware')
-        //   }
-        // }
-
-        stage('Colcon build on ROS2 workspace') {
-          agent {
-            docker {
-              image 'px4io/px4-dev-ros2-foxy:2021-08-18'
-              args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE'
-            }
-          }
-          steps {
-            sh 'ls -l'
-            sh '''#!/bin/bash -l
-              echo $0;
-              unset ROS_DISTRO;
-              mkdir -p colcon_ws/src;
-              cd colcon_ws;
-              git -C ${WORKSPACE}/colcon_ws/src/Firmware submodule update --init --recursive --force Tools/sitl_gazebo;
-              git -C ${WORKSPACE}/colcon_ws/src/Firmware fetch --tags;
-              source /opt/ros/foxy/setup.sh;
-              colcon build --event-handlers console_direct+ --symlink-install;
-            '''
-          }
-          post {
-            always {
-              sh 'rm -rf colcon_ws'
-            }
-          }
-          options {
-            checkoutToSubdirectory('colcon_ws/src/Firmware')
-          }
-        }

        stage('Airframe') {
          agent {
@@ -254,108 +184,6 @@ pipeline {
          }
        }

-        stage('microRTPS agent') {
-          agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
-          }
-          steps {
-            sh('export')
-            sh('git fetch --all --tags')
-            sh('make distclean; git clean -ff -x -d .')
-            sh('make px4_sitl_rtps')
-            withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) {
-              sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/micrortps_agent.git -b ${BRANCH_NAME}")
-              sh("rm -rf micrortps_agent/src micrortps_agent/idl")
-              sh('cp -R build/px4_sitl_rtps/src/modules/micrortps_bridge/micrortps_agent/* micrortps_agent')
-              sh('cd micrortps_agent; git status; git add src; git commit -a -m "Update microRTPS agent source code `date`" || true')
-              sh('cd micrortps_agent; git push origin ${BRANCH_NAME} || true')
-              sh('cd micrortps_agent; git status; git add idl; git commit -a -m "Update IDL definitions `date`" || true')
-              sh('cd micrortps_agent; git push origin ${BRANCH_NAME} || true')
-              sh('cd micrortps_agent; git status; git add CMakeLists.txt; git commit -a -m "Update CMakeLists.txt `date`" || true')
-              sh('cd micrortps_agent; git push origin ${BRANCH_NAME} || true')
-              sh('rm -rf micrortps_agent')
-            }
-          }
-          when {
-            anyOf {
-              branch 'master'
-              branch 'pr-jenkins' // for testing
-            }
-          }
-        }
-
-        stage('PX4 ROS msgs') {
-          agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
-          }
-          steps {
-            sh('export')
-            sh('make distclean; git clean -ff -x -d .')
-            withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) {
-              sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_msgs.git")
-              // 'master' branch
-              sh('./msg/tools/uorb_to_ros_msgs.py msg/ px4_msgs/msg/')
-              sh('cd px4_msgs; git status; git add .; git commit -a -m "Update message definitions `date`" || true')
-              sh('cd px4_msgs; git push origin master || true')
-              // 'ros1' branch
-              sh('cd px4_msgs; git checkout ros1')
-              sh('./msg/tools/uorb_to_ros_msgs.py msg/ px4_msgs/msg/')
-              sh('cd px4_msgs; git status; git add .; git commit -a -m "Update message definitions `date`" || true')
-              sh('cd px4_msgs; git push origin ros1 || true')
-              sh('rm -rf px4_msgs')
-            }
-          }
-          when {
-            anyOf {
-              branch 'master'
-              branch 'pr-jenkins' // for testing
-            }
-          }
-        }
-
-        stage('PX4 ROS2 bridge') {
-          agent {
-            docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
-          }
-          steps {
-            sh('export')
-            sh('make distclean; git clean -ff -x -d .')
-            withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) {
-              sh("git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/px4_ros_com.git -b ${BRANCH_NAME}")
-              // deploy uORB RTPS ID map
-              sh('./msg/tools/uorb_to_ros_urtps_topics.py -i msg/tools/urtps_bridge_topics.yaml -o px4_ros_com/templates/urtps_bridge_topics.yaml')
-              sh('cd px4_ros_com; git status; git add .; git commit -a -m "Update uORB RTPS ID map `date`" || true')
-              sh('cd px4_ros_com; git push origin ${BRANCH_NAME} || true')
-              // deploy uORB RTPS required tools
-              sh('cp msg/tools/uorb_rtps_classifier.py px4_ros_com/scripts/uorb_rtps_classifier.py')
-              sh('cp msg/tools/generate_microRTPS_bridge.py px4_ros_com/scripts/generate_microRTPS_bridge.py')
-              sh('cp msg/tools/px_generate_uorb_topic_files.py px4_ros_com/scripts/px_generate_uorb_topic_files.py')
-              sh('cp msg/tools/px_generate_uorb_topic_helper.py px4_ros_com/scripts/px_generate_uorb_topic_helper.py')
-              // deploy templates
-              sh('cp msg/templates/urtps/microRTPS_agent.cpp.em px4_ros_com/templates/microRTPS_agent.cpp.em')
-              sh('cp msg/templates/urtps/microRTPS_timesync.cpp.em px4_ros_com/templates/microRTPS_timesync.cpp.em')
-              sh('cp msg/templates/urtps/microRTPS_timesync.h.em px4_ros_com/templates/microRTPS_timesync.h.em')
-              sh('cp msg/templates/urtps/microRTPS_transport.cpp px4_ros_com/templates/microRTPS_transport.cpp')
-              sh('cp msg/templates/urtps/microRTPS_transport.h px4_ros_com/templates/microRTPS_transport.h')
-              sh('cp msg/templates/urtps/Publisher.cpp.em px4_ros_com/templates/Publisher.cpp.em')
-              sh('cp msg/templates/urtps/Publisher.h.em px4_ros_com/templates/Publisher.h.em')
-              sh('cp msg/templates/urtps/Subscriber.cpp.em px4_ros_com/templates/Subscriber.cpp.em')
-              sh('cp msg/templates/urtps/Subscriber.h.em px4_ros_com/templates/Subscriber.h.em')
-              sh('cp msg/templates/urtps/RtpsTopics.cpp.em px4_ros_com/templates/RtpsTopics.cpp.em')
-              sh('cp msg/templates/urtps/RtpsTopics.h.em px4_ros_com/templates/RtpsTopics.h.em')
-              sh('cd px4_ros_com; git status; git add .; git commit -a -m "Update uORB RTPS script tools `date`" || true')
-              sh('cd px4_ros_com; git push origin ${BRANCH_NAME} || true')
-              sh('rm -rf px4_msgs')
-            }
-          }
-          when {
-            anyOf {
-              branch 'master'
-              branch 'pr-jenkins' // for testing
-            }
-          }
-        }
-
        stage('S3') {
          agent {
            docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
@@ -17,6 +17,8 @@ menu "Toolchain"
            bool "posix"
        config PLATFORM_QURT
            bool "qurt"
+        config PLATFORM_ROS2
+            bool "ros2"
    endchoice

    config BOARD_PLATFORM
@@ -24,6 +26,7 @@ menu "Toolchain"
        default "nuttx" if PLATFORM_NUTTX
        default "posix" if PLATFORM_POSIX
        default "qurt" if PLATFORM_QURT
+        default "ros2" if PLATFORM_ROS2

    config BOARD_LOCKSTEP
        bool "Force enable lockstep"
@@ -170,11 +170,6 @@ ifdef PYTHON_EXECUTABLE
 	CMAKE_ARGS += -DPYTHON_EXECUTABLE=${PYTHON_EXECUTABLE}
 endif

-# Check if the microRTPS agent is to be built
-ifdef BUILD_MICRORTPS_AGENT
-  CMAKE_ARGS += -DBUILD_MICRORTPS_AGENT=ON
-endif
-
 # Functions
 # --------------------------------------------------------------------
 # describe how to build a cmake config
@@ -251,7 +246,7 @@ endef
 # Other targets
 # --------------------------------------------------------------------

-.PHONY: qgc_firmware px4fmu_firmware misc_qgc_extra_firmware check_rtps
+.PHONY: qgc_firmware px4fmu_firmware misc_qgc_extra_firmware

 # QGroundControl flashable NuttX firmware
 qgc_firmware: px4fmu_firmware misc_qgc_extra_firmware
@@ -276,15 +271,7 @@ misc_qgc_extra_firmware: \
 	check_airmind_mindpx-v2_default \
 	sizes

-# builds with RTPS
-check_rtps: \
-	check_px4_fmu-v3_rtps \
-	check_px4_fmu-v4_rtps \
-	check_px4_fmu-v4pro_rtps \
-	check_px4_sitl_rtps \
-	sizes
-
-.PHONY: sizes check quick_check check_rtps uorb_graphs
+.PHONY: sizes check quick_check uorb_graphs

 sizes:
 	@-find build -name *.elf -type f | xargs size 2> /dev/null || :
@@ -479,7 +466,7 @@ clang-tidy-quiet: px4_sitl_default-clang
 # TODO: Fix cppcheck errors then try --enable=warning,performance,portability,style,unusedFunction or --enable=all
 cppcheck: px4_sitl_default
 	@mkdir -p "$(SRC_DIR)"/build/cppcheck
-	@cppcheck -i"$(SRC_DIR)"/src/examples --enable=performance --std=c++14 --std=c99 --std=posix --project="$(SRC_DIR)"/build/px4_sitl_default/compile_commands.json --xml-version=2 2> "$(SRC_DIR)"/build/cppcheck/cppcheck-result.xml > /dev/null
+	@cppcheck -i"$(SRC_DIR)"/src/examples --enable=performance --std=c++17 --std=c99 --std=posix --project="$(SRC_DIR)"/build/px4_sitl_default/compile_commands.json --xml-version=2 2> "$(SRC_DIR)"/build/cppcheck/cppcheck-result.xml > /dev/null
 	@cppcheck-htmlreport --source-encoding=ascii --file="$(SRC_DIR)"/build/cppcheck/cppcheck-result.xml --report-dir="$(SRC_DIR)"/build/cppcheck --source-dir="$(SRC_DIR)"/src/

 shellcheck_all:
@@ -559,12 +546,3 @@ ifneq ($(ROS2_WS_DIR),)
 else
  ROS2_WS_DIR := ~/colcon_ws
 endif
-
-update_ros2_bridge:
-	@Tools/update_px4_ros2_bridge.sh --ws_dir ${ROS2_WS_DIR} --all
-
-update_px4_ros_com:
-	@Tools/update_px4_ros2_bridge.sh --ws_dir ${ROS2_WS_DIR} --px4_ros_com
-
-update_px4_msgs:
-	@Tools/update_px4_ros2_bridge.sh --ws_dir ${ROS2_WS_DIR} --px4_msgs
@@ -36,7 +36,6 @@ add_subdirectory(airframes)
 px4_add_romfs_files(
 	px4-rc.mavlink
 	px4-rc.params
-	px4-rc.rtps
 	px4-rc.simulator
 	rc.replay
 	rcS
@@ -44,6 +44,8 @@ param set-default MIS_TAKEOFF_ALT 30
 param set-default NAV_ACC_RAD 15
 param set-default NAV_DLL_ACT 2

+param set-default FW_USE_NPFG 1
+
 param set-default RWTO_TKOFF 1

 set MIXER_FILE etc/mixers-sitl/plane_sitl.main.mix
@@ -10,7 +10,7 @@
 # param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 2

-param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR_COUNT 5
 param set-default CA_ROTOR0_PX 0.1515
 param set-default CA_ROTOR0_PY 0.245
 param set-default CA_ROTOR0_KM 0.05
@@ -23,6 +23,9 @@ param set-default CA_ROTOR2_KM -0.05
 param set-default CA_ROTOR3_PX -0.1515
 param set-default CA_ROTOR3_PY 0.1875
 param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_ROTOR4_PX 0.2

 param set-default CA_SV_CS_COUNT 3
 param set-default CA_SV_CS0_TYPE 1
@@ -1,4 +0,0 @@
-#!/bin/sh
-# shellcheck disable=SC2154
-
-micrortps_client start -t UDP -r $((2019+2*px4_instance)) -s $((2020+2*px4_instance))
@@ -226,15 +226,9 @@ sensors start
 commander start
 navigator start

-# Try to start the micrortps_client with UDP transport if module exists
-if px4-micrortps_client status > /dev/null 2>&1
-then
-	. px4-rc.rtps
-fi
-
 if param greater -s MNT_MODE_IN -1
 then
-	vmount start
+	gimbal start
 fi

 if param greater -s TRIG_MODE 0
@@ -39,6 +39,26 @@ param set-default FW_RR_P 0.3

 param set-default RWTO_TKOFF 1

+param set-default CA_AIRFRAME 1
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.3
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TRQ_R 0.5
+param set-default CA_SV_CS0_TYPE 2
+param set-default CA_SV_CS1_TRQ_P 1.0
+param set-default CA_SV_CS1_TYPE 3
+param set-default CA_SV_CS2_TRQ_Y 1.0
+param set-default CA_SV_CS2_TYPE 4
+param set-default CA_SV_CS3_TYPE 10
+
+param set-default HIL_ACT_REV 2
+param set-default HIL_ACT_FUNC1 201
+param set-default HIL_ACT_FUNC2 202
+param set-default HIL_ACT_FUNC3 203
+param set-default HIL_ACT_FUNC4 101
+param set-default HIL_ACT_FUNC5 204
+param set-default HIL_ACT_FUNC6 400
+
 param set SYS_HITL 1

 # disable some checks to allow to fly
@@ -15,6 +15,23 @@ set MIXER quad_x

 param set SYS_HITL 1

+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC3 103
+param set-default HIL_ACT_FUNC4 104
+
 # disable some checks to allow to fly
 # - with usb
 param set-default CBRK_USB_CHK 197848
@@ -61,6 +61,37 @@ param set-default VT_MOT_ID 1234
 param set-default VT_FW_MOT_OFFID 1234
 param set-default VT_TYPE 2

+param set-default CA_AIRFRAME 2
+param set-default CA_ROTOR_COUNT 5
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_ROTOR4_PX 0.2
+
+param set-default CA_SV_CS_COUNT 3
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS2_TYPE 3
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC3 103
+param set-default HIL_ACT_FUNC4 104
+param set-default HIL_ACT_FUNC5 105
+param set-default HIL_ACT_FUNC6 201
+param set-default HIL_ACT_FUNC7 202
+param set-default HIL_ACT_FUNC8 203
+

 param set SYS_HITL 1

@@ -17,6 +17,23 @@ set PWM_OUT 1234

 param set UAVCAN_ENABLE 0

+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC3 103
+param set-default HIL_ACT_FUNC4 104
+
 # set SYS_HITL to 2 to start the SIH and avoid sensors startup
 param set SYS_HITL 2

@@ -17,6 +17,26 @@ set PWM_OUT 1234

 param set UAVCAN_ENABLE 0

+param set-default CA_AIRFRAME 1
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.3
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TRQ_R 0.5
+param set-default CA_SV_CS0_TYPE 2
+param set-default CA_SV_CS1_TRQ_P 1.0
+param set-default CA_SV_CS1_TYPE 3
+param set-default CA_SV_CS2_TRQ_Y 1.0
+param set-default CA_SV_CS2_TYPE 4
+param set-default CA_SV_CS3_TYPE 10
+
+param set-default HIL_ACT_REV 2
+param set-default HIL_ACT_FUNC1 201
+param set-default HIL_ACT_FUNC2 202
+param set-default HIL_ACT_FUNC3 203
+param set-default HIL_ACT_FUNC4 101
+param set-default HIL_ACT_FUNC5 204
+param set-default HIL_ACT_FUNC6 400
+
 # set SYS_HITL to 2 to start the SIH and avoid sensors startup
 param set-default SYS_HITL 2

@@ -26,6 +26,26 @@ param set-default VT_FW_DIFTHR_SC 0.3
 param set-default MPC_MAN_Y_MAX 60
 param set-default MC_PITCH_P 5

+param set-default CA_AIRFRAME 4
+param set-default CA_ROTOR_COUNT 2
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR0_PY 0.2
+param set-default CA_ROTOR1_KM -0.05
+param set-default CA_ROTOR1_PY -0.2
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TRQ_P 0.3
+param set-default CA_SV_CS0_TRQ_Y 0.3
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS1_TRQ_P 0.3
+param set-default CA_SV_CS1_TRQ_Y -0.3
+param set-default CA_SV_CS1_TYPE 6
+
+param set-default HIL_ACT_FUNC1 101
+param set-default HIL_ACT_FUNC2 102
+param set-default HIL_ACT_FUNC5 202
+param set-default HIL_ACT_FUNC6 201
+param set-default HIL_ACT_REV 32
+
 param set-default MAV_TYPE 19
 set MAV_TYPE 19
 set MIXER vtol_tailsitter_duo_sat
@@ -24,6 +24,30 @@

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

+param set-default CA_AIRFRAME 2
+param set-default CA_ROTOR_COUNT 5
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_ROTOR4_PX 0.2
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS3_TRQ_Y 1.0
+param set-default CA_SV_CS3_TYPE 4

 param set-default PWM_AUX_DIS5 950

@@ -13,6 +13,25 @@

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

+param set-default CA_AIRFRAME 4
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TRQ_P 0.5
+param set-default CA_SV_CS0_TRQ_Y 0.5
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS1_TRQ_P 0.5
+param set-default CA_SV_CS1_TRQ_Y -0.5
+param set-default CA_SV_CS1_TYPE 6

 param set-default PWM_MAIN_MAX 2000

@@ -31,6 +31,32 @@ param set-default VT_IDLE_PWM_MC 1100
 param set-default VT_TYPE 1
 param set-default VT_MOT_ID 1234
 param set-default VT_FW_MOT_OFFID 24
+
+param set-default CA_AIRFRAME 3
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR0_TILT 1
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR1_TILT 2
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR2_TILT 3
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR3_TILT 4
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS3_TRQ_Y 1.0
+param set-default CA_SV_CS3_TYPE 4
+param set-default CA_SV_TL_COUNT 4
+
 set MAV_TYPE 21

 set MIXER quad_x
@@ -23,6 +23,20 @@ param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0

+param set-default CA_AIRFRAME 4
+param set-default CA_ROTOR_COUNT 2
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR0_PY 0.2
+param set-default CA_ROTOR1_KM -0.05
+param set-default CA_ROTOR1_PY -0.2
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TRQ_P 0.5
+param set-default CA_SV_CS0_TRQ_Y 0.5
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS1_TRQ_P 0.5
+param set-default CA_SV_CS1_TRQ_Y -0.5
+param set-default CA_SV_CS1_TYPE 6
+
 param set-default MAV_TYPE 19
 set MAV_TYPE 19
 set MIXER vtol_tailsitter_duo
@@ -23,6 +23,18 @@

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

+param set-default CA_AIRFRAME 1
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_ROTOR0_PX 0.3
+param set-default CA_SV_CS_COUNT 4
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TRQ_R 0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS3_TRQ_Y 1.0
+param set-default CA_SV_CS3_TYPE 4
+
 param set-default PWM_AUX_RATE 50
 param set-default PWM_MAIN_RATE 50

@@ -21,3 +21,16 @@
 #

 . ${R}etc/init.d/rc.mc_defaults
+
+param set-default CA_ROTOR_COUNT 4
+param set-default CA_ROTOR0_PX 0.15
+param set-default CA_ROTOR0_PY 0.15
+param set-default CA_ROTOR1_PX -0.15
+param set-default CA_ROTOR1_PY -0.15
+param set-default CA_ROTOR2_PX 0.15
+param set-default CA_ROTOR2_PY -0.15
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.15
+param set-default CA_ROTOR3_PY 0.15
+param set-default CA_ROTOR3_KM -0.05
+
@@ -1,39 +0,0 @@
-#!/bin/sh
-#
-# @name S500 with control allocation
-#
-# @type Quadrotor x
-# @class Copter
-#
-# @maintainer Silvan Fuhrer
-#
-# @board px4_fmu-v2 exclude
-#
-
-. ${R}etc/init.d/rc.mc_defaults
-
-set MIXER skip
-set MIXER_AUX none
-
-param set-default SYS_CTRL_ALLOC 1
-
-param set-default CA_AIRFRAME 0
-
-param set-default CA_ROTOR_COUNT 4
-param set-default CA_ROTOR0_PX 0.177
-param set-default CA_ROTOR0_PY 0.177
-param set-default CA_ROTOR0_KM 0.05
-param set-default CA_ROTOR1_PX -0.177
-param set-default CA_ROTOR1_PY -0.177
-param set-default CA_ROTOR1_KM 0.05
-param set-default CA_ROTOR2_PX 0.177
-param set-default CA_ROTOR2_PY -0.177
-param set-default CA_ROTOR2_KM -0.05
-param set-default CA_ROTOR3_PX -0.177
-param set-default CA_ROTOR3_PY 0.177
-param set-default CA_ROTOR3_KM -0.05
-
-param set-default PWM_MAIN_FUNC1 101
-param set-default PWM_MAIN_FUNC2 102
-param set-default PWM_MAIN_FUNC3 103
-param set-default PWM_MAIN_FUNC4 104
@@ -46,7 +46,7 @@ param set-default COM_RC_LOSS_T 3


 # ekf2
-param set-default EKF2_AID_MASK 35
+param set-default EKF2_AID_MASK 33
 param set-default EKF2_BARO_DELAY 0
 param set-default EKF2_BARO_NOISE 2.0

@@ -147,7 +147,7 @@ param set-default MPC_Z_VEL_P 0.27


 # gimbal configuration
-param set-default MNT_MODE_IN 1
+param set-default MNT_MODE_IN 0
 param set-default MNT_MODE_OUT 1
 param set-default MNT_MAN_PITCH 2
 param set-default MNT_RC_IN_MODE 1
@@ -164,6 +164,8 @@ param set-default RC_MAP_AUX2       5
 param set-default RC_MAP_AUX3       10
 param set-default RC_MAP_AUX4       8
 param set-default RC_MAP_FLTMODE    6
+param set-default RC_MAP_RETURN_SW  7
+
 param set-default RC1_TRIM          1000


@@ -23,6 +23,23 @@

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

+param set-default CA_ROTOR_COUNT 6
+param set-default CA_ROTOR0_PX 0.0
+param set-default CA_ROTOR0_PY 0.5
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR1_PX 0.0
+param set-default CA_ROTOR1_PY -0.5
+param set-default CA_ROTOR2_PX 0.43
+param set-default CA_ROTOR2_PY -0.25
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.43
+param set-default CA_ROTOR3_PY 0.25
+param set-default CA_ROTOR4_PX 0.43
+param set-default CA_ROTOR4_PY 0.25
+param set-default CA_ROTOR5_PX -0.43
+param set-default CA_ROTOR5_PY -0.25
+param set-default CA_ROTOR5_KM -0.05
+
 set MIXER hexa_x

 # Need to set all 8 channels
@@ -1,52 +0,0 @@
-#!/bin/sh
-#
-# @name Hex X with control allocation
-#
-# @type Hexarotor x
-# @class Copter
-#
-# @maintainer Silvan Fuhrer
-#
-# @board px4_fmu-v2 exclude
-#
-
-. ${R}etc/init.d/rc.mc_defaults
-
-param set-default SYS_CTRL_ALLOC 1
-
-param set-default CA_AIRFRAME 0
-param set-default CA_ROTOR_COUNT 6
-
-param set-default CA_ROTOR0_PX 0.0
-param set-default CA_ROTOR0_PY 0.275
-param set-default CA_ROTOR0_KM -0.05
-
-param set-default CA_ROTOR1_PX 0.0
-param set-default CA_ROTOR1_PY -0.275
-param set-default CA_ROTOR1_KM 0.05
-
-param set-default CA_ROTOR2_PX 0.238
-param set-default CA_ROTOR2_PY -0.1375
-param set-default CA_ROTOR2_KM -0.05
-
-param set-default CA_ROTOR3_PX -0.238
-param set-default CA_ROTOR3_PY 0.1375
-param set-default CA_ROTOR3_KM 0.05
-
-param set-default CA_ROTOR4_PX 0.238
-param set-default CA_ROTOR4_PY 0.1375
-param set-default CA_ROTOR4_KM 0.05
-
-param set-default CA_ROTOR5_PX -0.238
-param set-default CA_ROTOR5_PY -0.1375
-param set-default CA_ROTOR5_KM -0.05
-
-param set-default PWM_MAIN_FUNC1 101
-param set-default PWM_MAIN_FUNC2 102
-param set-default PWM_MAIN_FUNC3 103
-param set-default PWM_MAIN_FUNC4 104
-param set-default PWM_MAIN_FUNC5 105
-param set-default PWM_MAIN_FUNC6 106
-
-set MIXER skip
-set MIXER_AUX none
@@ -23,6 +23,23 @@

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

+param set-default CA_ROTOR_COUNT 6
+param set-default CA_ROTOR0_PX 0.5
+param set-default CA_ROTOR0_PY 0.0
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR1_PX -0.5
+param set-default CA_ROTOR1_PY 0.0
+param set-default CA_ROTOR2_PX -0.25
+param set-default CA_ROTOR2_PY -0.43
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX 0.25
+param set-default CA_ROTOR3_PY 0.43
+param set-default CA_ROTOR4_PX 0.25
+param set-default CA_ROTOR4_PY -0.43
+param set-default CA_ROTOR5_PX -0.25
+param set-default CA_ROTOR5_PY 0.43
+param set-default CA_ROTOR5_KM -0.05
+
 set MIXER hexa_+

 # Need to set all 8 channels
@@ -25,6 +25,28 @@

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

+param set-default CA_ROTOR_COUNT 8
+param set-default CA_ROTOR0_KM -0.05
+param set-default CA_ROTOR0_PX 0.46
+param set-default CA_ROTOR0_PY 0.19
+param set-default CA_ROTOR1_KM -0.05
+param set-default CA_ROTOR1_PX -0.46
+param set-default CA_ROTOR1_PY -0.19
+param set-default CA_ROTOR2_PX 0.19
+param set-default CA_ROTOR2_PY 0.46
+param set-default CA_ROTOR3_PX -0.46
+param set-default CA_ROTOR3_PY 0.19
+param set-default CA_ROTOR4_PX 0.46
+param set-default CA_ROTOR4_PY -0.19
+param set-default CA_ROTOR5_PX -0.19
+param set-default CA_ROTOR5_PY -0.46
+param set-default CA_ROTOR6_KM -0.05
+param set-default CA_ROTOR6_PX 0.19
+param set-default CA_ROTOR6_PY -0.46
+param set-default CA_ROTOR7_KM -0.05
+param set-default CA_ROTOR7_PX -0.19
+param set-default CA_ROTOR7_PY 0.46
+
 set MIXER octo_x

 set PWM_OUT 12345678
@@ -72,7 +72,6 @@ px4_add_romfs_files(
 	4015_holybro_s500
 	4016_holybro_px4vision
 	4017_nxp_hovergames
-	4018_s500_ctrlalloc
 	4019_x500_v2
 	4030_3dr_solo
 	4031_3dr_quad
@@ -100,7 +99,6 @@ px4_add_romfs_files(
 	# [6000, 6999] Hexarotor x"
 	6001_hexa_x
 	6002_draco_r
-	6003_hexa_x_ctrlalloc

 	# [7000, 7999] Hexarotor +"
 	7001_hexa_+
@@ -149,6 +149,19 @@ then
 	ms5525_airspeed start -X
 fi

+# IR-LOCK sensor external I2C
+if param compare -s SENS_EN_IRLOCK 1
+then
+	irlock start -X
+fi
+
+# PCF8583 counter (RPM sensor)
+if param compare -s SENS_EN_PCF8583 1
+then
+	pcf8583 start -X
+	pcf8583 start -X -a 0x51
+fi
+
 # probe for optional external I2C devices
 if param compare SENS_EXT_I2C_PRB 1
 then
@@ -486,11 +486,11 @@ else
 	. ${R}etc/init.d/rc.thermal_cal

 	#
-	# Start vmount to control mounts such as gimbals, disabled by default.
+	# Start gimbal to control mounts such as gimbals, disabled by default.
 	#
 	if param greater -s MNT_MODE_IN -1
 	then
-		vmount start
+		gimbal start
 	fi

 	# Check for flow sensor
@@ -35,7 +35,6 @@ do
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_0_CONFIG 101' || true
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_1_CONFIG 102' || true
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_2_CONFIG 103' || true
-	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set MAV_3_CONFIG 104' || true

 	# enable all GPS
 	${DIR}/run_nsh_cmd.py --device ${SERIAL_DEVICE} --cmd 'param set GPS_1_CONFIG 201' || true
@@ -8,7 +8,6 @@ if [ $# -gt 0 ]; then
 fi

 exec find boards msg src platforms test \
-    -path msg/templates/urtps -prune -o \
    -path platforms/nuttx/NuttX -prune -o \
    -path platforms/qurt/dspal -prune -o \
    -path src/drivers/uavcan/libuavcan -prune -o \
@@ -21,8 +20,6 @@ exec find boards msg src platforms test \
    -path src/modules/ekf2/EKF -prune -o \
    -path src/modules/gyro_fft/CMSIS_5 -prune -o \
    -path src/modules/mavlink/mavlink -prune -o \
-    -path src/modules/micrortps_bridge/micro-CDR -prune -o \
-    -path src/modules/micrortps_bridge/microRTPS_client -prune -o \
    -path test/mavsdk_tests/catch2 -prune -o \
    -path src/lib/crypto/monocypher -prune -o \
    -path src/lib/crypto/libtomcrypt -prune -o \
@@ -1,15 +0,0 @@
-#!/usr/bin/env bash
-set -e
-
-SCRIPT_DIR=$0
-if [[ ${SCRIPT_DIR:0:1} != '/' ]]; then
-  SCRIPT_DIR=$(dirname $(realpath -s "$PWD/$0"))
-fi
-
-PX4_DIR=$(cd "$(dirname $(dirname $SCRIPT_DIR))" && pwd)
-
-if [ -d $PX4_DIR/build/*_rtps ]; then
-  cd $PX4_DIR/build/*_rtps/src/modules/micrortps_bridge/micrortps_agent/
-  cmake -Bbuild
-  cmake --build build -j$(nproc --all)
-fi
@@ -93,7 +93,6 @@ pca9685_pwm_out,CONFIG_DRIVERS_PCA9685_PWM_OUT=y
 power_monitor/ina226,CONFIG_DRIVERS_POWER_MONITOR_INA226=y
 power_monitor/voxlpm,CONFIG_DRIVERS_POWER_MONITOR_VOXLPM=y
 pps_capture,CONFIG_DRIVERS_PPS_CAPTURE=y
-protocol_splitter,CONFIG_DRIVERS_PROTOCOL_SPLITTER=y
 pwm_input,CONFIG_DRIVERS_PWM_INPUT=y
 pwm_out_sim,CONFIG_DRIVERS_PWM_OUT_SIM=y
 pwm_out,CONFIG_DRIVERS_PWM_OUT=y
@@ -138,7 +137,6 @@ mc_att_control,CONFIG_MODULES_MC_ATT_CONTROL=y
 mc_hover_thrust_estimator,CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 mc_pos_control,CONFIG_MODULES_MC_POS_CONTROL=y
 mc_rate_control,CONFIG_MODULES_MC_RATE_CONTROL=y
-micrortps_bridge,CONFIG_MODULES_MICRORTPS_BRIDGE=y
 navigator,CONFIG_MODULES_NAVIGATOR=y
 px4iofirmware,CONFIG_MODULES_PX4IOFIRMWARE=y
 rc_update,CONFIG_MODULES_RC_UPDATE=y
@@ -150,7 +148,7 @@ simulator,CONFIG_MODULES_SIMULATOR=y
 temperature_compensation,CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 uuv_att_control,CONFIG_MODULES_UUV_ATT_CONTROL=y
 uuv_pos_control,CONFIG_MODULES_UUV_POS_CONTROL=y
-vmount,CONFIG_MODULES_VMOUNT=y
+gimbal,CONFIG_MODULES_GIMBAL=y
 vtol_att_control,CONFIG_MODULES_VTOL_ATT_CONTROL=y
 bl_update,CONFIG_SYSTEMCMDS_BL_UPDATE=y
 dmesg,CONFIG_SYSTEMCMDS_DMESG=y
@@ -330,9 +330,11 @@ def get_mixers(yaml_config, output_functions, verbose):
        option = select_param + '==' + str(type_index)
        mixer_config = {
                'option': option,
+                'help-url': 'https://docs.px4.io/master/en/config/actuators.html',
            }
-        if 'type' in current_type:
-            mixer_config['type'] = current_type['type']
+        for optional in ['type', 'title']:
+            if optional in current_type:
+                mixer_config[optional] = current_type[optional]
        actuators = []
        for actuator_conf in current_type['actuators']:
            actuator = {
@@ -7,7 +7,8 @@ import os
 import math
 import matplotlib.pyplot as plt
 import numpy as np
-import scipy as sp
+
+from scipy.signal import medfilt

 from pyulog import *

@@ -65,7 +66,7 @@ def resampleWithDeltaX(x,y):
    return resampledX,resampledY

 def median_filter(data):
-    return sp.signal.medfilt(data, 31)
+    return medfilt(data, 31)

 parser = argparse.ArgumentParser(description='Reads in IMU data from a static thermal calibration test and performs a curve fit of gyro, accel and baro bias vs temperature')
 parser.add_argument('filename', metavar='file.ulg', help='ULog input file')
@@ -1,5 +0,0 @@
-$(info px4_developer.mk inclded)
-ifeq ($(UAVCAN_BL_OVERRIDE),y)
-$(info **************************  UAVCAN BOOT LOADERS built for In place application Debugging ***************************************)
-export EXTRAFLAGS +=-DDEBUG_APPLICATION_INPLACE
-endif
@@ -16,7 +16,7 @@ class ModuleDocumentation(object):
    valid_categories = ['driver', 'estimator', 'controller', 'system',
                        'communication', 'command', 'template', 'simulation', 'autotune']
    valid_subcategories = ['', 'distance_sensor', 'imu', 'airspeed_sensor',
-                           'magnetometer', 'baro', 'optical_flow']
+                           'magnetometer', 'baro', 'optical_flow', 'rpm_sensor']

    max_line_length = 80 # wrap lines that are longer than this

@@ -268,10 +268,14 @@ for serial_command in serial_commands:
                default_port_str = port_config['default'][i]
            else:
                default_port_str = port_config['default']
+
            if default_port_str != "":
                if default_port_str not in serial_ports:
                    raise Exception("Default Port {:} not found for {:}".format(default_port_str, serial_command['label']))
-                default_port = serial_ports[default_port_str]['index']
+
+                if default_port_str in dict(board_ports).keys():
+                    default_port = serial_ports[default_port_str]['index']
+

        commands.append({
            'command': serial_command['command'],
@@ -9,8 +9,6 @@
 ## - jMAVSim simulator (omit with arg: --no-sim-tools)
 ## - Gazebo simulator (not by default, use --gazebo)
 ##
-## Not Installs:
-## - FastRTPS and FastCDR

 INSTALL_NUTTX="true"
 INSTALL_SIM="true"
@@ -122,7 +120,7 @@ if [[ $INSTALL_SIM == "true" ]]; then
 	echo
 	echo "Installing PX4 simulation dependencies"

-	# java (jmavsim or fastrtps)
+	# java (jmavsim)
 	sudo pacman -S --noconfirm --needed \
 		ant \
 		jdk-openjdk \
@@ -10,8 +10,6 @@ set -e
 ## - NuttX toolchain (omit with arg: --no-nuttx)
 ## - jMAVSim and Gazebo9 simulator (omit with arg: --no-sim-tools)
 ##
-## Not Installs:
-## - FastRTPS and FastCDR

 INSTALL_NUTTX="true"
 INSTALL_SIM="true"
@@ -208,7 +206,7 @@ if [[ $INSTALL_SIM == "true" ]]; then
 		java_version=14
 		gazebo_version=11
 	fi
-	# Java (jmavsim or fastrtps)
+	# Java (jmavsim)
 	sudo DEBIAN_FRONTEND=noninteractive apt-get -y --quiet --no-install-recommends install \
 		ant \
 		openjdk-$java_version-jre \
@@ -192,7 +192,7 @@ elif [ "$program" == "ignition" ] && [ -z "$no_sim" ]; then
 		ignition_headless=""
 	fi
 	source "$src_path/Tools/setup_ignition.bash" "${src_path}" "${build_path}"
-	ign gazebo ${verbose} ${ignition_headless} -r "${src_path}/Tools/simulation-ignition/worlds/${model}.world"&
+	ign gazebo --force-version 5 ${verbose} ${ignition_headless} -r "${src_path}/Tools/simulation-ignition/worlds/${model}.world"&
 elif [ "$program" == "flightgear" ] && [ -z "$no_sim" ]; then
 	echo "FG setup"
 	cd "${src_path}/Tools/flightgear_bridge/"
@@ -1,135 +0,0 @@
-#!/usr/bin/env bash
-set -e
-
-agent_template_files_updated=0
-code_generator_files_updated=0
-
-# parse help argument
-if [[ $1 == "-h" ]] || [[ $1 == "--help" ]]; then
-  echo -e "Usage: update_px4_ros2_bridge.bash [options...] \t This script allows to update px4_ros_com and px4_msgs in a specified directory." >&2
-  echo
-  echo -e "\t--ws_dir \t\t Location of the ament/colcon workspace. Default: $HOME/colcon_ws."
-  echo -e "\t--px4_ros_com \t\t Updates px4_ros_com microRTPS agent code generation and templates."
-  echo -e "\t--px4_msgs \t\t Updates px4_msgs messages definition files."
-  echo -e "\t--all \t\t Updates both px4_ros_com and px4_msgs."
-  echo
-  exit 0
-fi
-
-# parse the arguments
-while [ $# -gt 0 ]; do
-  if [[ $1 == *"--"* ]]; then
-    v="${1/--/}"
-    if [ ! -z $2 ]; then
-      declare $v="$2"
-    else
-      declare $v=1
-    fi
-  fi
-  shift
-done
-
-# get script directory
-SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
-
-# get PX4-Autopilot directory
-PX4_DIR=$(cd "$(dirname "$SCRIPT_DIR")" && pwd)
-
-function compare_and_update () {
-  cmp -s "$1" "$2"
-  if [ $? -eq 1 ]; then
-    cp "$1" "$2"
-    return 0
-  fi
-
-  return 1;
-}
-
-# update microRTPS agent code generators / helpers
-function update_agent_code {
-  declare -a templates=( \
-    "microRTPS_agent.cpp.em" \
-    "microRTPS_timesync.cpp.em" \
-    "microRTPS_timesync.h.em" \
-    "microRTPS_transport.cpp" \
-    "microRTPS_transport.h" \
-    "Publisher.cpp.em" \
-    "Publisher.h.em" \
-    "Subscriber.cpp.em" \
-    "Subscriber.h.em" \
-    "RtpsTopics.cpp.em" \
-    "RtpsTopics.h.em" \
-    )
-
-  for file in ${templates[@]}; do
-    compare_and_update "$PX4_DIR/msg/templates/urtps/$file" "$ws_dir/src/px4_ros_com/templates/$file" \
-      && echo -e "--\t\t- '$ws_dir/src/px4_ros_com/templates/$file' updated" && ((agent_template_files_updated+=1))
-  done
-  if [ $agent_template_files_updated -eq 0 ]; then
-    echo -e "--\t\t- No template files updated"
-  elif [ $agent_template_files_updated -eq 1 ]; then
-    echo -e "--\t\t  - 1 template file updated"
-  else
-    echo -e "--\t\t  - $agent_template_files_updated template files updated"
-  fi
-}
-
-# update microRTPS agent code templates
-function update_agent_templates {
-  declare -a code_generators=( \
-    "uorb_rtps_classifier.py" \
-    "generate_microRTPS_bridge.py" \
-    "px_generate_uorb_topic_files.py" \
-    )
-  for file in ${code_generators[@]}; do
-    compare_and_update "$PX4_DIR/msg/tools/$file $ws_dir/src/px4_ros_com/scripts/$file" \
-      && echo -e "--\t\t- '$ws_dir/src/px4_ros_com/scripts/$file' updated" && ((code_generator_files_updated+=1))
-  done
-  if [ $code_generator_files_updated -eq 0 ]; then
-    echo -e "--\t\t- No code generators / helpers files updated"
-  elif [ $code_generator_files_updated -eq 1 ]; then
-    echo -e "--\t\t  - 1 code generator / helper file updated"
-  else
-    echo -e "--\t\t  - $code_generator_files_updated code generator / helper files updated"
-  fi
-}
-
-# update px4_ros_com files
-function update_px4_ros_com {
-  python3 $PX4_DIR/msg/tools/uorb_to_ros_urtps_topics.py -i $PX4_DIR/msg/tools/urtps_bridge_topics.yaml -o $ws_dir/src/px4_ros_com/templates/urtps_bridge_topics.yaml
-  echo -e "---------------          \033[1mmicroRTPS agent code generation and templates update\033[0m          ----------------"
-  echo "-------------------------------------------------------------------------------------------------------"
-  update_agent_code
-  update_agent_templates
-  return 0
-}
-
-# function to update px4_msgs
-function update_px4_msgs {
-  find "$ws_dir/src/px4_msgs/msg/" -maxdepth 1 -type f -delete
-  python3 $PX4_DIR/msg/tools/uorb_to_ros_msgs.py $PX4_DIR/msg/ $ws_dir/src/px4_msgs/msg/
-}
-
-# decisor
-echo "-------------------------------------------------------------------------------------------------------"
-if [ -d "${ws_dir}" ]; then
-  ws_dir=$(cd "$ws_dir" && pwd)
-  if [ ! -z ${all} ]; then
-    update_px4_ros_com
-    echo "-------------------------------------------------------------------------------------------------------"
-    update_px4_msgs
-  elif [ ! -z ${px4_ros_com} ]; then
-    update_px4_ros_com
-    echo "-------------------------------------------------------------------------------------------------------"
-  elif [ ! -z ${px4_msgs} ]; then
-    update_px4_msgs
-  fi
-  echo -e "--------------------------------          \033[0;32mUpdate successful!\033[0m          ---------------------------------"
-  echo "-------------------------------------------------------------------------------------------------------"
-  exit 0
-else
-  echo -e "-- \033[0;31mWorkspace directory doesn't exist...\033[0m"
-  echo -e "----------------------------------          \033[0;31mUpdate failed!\033[0m          -----------------------------------"
-  echo "-------------------------------------------------------------------------------------------------------"
-  exit $ERRCODE
-fi
@@ -74,7 +74,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -31,7 +31,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -71,7 +71,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -56,7 +56,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -17,7 +17,6 @@ CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
-CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOAD_MON=y
 CONFIG_MODULES_LOGGER=y
@@ -44,4 +44,6 @@ px4_add_module(
 		syslink.c
 	DEPENDS
 		battery
+	SUBSCRIPTIONS
+		"px4::msg::BatteryStatus /battery_status"
 	)
@@ -76,7 +76,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -77,7 +77,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -72,7 +72,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -74,7 +74,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -58,7 +58,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_DYN=y
 CONFIG_SYSTEMCMDS_LED_CONTROL=y
@@ -3,4 +3,3 @@ CONFIG_BOARD_ARCHITECTURE="cortex-m7"
 CONFIG_BOARD_ROMFSROOT=""
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
 CONFIG_DRIVERS_BOOTLOADERS=y
-CONFIG_DRIVERS_LIGHTS_RGBLED_NCP5623C=y
@@ -44,6 +44,8 @@ if("${PX4_BOARD_LABEL}" STREQUAL  "canbootloader")
 			nuttx_arch
 			nuttx_drivers
 			canbootloader
+			drivers__device
+			px4_platform
 	)
 	target_include_directories(drivers_board PRIVATE ${PX4_SOURCE_DIR}/platforms/nuttx/src/canbootloader)

@@ -69,7 +69,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -15,20 +15,17 @@ CONFIG_DRIVERS_OSD=y
 CONFIG_DRIVERS_PWM_OUT=y
 CONFIG_DRIVERS_RC_INPUT=y
 CONFIG_DRIVERS_TELEMETRY_FRSKY_TELEMETRY=y
-CONFIG_DRIVERS_TONE_ALARM=y
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_COMMANDER=y
 CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
-CONFIG_MODULES_EVENTS=y
 CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_LAND_DETECTOR=y
 CONFIG_MODULES_LOGGER=y
 CONFIG_MODULES_MANUAL_CONTROL=y
 CONFIG_MODULES_MAVLINK=y
 CONFIG_MODULES_MC_ATT_CONTROL=y
-CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
 CONFIG_MODULES_NAVIGATOR=y
@@ -90,6 +90,7 @@ CONFIG_IDLETHREAD_STACKSIZE=750
 CONFIG_LIBC_FLOATINGPOINT=y
 CONFIG_LIBC_LONG_LONG=y
 CONFIG_LIBC_STRERROR=y
+CONFIG_LIBC_STRERROR_SHORT=y
 CONFIG_MEMSET_64BIT=y
 CONFIG_MEMSET_OPTSPEED=y
 CONFIG_MMCSD=y
@@ -62,7 +62,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -133,6 +133,8 @@

 #define GPIO_RSSI_IN                       /* PC5  */ (GPIO_INPUT|GPIO_PULLDOWN|GPIO_PORTC|GPIO_PIN5)

+#define GPIO_RF_SWITCH                     /* PE13  */ (GPIO_OUTPUT|GPIO_PULLDOWN|GPIO_PORTE|GPIO_PIN13)
+
 /* Power switch controls ******************************************************/

 #define SDIO_SLOTNO                    0  /* Only one slot */
@@ -175,6 +177,7 @@
 		PX4_ADC_GPIO,                     \
 		GPIO_TONE_ALARM_IDLE,             \
 		GPIO_RSSI_IN,                     \
+		GPIO_RF_SWITCH,                   \
 	}

 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -160,6 +160,9 @@ stm32_boardinitialize(void)

 	board_control_spi_sensors_power_configgpio();

+	/* Turn bluetooth off by default (no mavlink support yet) */
+	px4_arch_gpiowrite(GPIO_RF_SWITCH, 0);
+
 	/* configure USB interfaces */

 	stm32_usbinitialize();
@@ -1,6 +1,5 @@
 CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m7"
-CONFIG_BOARD_IO="px4_io-v2_default"
 CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS0"
 CONFIG_BOARD_SERIAL_TEL1="/dev/ttyS1"
 CONFIG_BOARD_SERIAL_TEL2="/dev/ttyS2"
@@ -47,6 +46,7 @@ CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=y
 CONFIG_MODULES_BATTERY_STATUS=y
 CONFIG_MODULES_CAMERA_FEEDBACK=y
 CONFIG_MODULES_COMMANDER=y
+CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_DATAMAN=y
 CONFIG_MODULES_EKF2=y
 CONFIG_MODULES_ESC_BATTERY=y
@@ -55,6 +55,7 @@ CONFIG_MODULES_FLIGHT_MODE_MANAGER=y
 CONFIG_MODULES_FW_ATT_CONTROL=y
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_FW_POS_CONTROL_L1=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_GYRO_CALIBRATION=y
 CONFIG_MODULES_GYRO_FFT=y
 CONFIG_MODULES_LAND_DETECTOR=y
@@ -70,7 +71,6 @@ CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=y
 CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y
 CONFIG_MODULES_MC_POS_CONTROL=y
 CONFIG_MODULES_MC_RATE_CONTROL=y
-CONFIG_MODULES_CONTROL_ALLOCATOR=y
 CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_ROVER_POS_CONTROL=y
@@ -79,10 +79,9 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
-CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
+CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_DMESG=y
 CONFIG_SYSTEMCMDS_DUMPFILE=y
 CONFIG_SYSTEMCMDS_GPIO=y
@@ -2,6 +2,7 @@ CONFIG_BOARD_TOOLCHAIN="arm-none-eabi"
 CONFIG_BOARD_ARCHITECTURE="cortex-m4"
 CONFIG_BOARD_ROMFSROOT="cannode"
 CONFIG_BOARD_CONSTRAINED_MEMORY=y
+CONFIG_BOARD_SERIAL_GPS1="/dev/ttyS2"
 CONFIG_DRIVERS_BAROMETER_DPS310=y
 CONFIG_DRIVERS_BOOTLOADERS=y
 CONFIG_DRIVERS_GPS=y
@@ -15,6 +16,7 @@ CONFIG_SYSTEMCMDS_NSHTERM=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
 CONFIG_SYSTEMCMDS_SERIAL_PASSTHRU=y
+CONFIG_SERIAL_PASSTHRU_UBLOX=y
 CONFIG_SYSTEMCMDS_TOP=y
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=y
 CONFIG_SYSTEMCMDS_UORB=y
@@ -3,8 +3,6 @@
 # Matek M9NF4 CAN specific board sensors init
 #------------------------------------------------------------------------------

-gps start -d /dev/ttyS2
-
 icm20602 -s start

 rm3100 -b 2 -s start
@@ -1,23 +0,0 @@
-menu "SERIAL_PASSTHRU Configuration"
-config SERIAL_PASSTHRU_UBLOX
-	bool "Detect and Auto Connect on U-Center messages"
-	default n
-	---help---
-		This option will enable the cdc_acm_check to launch
-		The passthru driver.
-
-config SERIAL_PASSTHRU_UBLOX_DEV
-	string "Device path of the GPS"
-	depends on SERIAL_PASSTHRU_UBLOX
-	default "/dev/ttyS2"
-	---help---
-		This is the path of the device used as the right side
-		of the passthru.
-
-config SERIAL_PASSTHRU_UBLOX_BAUDRATE
-	string "baudrate"
-	depends on SERIAL_PASSTHRU_UBLOX
-	default "115200"
-	---help---
-		This option sets the baudrate for the passthru.
-endmenu
@@ -52,7 +52,7 @@ CONFIG_MODULES_NAVIGATOR=y
 CONFIG_MODULES_RC_UPDATE=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
 CONFIG_SYSTEMCMDS_DMESG=y
@@ -1,10 +1,10 @@
 {
-    "board_id": 139,
+    "board_id": 1013,
    "magic": "PX4FWv1",
-    "description": "Firmware for the MatekH743-slim board",
+    "description": "Firmware for the MatekH743 board",
    "image": "",
    "build_time": 0,
-    "summary": "MatekH743-slim",
+    "summary": "MatekH743",
    "version": "0.1",
    "image_size": 0,
    "image_maxsize": 1966080,
@@ -30,11 +30,11 @@ CONFIG_BOARD_RESET_ON_ASSERT=2
 CONFIG_C99_BOOL8=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_IFLOWCONTROL=y
-CONFIG_CDCACM_PRODUCTID=0x004b
-CONFIG_CDCACM_PRODUCTSTR="Matek H743-slim"
+CONFIG_CDCACM_PRODUCTID=0x1013
+CONFIG_CDCACM_PRODUCTSTR="MatekH743"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
-CONFIG_CDCACM_VENDORID=0x3162
+CONFIG_CDCACM_VENDORID=0x1209
 CONFIG_CDCACM_VENDORSTR="Matek"
 CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
@@ -284,17 +284,17 @@

 #define STM32_SDMMC_INIT_CLKDIV     (300 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)

-/* 25 MHz Max for now, 25 mHZ = PLL1Q/(2*div), div =  PLL1Q/(2*freq)
- * div = 4.8 = 240 / 50, So round up to 5 for default speed 24 MB/s
+/* 20 MHz Max for now - more reliable on some boards than 25 MHz
+ * 20 MHz = PLL1Q/(2*div), div =  PLL1Q/(2*freq), div = 6 = 240 / 40
 */

 #if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
-#  define STM32_SDMMC_MMCXFR_CLKDIV   (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#  define STM32_SDMMC_MMCXFR_CLKDIV   (6 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #else
 #  define STM32_SDMMC_MMCXFR_CLKDIV   (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #endif
 #if defined(CONFIG_STM32H7_SDMMC_XDMA) || defined(CONFIG_STM32H7_SDMMC_IDMA)
-#  define STM32_SDMMC_SDXFR_CLKDIV    (5 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
+#  define STM32_SDMMC_SDXFR_CLKDIV    (6 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #else
 #  define STM32_SDMMC_SDXFR_CLKDIV    (100 << STM32_SDMMC_CLKCR_CLKDIV_SHIFT)
 #endif
@@ -46,12 +46,12 @@ CONFIG_BUILTIN=y
 CONFIG_C99_BOOL8=y
 CONFIG_CDCACM=y
 CONFIG_CDCACM_IFLOWCONTROL=y
-CONFIG_CDCACM_PRODUCTID=0x0036
-CONFIG_CDCACM_PRODUCTSTR="MatekH743-slim"
+CONFIG_CDCACM_PRODUCTID=0x1013
+CONFIG_CDCACM_PRODUCTSTR="MatekH743"
 CONFIG_CDCACM_RXBUFSIZE=600
 CONFIG_CDCACM_TXBUFSIZE=12000
-CONFIG_CDCACM_VENDORID=0x1B8C
-CONFIG_CDCACM_VENDORSTR="PX4"
+CONFIG_CDCACM_VENDORID=0x1209
+CONFIG_CDCACM_VENDORSTR="Matek"
 CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_HARDFAULT_ALERT=y
@@ -111,17 +111,17 @@

 /* PWM
 */
-#define DIRECT_PWM_OUTPUT_CHANNELS   8
-#define DIRECT_INPUT_TIMER_CHANNELS  8
+#define DIRECT_PWM_OUTPUT_CHANNELS   10
+#define DIRECT_INPUT_TIMER_CHANNELS  10

 #define BOARD_HAS_PWM  DIRECT_PWM_OUTPUT_CHANNELS


 /* Spare GPIO */

-// #define GPIO_PG6                        /* PG6  */  (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN6)
-// #define GPIO_PD15                       /* PD15 */  (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTD|GPIO_PIN15)
-// #define GPIO_PG15                       /* PG15 */  (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN15)
+#define GPIO_VIDEO_PWR          /* PD10 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN10)
+#define GPIO_VIDEO_CAM          /* PD11 */  (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN11)
+// #define GPIO_PG15               /* PG15 */  (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTG|GPIO_PIN15)


 /* Tone alarm output */
@@ -174,6 +174,8 @@
 		GPIO_nLED_BLUE, \
 		GPIO_nLED_GREEN, \
 		GPIO_TONE_ALARM_IDLE, \
+		GPIO_VIDEO_PWR, \
+		GPIO_VIDEO_CAM, \
 	}

 #define BOARD_ENABLE_CONSOLE_BUFFER
@@ -96,7 +96,7 @@
 #define INTERFACE_USART                1
 #define INTERFACE_USART_CONFIG         "/dev/ttyS0,57600"
 #define BOOT_DELAY_ADDRESS             0x000001a0
-#define BOARD_TYPE                     139
+#define BOARD_TYPE                     1013
 #define _FLASH_KBYTES                  (*(uint32_t *)0x1FF1E880)
 #define BOARD_FLASH_SECTORS            (15)
 #define BOARD_FLASH_SIZE               (_FLASH_KBYTES * 1024)
@@ -60,6 +60,10 @@
 #include <px4_platform/gpio.h>
 #include <px4_platform/board_dma_alloc.h>

+// # if defined(FLASH_BASED_PARAMS)
+// #  include <parameters/flashparams/flashfs.h>
+// #endif
+
 __BEGIN_DECLS
 extern void led_init(void);
 extern void led_on(int led);
@@ -174,6 +178,25 @@ __EXPORT int board_app_initialize(uintptr_t arg)

 #endif

+
+// #if defined(FLASH_BASED_PARAMS)
+// 	static sector_descriptor_t params_sector_map[] = {
+// 		{6, 128 * 1024, 0x081C0000},
+// 		{7, 128 * 1024, 0x081E0000},
+// 		{0, 0, 0},
+// 	};
+
+// 	/* Initialize the flashfs layer to use heap allocated memory */
+// 	int result = parameter_flashfs_init(params_sector_map, NULL, 0);
+
+// 	if (result != OK) {
+// 		syslog(LOG_ERR, "[boot] FAILED to init params in FLASH %d\n", result);
+// 		led_on(LED_BLUE);
+// 		return -ENODEV;
+// 	}
+
+// #endif
+
 	/* Configure the HW based on the manifest */
 	px4_platform_configure();

@@ -37,7 +37,7 @@ constexpr io_timers_t io_timers[MAX_IO_TIMERS] = {
 	initIOTimer(Timer::Timer3, DMA{DMA::Index1, DMA::Stream2, DMA::Channel5}),
 	initIOTimer(Timer::Timer5, DMA{DMA::Index1, DMA::Stream0, DMA::Channel6}),
 	initIOTimer(Timer::Timer4, DMA{DMA::Index1, DMA::Stream6, DMA::Channel2}),
-	// initIOTimer(Timer::Timer15),
+	initIOTimer(Timer::Timer15),
 };

 constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
@@ -49,10 +49,10 @@ constexpr timer_io_channels_t timer_io_channels[MAX_TIMER_IO_CHANNELS] = {
 	initIOTimerChannel(io_timers, {Timer::Timer5, Timer::Channel4}, {GPIO::PortA, GPIO::Pin3}),
 	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel1}, {GPIO::PortD, GPIO::Pin12}),
 	initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel2}, {GPIO::PortD, GPIO::Pin13}),
+	initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}),
+	initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel2}, {GPIO::PortE, GPIO::Pin6}),
 	// initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel3}, {GPIO::PortD, GPIO::Pin14}),
 	// initIOTimerChannel(io_timers, {Timer::Timer4, Timer::Channel4}, {GPIO::PortD, GPIO::Pin15}),
-	// initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel1}, {GPIO::PortE, GPIO::Pin5}),
-	// initIOTimerChannel(io_timers, {Timer::Timer15, Timer::Channel2}, {GPIO::PortE, GPIO::Pin6}),
 };

 constexpr io_timers_channel_mapping_t io_timers_channel_mapping =
@@ -74,7 +74,7 @@ CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
 CONFIG_MODULES_UUV_ATT_CONTROL=y
 CONFIG_MODULES_UUV_POS_CONTROL=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
@@ -1 +0,0 @@
-CONFIG_MODULES_MICRORTPS_BRIDGE=y
@@ -70,7 +70,7 @@ CONFIG_MODULES_ROVER_POS_CONTROL=y
 CONFIG_MODULES_SENSORS=y
 CONFIG_MODULES_SIH=y
 CONFIG_MODULES_TEMPERATURE_COMPENSATION=y
-CONFIG_MODULES_VMOUNT=y
+CONFIG_MODULES_GIMBAL=y
 CONFIG_MODULES_VTOL_ATT_CONTROL=y
 CONFIG_SYSTEMCMDS_BL_UPDATE=y
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y
--- a/Show More
+++ b/Show More