lib/matrix: inline common Vector3f operators

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
2026-05-28 08:20:05 +08:00 · 2022-01-21 12:53:26 -05:00 · 2022-01-21 09:32:30 -05:00
1371 changed files with 23443 additions and 84550 deletions
@@ -28,7 +28,7 @@ pipeline {
          ]

          def base_builds = [
-            target: ["px4_sitl_default"],
+            target: ["px4_sitl_rtps"],
            image: docker_images.base,
            archive: false
          ]
@@ -58,11 +58,8 @@ pipeline {
                     "holybro_can-gps-v1_default",
                     "holybro_durandal-v1_default",
                     "holybro_kakutef7_default",
-                     "holybro_kakuteh7_default",
                     "holybro_pix32v5_default",
                     "matek_h743-slim",
-                     "matek_gnss-m9n-f4_canbootloader",
-                     "matek_gnss-m9n-f4_default",
                     "modalai_fc-v1_default",
                     "modalai_fc-v1_rtps",
                     "modalai_fc-v2_default",
@@ -73,8 +73,10 @@ 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'
                }
              }
@@ -143,8 +145,10 @@ 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'
                }
              }
@@ -213,8 +217,10 @@ 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'
                }
              }
@@ -282,8 +288,10 @@ 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'
                }
              }
@@ -352,8 +360,10 @@ 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'
                }
              }
@@ -442,8 +452,10 @@ 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'
                }
              }
@@ -524,8 +536,10 @@ 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'
                }
              }
@@ -594,8 +608,10 @@ 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'
                }
              }
@@ -664,8 +680,10 @@ 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'
                }
              }
@@ -705,21 +723,18 @@ void quickCalibrate() {

  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "gyro_calibration status || true"'

-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate accel quick; sleep 1"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate accel quick"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_ACC*"'

-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate gyro; sleep 2"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate gyro"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_GYRO*"'

-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate level; sleep 2"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate level"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show SENS*"'

-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate mag quick; sleep 1"'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate mag quick"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_MAG*"'

-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "commander calibrate baro; sleep 5"'
-  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_BARO*"'
-
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "param show CAL_*"' // parameters after
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "sensors status"'
 }
@@ -778,7 +793,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" || 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 "ver all"'
  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "work_queue status"'
  // stop logger
@@ -794,7 +809,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 6001 8001 10016'
+  sh './Tools/HIL/test_airframes.sh `find /dev/serial -name *usb-*` 1000 1001 2100 3000 4001 4018 6001 8001 10016'

  resetParameters()

@@ -847,7 +862,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" || true'
+  sh './Tools/HIL/run_nsh_cmd.py --device `find /dev/serial -name *usb-*` --cmd "uorb top -1 -a"'

  // 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'
@@ -864,7 +879,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 estimator_gps_status" || 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 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'
@@ -20,7 +20,6 @@ jobs:
          ark_can-flow,
          ark_can-gps,
          ark_can-rtk-gps,
-          ark_cannode,
          atl_mantis-edu,
          av_x-v1,
          bitcraze_crazyflie,
@@ -37,7 +36,6 @@ jobs:
          holybro_kakuteh7,
          holybro_pix32v5,
          matek_h743-slim,
-          matek_gnss-m9n-f4,
          modalai_fc-v1,
          modalai_fc-v2,
          mro_ctrl-zero-f7,
@@ -29,9 +29,9 @@ jobs:
        token: ${{ secrets.ACCESS_TOKEN }}

    - name: Download MAVSDK
-      run: wget "https://github.com/mavlink/MAVSDK/releases/download/v$(cat test/mavsdk_tests/MAVSDK_VERSION)/libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
+      run: wget "https://github.com/mavlink/MAVSDK/releases/download/v$(cat test/mavsdk_tests/MAVSDK_VERSION)/mavsdk_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
    - name: Install MAVSDK
-      run: dpkg -i "libmavsdk-dev_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"
+      run: dpkg -i "mavsdk_$(cat test/mavsdk_tests/MAVSDK_VERSION)_ubuntu20.04_amd64.deb"

    - name: Prepare ccache timestamp
      id: ccache_cache_timestamp
@@ -64,6 +64,3 @@
 	path = src/lib/crypto/libtommath
 	url = https://github.com/PX4/libtommath.git
 	branch = px4
-[submodule "src/modules/microdds_client/Micro-XRCE-DDS-Client"]
-	path = src/modules/microdds_client/Micro-XRCE-DDS-Client
-	url = https://github.com/eProsima/Micro-XRCE-DDS-Client.git
@@ -31,6 +31,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
@@ -121,16 +126,6 @@ CONFIG:
      buildType: MinSizeRel
      settings:
        CONFIG: ark_can-rtk-gps_canbootloader
-    ark_cannode_default:
-      short: ark_cannode_default
-      buildType: MinSizeRel
-      settings:
-        CONFIG: ark_cannode_default
-    ark_cannode_canbootloader:
-      short: ark_cannode_canbootloader
-      buildType: MinSizeRel
-      settings:
-        CONFIG: ark_cannode_canbootloader
    atl_mantis-edu_default:
      short: atl_mantis-edu
      buildType: MinSizeRel
@@ -206,16 +201,6 @@ CONFIG:
      buildType: MinSizeRel
      settings:
        CONFIG: matek_h743-slim_default
-    matek_gnss-m9n-f4_canbootloader:
-      short: matek_gnss-m9n-f4_canbootloader
-      buildType: MiniSizeRel
-      settings:
-        CONFIG: matek_m9nf4can_canbootloader
-    matek_gnss-m9n-f4_default:
-      short: matek_gnss-m9n-f4_default
-      buildType: MiniSizeRel
-      settings:
-        CONFIG: matek_gnss-m9n-f4_default
    modalai_fc-v1_default:
      short: modalai_fc-v1
      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"
+        ]
 }
@@ -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)

@@ -124,12 +126,6 @@ define_property(GLOBAL PROPERTY PX4_MODULE_LIBRARIES
                 BRIEF_DOCS "PX4 module libs"
                 FULL_DOCS "List of all PX4 module libraries"
                 )
-
-define_property(GLOBAL PROPERTY PX4_KERNEL_MODULE_LIBRARIES
-                 BRIEF_DOCS "PX4 kernel side module libs"
-                 FULL_DOCS "List of all PX4 kernel module libraries"
-                 )
-
 define_property(GLOBAL PROPERTY PX4_MODULE_PATHS
                 BRIEF_DOCS "PX4 module paths"
                 FULL_DOCS "List of paths to all PX4 modules"
@@ -138,30 +134,40 @@ 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 +203,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,7 +238,7 @@ 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)
@@ -252,6 +254,10 @@ else()
 	SET(BUILD_SHARED_LIBS OFF)
 endif()

+if(EXISTS "${PX4_SOURCE_DIR}/platforms/${PX4_PLATFORM}/cmake/init.cmake")
+	include(init)
+endif()
+
 #=============================================================================

 # gold linker - use if available (posix only for now)
@@ -321,13 +327,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 +334,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
 #
@@ -412,13 +418,17 @@ endif()
 # subdirectories
 #
 add_library(parameters_interface INTERFACE)
-add_library(kernel_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})
@@ -435,17 +445,131 @@ add_subdirectory(src/lib/metadata EXCLUDE_FROM_ALL)

 # must be the last module before firmware
 add_subdirectory(src/lib/parameters EXCLUDE_FROM_ALL)
-
-if(${PX4_PLATFORM} STREQUAL "nuttx" AND NOT CONFIG_BUILD_FLAT)
-target_link_libraries(parameters_interface INTERFACE usr_parameters)
-target_link_libraries(kernel_parameters_interface INTERFACE parameters)
-else()
 target_link_libraries(parameters_interface INTERFACE parameters)
-endif()

 # 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'
 #
@@ -473,17 +597,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()
@@ -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"
@@ -118,11 +121,6 @@ config BOARD_CRYPTO
    help
        Enable PX4 Crypto Support. Select the implementation under drivers

-config BOARD_PROTECTED
-    bool "Memory protection"
-    help
-        Enable memory protection via MPU/MMU
-
 menu "Serial ports"

    config BOARD_SERIAL_URT6
@@ -479,7 +479,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:
@@ -10,7 +10,6 @@
 # enable fusion of landing target velocity
 param set-default LTEST_MODE 1
 param set-default PLD_HACC_RAD 0.1
-param set-default RTL_PLD_MD 2

 # Start up Landing Target Estimator module
 landing_target_estimator start
@@ -8,5 +8,8 @@
 # disable circuit breaker for airspeed sensor
 param set-default CBRK_AIRSPD_CHK 162128

+set MAV_TYPE 12
+param set MAV_TYPE ${MAV_TYPE}
+
 set MIXER_FILE etc/mixers-sitl/uuv_x_sitl.main.mix
 set MIXER custom
@@ -8,5 +8,8 @@
 # disable circuit breaker for airspeed sensor
 param set-default CBRK_AIRSPD_CHK 162128

+set MAV_TYPE 12
+param set MAV_TYPE ${MAV_TYPE}
+
 set MIXER_FILE etc/mixers-sitl/uuv_x_sitl.main.mix
 set MIXER custom
@@ -67,3 +67,4 @@ param set-default PWM_MAIN_FUNC7 107
 param set-default PWM_MAIN_FUNC8 108

 set MIXER skip
+
@@ -44,8 +44,6 @@ 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 5
+param set-default CA_ROTOR_COUNT 4
 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,9 +23,6 @@ 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
@@ -71,6 +68,7 @@ param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_D_ACC 0.1

 param set-default NAV_ACC_RAD 5
+param set-default NAV_LOITER_RAD 80

 param set-default VT_FWD_THRUST_EN 4
 param set-default VT_F_TRANS_THR 0.75
@@ -79,5 +77,7 @@ param set-default VT_FW_MOT_OFFID 1234
 param set-default VT_B_TRANS_DUR 8
 param set-default VT_TYPE 2

+set MAV_TYPE 22
+
 set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix
 set MIXER custom
@@ -7,8 +7,6 @@

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

-param set-default MAV_TYPE 20
-
 # param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 4

@@ -70,6 +68,7 @@ param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_D_ACC 0.1

 param set-default NAV_ACC_RAD 5
+param set-default NAV_LOITER_RAD 80

 param set-default VT_FW_DIFTHR_EN 1
 param set-default VT_FW_DIFTHR_SC 0.5
@@ -79,5 +78,7 @@ param set-default VT_TYPE 0

 param set-default WV_EN 0

+set MAV_TYPE 20
+
 set MIXER_FILE etc/mixers-sitl/quad_x_vtol.main.mix
 set MIXER custom
@@ -7,8 +7,6 @@

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

-param set-default MAV_TYPE 21
-
 # param set-default SYS_CTRL_ALLOC 1
 param set-default CA_AIRFRAME 3

@@ -82,6 +80,7 @@ param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_D_ACC 0.1

 param set-default NAV_ACC_RAD 5
+param set-default NAV_LOITER_RAD 80

 param set-default VT_B_TRANS_DUR 8
 param set-default VT_FWD_THRUST_EN 4
@@ -89,5 +88,7 @@ param set-default VT_MOT_ID 1234
 param set-default VT_TILT_TRANS 0.6
 param set-default VT_TYPE 1

+set MAV_TYPE 21
+
 set MIXER_FILE etc/mixers-sitl/tiltrotor_sitl.main.mix
 set MIXER custom
@@ -38,6 +38,7 @@ param set-default MPC_XY_VEL_I_ACC 4
 param set-default MPC_XY_VEL_D_ACC 0.1

 param set-default NAV_ACC_RAD 5
+param set-default NAV_LOITER_RAD 80

 param set-default VT_FWD_THRUST_EN 4
 param set-default VT_F_TRANS_THR 0.75
@@ -50,5 +51,7 @@ param set-default RC_MAP_AUX1 8
 param set-default RC_MAP_AUX2 9
 param set-default RC_MAP_AUX3 10

+set MAV_TYPE 22
+
 set MIXER_FILE etc/mixers-sitl/standard_vtol_sitl.main.mix
 set MIXER custom
@@ -37,4 +37,6 @@ param set-default PWM_MAIN_FUNC2 201
 param set-default PWM_MAIN_FUNC6 101
 param set-default PWM_MAIN_FUNC7 101

+set MAV_TYPE 10
+
 set MIXER_FILE skip
@@ -37,4 +37,6 @@ param set-default PWM_MAIN_FUNC2 101
 param set-default PWM_MAIN_FUNC6 102
 param set-default PWM_MAIN_FUNC7 102

+set MAV_TYPE 10
+
 set MIXER_FILE skip
@@ -10,8 +10,6 @@

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

-param set-default MAV_TYPE 10
-
 param set-default GND_L1_DIST 5
 param set-default GND_SP_CTRL_MODE 1
 param set-default GND_SPEED_D 3
@@ -35,4 +33,6 @@ param set-default CBRK_AIRSPD_CHK 162128
 param set-default GND_MAX_ANG 0.6
 param set-default GND_WHEEL_BASE 3.0

+set MAV_TYPE 10
+
 set MIXER_FILE etc/mixers-sitl/rover_ackermann_sitl.main.mix
@@ -47,4 +47,6 @@ param set-default CA_R_REV 3
 param set-default PWM_MAIN_FUNC1 101
 param set-default PWM_MAIN_FUNC2 102

+set MAV_TYPE 11
+
 set MIXER skip
@@ -9,8 +9,6 @@

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

-param set-default MAV_TYPE 13
-
 param set-default MC_PITCHRATE_P 0.1
 param set-default MC_PITCHRATE_I 0.05
 param set-default MC_PITCH_P 6.0
@@ -26,4 +24,6 @@ param set-default TRIG_MODE 4
 param set-default MNT_MODE_IN 4
 param set-default MNT_DO_STAB 2

+set MAV_TYPE 13
+
 set MIXER hexa_x
@@ -7,8 +7,6 @@

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

-param set-default MAV_TYPE 13
-
 param set-default MC_PITCHRATE_P 0.0800
 param set-default MC_PITCHRATE_I 0.0400
 param set-default MC_PITCHRATE_D 0.0010
@@ -28,4 +26,6 @@ param set-default MNT_MODE_IN 4
 param set-default MNT_MODE_OUT 2
 param set-default MAV_PROTO_VER 2

+set MAV_TYPE 13
+
 set MIXER hexa_x
@@ -7,8 +7,6 @@

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

-param set-default MAV_TYPE 13
-
 param set-default SYS_CTRL_ALLOC 1

 param set-default MC_PITCHRATE_P 0.0800
@@ -60,5 +58,7 @@ param set-default PWM_MAIN_FUNC4 104
 param set-default PWM_MAIN_FUNC5 105
 param set-default PWM_MAIN_FUNC6 106

+set MAV_TYPE 13
+
 set MIXER skip
 set MIXER_AUX none
@@ -10,7 +10,7 @@ fi

 if [ ! -f replay_params.txt ]; then
 	echo "Creating $(pwd)/replay_params.txt"
-	ulog_params -i "${replay}" -l ' ' | grep -e '^EKF2' > replay_params.txt
+	ulog_params -i "${replay}" -d ' ' | grep -e '^EKF2' > replay_params.txt
 fi

 publisher_rules_file="orb_publisher.rules"
@@ -21,7 +21,6 @@ ignore_others: false
 EOF

 param set SDLOG_DIRS_MAX 7
-param set SDLOG_PROFILE 3

 # apply all params before ekf starts, as some params cannot be changed after startup
 replay tryapplyparams
@@ -24,6 +24,7 @@ fi

 # initialize script variables
 set IO_PRESENT                  no
+set MAV_TYPE                    none
 set MIXER                       none
 set MIXER_AUX                   none
 set MIXER_FILE                  none
@@ -223,13 +224,6 @@ rc_update start
 manual_control start
 sensors start
 commander start
-
-# Configure vehicle type specific parameters.
-# Note: rc.vehicle_setup is the entry point for rc.interface,
-#       rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
-#
-. ${R}etc/init.d/rc.vehicle_setup
-
 navigator start

 # Try to start the micrortps_client with UDP transport if module exists
@@ -240,7 +234,7 @@ fi

 if param greater -s MNT_MODE_IN -1
 then
-	gimbal start
+	vmount start
 fi

 if param greater -s TRIG_MODE 0
@@ -259,6 +253,12 @@ then
 	gyro_calibration start
 fi

+# Configure vehicle type specific parameters.
+# Note: rc.vehicle_setup is the entry point for rc.interface,
+#       rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
+#
+. ${R}etc/init.d/rc.vehicle_setup
+
 #user defined mavlink streams for instances can be in PATH
 . px4-rc.mavlink

@@ -39,26 +39,6 @@ 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
@@ -36,5 +36,4 @@ param set-default MC_PITCHRATE_P 0.19
 param set-default MC_PITCHRATE_I 0.05
 param set-default MC_PITCHRATE_D 0.004
 param set-default MC_YAW_P 4
-
 set MIXER quad_w
@@ -15,23 +15,6 @@ 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
@@ -49,6 +49,7 @@ param set-default MPC_Z_VEL_MAX_DN 1.5

 param set-default NAV_ACC_RAD 5
 param set-default NAV_DLL_ACT 2
+param set-default NAV_LOITER_RAD 80

 param set-default RTL_DESCEND_ALT 10
 param set-default RTL_RETURN_ALT 30
@@ -60,37 +61,6 @@ 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

@@ -103,7 +73,7 @@ param set-default CBRK_SUPPLY_CHK 894281
 param set-default COM_PREARM_MODE 0
 param set-default CBRK_IO_SAFETY 22027

-param set-default MAV_TYPE 22
+set MAV_TYPE 22

 set MIXER standard_vtol_hitl

@@ -17,23 +17,6 @@ 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,26 +17,6 @@ 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,28 +26,8 @@ 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

 set PWM_OUT 1234
@@ -24,38 +24,13 @@

 . ${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

 param set-default VT_TYPE 2
 param set-default VT_MOT_ID 1234
 param set-default VT_FW_MOT_OFFID 1234
-
-param set-default MAV_TYPE 22
+set MAV_TYPE 22

 set MIXER quad_x
 set MIXER_AUX vtol_AAERT
@@ -19,6 +19,7 @@

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

+
 param set-default MAV_TYPE 19

 param set-default MC_ROLL_P 6
@@ -37,6 +38,7 @@ param set-default VT_IDLE_PWM_MC  1080
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_ID 12
 param set-default VT_TYPE 0
+set MAV_TYPE 19

 set MIXER vtol_tailsitter_duo

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

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

-param set-default MAV_TYPE 21

 param set-default MC_ROLL_P 7
 param set-default MC_ROLLRATE_P 0.19
@@ -46,6 +45,7 @@ param set-default VT_TILT_TRANS 0.5
 param set-default VT_TILT_FW 0.9
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 1
+set MAV_TYPE 21

 set MIXER firefly6
 set MIXER_AUX firefly6
@@ -13,33 +13,13 @@

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

-param set-default MAV_TYPE 20
-
-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

 param set-default VT_MOT_ID 1234
 param set-default VT_IDLE_PWM_MC  1080
 param set-default VT_TYPE 0
+set MAV_TYPE 20

 set MIXER quad_x_vtol

@@ -22,13 +22,13 @@

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

-param set-default MAV_TYPE 20
-
 param set-default PWM_MAIN_MAX 2000

 param set-default VT_IDLE_PWM_MC  1080
 param set-default VT_TYPE 0

+set MAV_TYPE 20
+
 set MIXER quad_+_vtol

 set PWM_OUT 1234
@@ -52,6 +52,7 @@ param set-default VT_MOT_ID 1234
 param set-default VT_FW_MOT_OFFID 1234
 param set-default VT_IDLE_PWM_MC 1080
 param set-default VT_TYPE 2
+set MAV_TYPE 22

 set MIXER quad_x
 set MIXER_AUX vtol_AAERT
@@ -41,6 +41,7 @@ param set-default VT_FW_MOT_OFFID 1234
 param set-default VT_F_TRANS_THR 0.75
 param set-default VT_IDLE_PWM_MC 1080
 param set-default VT_TYPE 2
+set MAV_TYPE 22

 set MIXER quad_x
 set MIXER_AUX vtol_delta
@@ -33,6 +33,7 @@ param set-default VT_MOT_ID 1234
 param set-default VT_FW_MOT_OFFID 1234
 param set-default VT_IDLE_PWM_MC 1080
 param set-default VT_TYPE 2
+set MAV_TYPE 22

 set MIXER quad_x
 set MIXER_AUX vtol_AAVVT
@@ -46,6 +46,7 @@ param set-default VT_IDLE_PWM_MC 1080
 param set-default VT_MOT_ID 1234
 param set-default VT_FW_MOT_OFFID 1234
 param set-default VT_TYPE 2
+set MAV_TYPE 22

 set MIXER quad_x
 set MIXER_AUX vtol_AAERT
@@ -77,6 +77,8 @@ param set-default VT_TRANS_MIN_TM 5
 param set-default VT_TRANS_TIMEOUT    30
 param set-default VT_TYPE 2

+set MAV_TYPE 22
+
 set MIXER quad_x
 set MIXER_AUX vtol_AAERT

@@ -13,8 +13,6 @@

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

-param set-defualt MAV_TYPE 21
-
 param set-default PWM_AUX_DISARM 1000
 param set-default PWM_AUX_MAX 2000
 param set-default PWM_AUX_MIN 1000
@@ -30,6 +28,7 @@ param set-default VT_TILT_MC 0.08
 param set-default VT_TILT_TRANS 0.5
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 1
+set MAV_TYPE 21

 set MIXER claire
 set MIXER_AUX claire
@@ -22,8 +22,6 @@

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

-param set-defualt MAV_TYPE 21
-
 param set-default CBRK_AIRSPD_CHK 162128

 param set-default FW_ARSP_MODE 1
@@ -66,6 +64,7 @@ param set-default VT_TRANS_MIN_TM 1.2
 param set-default VT_TRANS_P2_DUR 1.3
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_TYPE 1
+set MAV_TYPE 21

 set MIXER vtol_convergence

@@ -22,8 +22,6 @@

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

-param set-default MAV_TYPE 22
-
 param set-default BAT1_CAPACITY 23000
 param set-default BAT1_N_CELLS 4
 param set-default BAT1_R_INTERNAL 0.0025
@@ -136,6 +134,7 @@ param set-default VT_TRANS_TIMEOUT 22
 param set-default VT_F_TRANS_RAMP 4

 param set-default COM_RC_OVERRIDE 0
+set MAV_TYPE 22

 set MIXER deltaquad
 set MIXER_AUX pass
@@ -23,8 +23,6 @@

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

-param set-default MAV_TYPE 22
-
 param set-default BAT1_N_CELLS 6

 param set-default FW_AIRSPD_MAX 30
@@ -96,6 +94,8 @@ param set-default VT_PSHER_RMP_DT 2
 param set-default VT_TRANS_MIN_TM 4
 param set-default VT_TYPE 2

+set MAV_TYPE 22
+
 set MIXER babyshark
 set MIXER_AUX pass

@@ -27,37 +27,11 @@

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

-param set-default MAV_TYPE 21
-
 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
 set MIXER_AUX vtol_TTTTAAER
@@ -31,6 +31,7 @@ param set-default PWM_AUX_DIS5 950
 param set-default VT_TYPE 2
 param set-default VT_MOT_ID 12345678
 param set-default VT_FW_MOT_OFFID 12345678
+set MAV_TYPE 22

 set MIXER octo_cox
 set MIXER_AUX vtol_AAERT
@@ -19,26 +19,12 @@

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

-param set-default MAV_TYPE 19
-
 param set-default VT_ELEV_MC_LOCK 0
 param set-default VT_MOT_COUNT 2
 param set-default VT_TYPE 0

-param set-default 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

 set PWM_OUT 1234
@@ -18,7 +18,4 @@

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

-# MAV_TYPE_TRICOPTER 15
-param set-default MAV_TYPE 15
-
 set MIXER tri_y_yaw+
@@ -18,7 +18,4 @@

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

-# MAV_TYPE_TRICOPTER 15
-param set-default MAV_TYPE 15
-
 set MIXER tri_y_yaw-
@@ -17,9 +17,7 @@
 #

 . ${R}etc/init.d/rc.mc_defaults
-
-# MAV_TYPE_COAXIAL 3
-param set-default MAV_TYPE 3
+set MIXER coax

 param set-default MC_ROLLRATE_P 0.17
 param set-default MC_ROLLRATE_I 0.05
@@ -38,9 +36,6 @@ param set-default PWM_MAIN_RATE 400

 param set-default RTL_RETURN_ALT 30
 param set-default RTL_DESCEND_ALT 10
-
-set MIXER coax
-
 # This is the gimbal pass mixer
 set MIXER_AUX pass

@@ -20,7 +20,7 @@
 . ${R}etc/init.d/rc.mc_defaults

 # Configure as helicopter (number 4 defined in commander_helper.cpp)
-param set-default MAV_TYPE 4
+set MAV_TYPE 4

 set MIXER blade130

@@ -41,6 +41,5 @@ param set-default FW_R_LIM 40
 param set-default FW_P_LIM_MAX 25
 param set-default FW_P_LIM_MIN -5
 param set-default FW_P_RMAX_NEG 20
-
 set MIXER TF-G2
 set MIXER_AUX pass
@@ -24,5 +24,8 @@ param set-default MAV_0_CONFIG 102
 param set-default GPS_UBX_DYNMODEL 8
 param set-default SER_TEL2_BAUD 9600

+set MAV_TYPE 8
+param set MAV_TYPE ${MAV_TYPE}
+
 set MIXER IO_pass
 set MIXER_AUX pass
@@ -23,18 +23,6 @@

 . ${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

@@ -26,7 +26,7 @@
 # @board px4_fmu-v2 exclude
 #

-. ${R}etc/init.d/rc.mc_defaults
+set VEHICLE_TYPE mc

 param set-default NAV_ACC_RAD 2

@@ -39,7 +39,6 @@ param set-default PWM_MAIN_RATE 400

 param set-default RTL_DESCEND_ALT 10
 param set-default RTL_RETURN_ALT 30
-
 set MIXER dodeca_top_cox
 set MIXER_AUX dodeca_bottom_cox

@@ -41,6 +41,9 @@ param set-default FW_RR_P 0.04

 param set-default PWM_MAIN_DISARM 1000

+# Configure this as plane.
+set MAV_TYPE 1
+
 # Set mixer.
 set MIXER fw_generic_wing

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

 . ${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
-
@@ -0,0 +1,39 @@
+#!/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
@@ -24,6 +24,8 @@

 set MIXER quad_s250aq

+set MAV_TYPE 2
+
 param set-default ATT_BIAS_MAX 0

 param set-default CBRK_IO_SAFETY 22027
@@ -46,7 +46,7 @@ param set-default COM_RC_LOSS_T 3


 # ekf2
-param set-default EKF2_AID_MASK 33
+param set-default EKF2_AID_MASK 35
 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 0
+param set-default MNT_MODE_IN 1
 param set-default MNT_MODE_OUT 1
 param set-default MNT_MAN_PITCH 2
 param set-default MNT_RC_IN_MODE 1
@@ -164,8 +164,6 @@ 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


@@ -21,7 +21,10 @@
 # @board cuav_x7pro exclude
 #

-. ${R}etc/init.d/rc.mc_defaults
+set VEHICLE_TYPE mc
+set MIXER quad_x
+set PWM_OUT 1234
+

 # Attitude & rate gains
 param set-default MC_ROLLRATE_D 0.0013
@@ -21,7 +21,10 @@
 # @maintainer Hyon Lim <lim@uvify.com>
 #

-. ${R}etc/init.d/rc.mc_defaults
+set VEHICLE_TYPE mc
+set MIXER quad_x
+set PWM_OUT 1234
+

 # Attitude & rate gains
 param set-default MC_ROLLRATE_D 0.0013
@@ -24,6 +24,10 @@

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

+
+# Configure this as Quadrotor
+# set MAV_TYPE 14
+
 # Set mixer
 set MIXER tilt_quad
 set MIXER_AUX tilt_quad
@@ -48,6 +48,8 @@ param set-default NAV_ACC_RAD 0.5
 param set-default PWM_MAIN_DISARM 1500
 param set-default PWM_MAIN_MAX 2000
 param set-default PWM_MAIN_MIN 1000
+# Configure this as rover
+set MAV_TYPE 10

 # Set mixer
 set MIXER rover_generic
@@ -76,6 +76,9 @@ param set-default RBCLW_SER_CFG 104
 # Start this driver after setting parameters, because the driver uses some of those parameters.
 # roboclaw start /dev/ttyS3

+# Configure this as rover
+set MAV_TYPE 10
+
 # Set mixer
 set MIXER generic_diff_rover

@@ -65,6 +65,9 @@ param set-default PWM_MAIN_MIN4 970
 # Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor
 param set-default CBRK_AIRSPD_CHK 162128

+# Configure this as rover
+set MAV_TYPE 10
+
 # Set mixer
 set MIXER rover_diff_and_servo

@@ -16,4 +16,7 @@
 # disable circuit breaker for airspeed sensor
 param set-default CBRK_AIRSPD_CHK 162128

+set MAV_TYPE 12
+param set MAV_TYPE ${MAV_TYPE}
+
 set MIXER uuv_x
@@ -16,4 +16,7 @@
 # disable circuit breaker for airspeed sensor
 param set-default CBRK_AIRSPD_CHK 162128

+set MAV_TYPE 12
+param set MAV_TYPE ${MAV_TYPE}
+
 set MIXER uuv_x
@@ -23,26 +23,6 @@

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

-# MAV_TYPE_HEXAROTOR 13
-param set-default MAV_TYPE 13
-
-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
@@ -27,8 +27,9 @@

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

-# MAV_TYPE_HEXAROTOR 13
-param set-default MAV_TYPE 13
+set MIXER hexa_x
+set PWM_OUT 12345678
+

 ###############################################
 # Attitude & rate gains
@@ -119,7 +120,3 @@ param set-default MAV_1_MODE 2
 param set-default MAV_1_FORWARD 1
 param set-default MAV_1_RATE 800000
 param set-default SER_TEL2_BAUD 921600
-
-
-set MIXER hexa_x
-set PWM_OUT 12345678
@@ -0,0 +1,52 @@
+#!/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,26 +23,6 @@

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

-# MAV_TYPE_HEXAROTOR 13
-param set-default MAV_TYPE 13
-
-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,31 +25,6 @@

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

-# MAV_TYPE_OCTOROTOR 14
-param set-default MAV_TYPE 14
-
-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
@@ -25,9 +25,6 @@

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

-# MAV_TYPE_OCTOROTOR 14
-param set-default MAV_TYPE 14
-
 set MIXER octo_+

 set PWM_OUT 12345678
@@ -72,6 +72,7 @@ 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
@@ -99,6 +100,7 @@ px4_add_romfs_files(
 	# [6000, 6999] Hexarotor x"
 	6001_hexa_x
 	6002_draco_r
+	6003_hexa_x_ctrlalloc

 	# [7000, 7999] Hexarotor +"
 	7001_hexa_+
@@ -7,9 +7,6 @@

 set VEHICLE_TYPE airship

-# MAV_TYPE_AIRSHIP 7
-param set-default MAV_TYPE 7
-
 #
 # This is the gimbal pass mixer.
 #
@@ -7,9 +7,6 @@

 set VEHICLE_TYPE fw

-# MAV_TYPE_FREE_BALLOON 8
-param set-default MAV_TYPE 8
-
 #
 # Default parameters for balloon UAVs.
 #
@@ -7,9 +7,6 @@

 set VEHICLE_TYPE rover

-# MAV_TYPE_SURFACE_BOAT 11
-param set-default MAV_TYPE 11
-
 #
 # Default parameters for UGVs.
 #
@@ -7,15 +7,14 @@

 set VEHICLE_TYPE fw

-# MAV_TYPE_FIXED_WING 1
-param set-default MAV_TYPE 1
-
 #
 # Default parameters for fixed wing UAVs.
 #
 param set-default COM_POS_FS_DELAY 5
 param set-default COM_POS_FS_EPH 15
 param set-default COM_POS_FS_EPV 30
+param set-default COM_POS_FS_GAIN 0
+param set-default COM_POS_FS_PROB 1
 param set-default COM_VEL_FS_EVH 5
 # Disable preflight disarm to not interfere with external launching
 param set-default COM_DISARM_PRFLT -1
@@ -7,9 +7,6 @@

 set VEHICLE_TYPE mc

-# MAV_TYPE_QUADROTOR 2
-param set-default MAV_TYPE 2
-
 if param compare IMU_GYRO_RATEMAX 400
 then
 	param set-default IMU_GYRO_RATEMAX 800
@@ -7,9 +7,6 @@

 set VEHICLE_TYPE rover

-# MAV_TYPE_GROUND_ROVER 10
-param set-default MAV_TYPE 10
-
 #
 # Default parameters for UGVs.
 #
@@ -83,28 +83,16 @@ then
 	teraranger start -X
 fi

-# paa3905 optical flow sensor (external SPI)
-if param greater -s SENS_EN_PAA3905 0
-then
-	paa3905 -S start
-fi
-
-# paw3902 optical flow sensor (external SPI)
-if param greater -s SENS_EN_PAW3902 0
-then
-	paw3902 -S start
-fi
-
-# pmw3901 optical flow sensor (external SPI)
+# Possible external pmw3901 optical flow sensor
 if param greater -s SENS_EN_PMW3901 0
 then
 	pmw3901 -S start
 fi

-# Check for px4flow sensor
-if param compare -s SENS_EN_PX4FLOW 1
+# Possible external paw3902 optical flow sensor
+if param greater -s SENS_EN_PAW3902 0
 then
-	px4flow start -X
+	paw3902 -S start
 fi

 # vl53l1x i2c distance sensor
@@ -142,13 +130,6 @@ then
 	fi
 fi

-# SHT3x temperature and hygrometer sensor, external I2C
-if param compare -s SENS_EN_SHT3X 1
-then
-	sht3x start -X
-	sht3x start -X -a 0x45
-fi
-
 # TE MS4525 differential pressure sensor external I2C
 if param compare -s SENS_EN_MS4525 1
 then
@@ -161,26 +142,6 @@ then
 	ms5525_airspeed start -X
 fi

-# IR-LOCK sensor external I2C
-if param compare -s SENS_EN_IRLOCK 1
-then
-	irlock start -X
-fi
-
-# SPL06 sensor external I2C
-if param compare -s SENS_EN_SPL06 1
-then
-	spl06 -X start
-	spl06 -X -a 0x77 start
-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
@@ -7,9 +7,6 @@

 set VEHICLE_TYPE uuv

-# MAV_TYPE_SUBMARINE 12
-param set-default MAV_TYPE 12
-
 param set-default PWM_MAIN_MAX 1950
 param set-default PWM_MAIN_MIN 1050
 param set-default PWM_MAIN_DISARM 1500
@@ -12,9 +12,19 @@ if [ $VEHICLE_TYPE = fw ]
 then
 	if [ $MIXER = none ]
 	then
-		echo "FW mixer undefined"
+		# Set default mixer for fixed wing if not defined.
+		set MIXER AERT
 	fi

+	if [ $MAV_TYPE = none ]
+	then
+		# Set a default MAV_TYPE = 1 if not defined.
+		set MAV_TYPE 1
+	fi
+
+	# Set the mav type parameter.
+	param set MAV_TYPE ${MAV_TYPE}
+
 	# Load mixer and configure outputs.
 	. ${R}etc/init.d/rc.interface

@@ -32,6 +42,41 @@ then
 		echo "MC mixer undefined"
 	fi

+	if [ $MAV_TYPE = none ]
+	then
+		# Set a default MAV_TYPE = 2 if not defined.
+		set MAV_TYPE 2
+
+		# Use mixer to detect vehicle type
+		if [ $MIXER = coax ]
+		then
+			set MAV_TYPE 3
+		fi
+		if [ $MIXER = hexa_x -o $MIXER = hexa_+ ]
+		then
+			set MAV_TYPE 13
+		fi
+		if [ $MIXER = hexa_cox ]
+		then
+			set MAV_TYPE 13
+		fi
+		if [ $MIXER = octo_x -o $MIXER = octo_+ ]
+		then
+			set MAV_TYPE 14
+		fi
+		if [ $MIXER = octo_cox -o $MIXER = octo_cox_w ]
+		then
+			set MAV_TYPE 14
+		fi
+		if [ $MIXER = tri_y_yaw- -o $MIXER = tri_y_yaw+ ]
+		then
+			set MAV_TYPE 15
+		fi
+	fi
+
+	# Set the mav type parameter.
+	param set MAV_TYPE ${MAV_TYPE}
+
 	# Load mixer and configure outputs.
 	. ${R}etc/init.d/rc.interface

@@ -46,9 +91,19 @@ if [ $VEHICLE_TYPE = rover ]
 then
 	if [ $MIXER = none ]
 	then
-		echo "rover mixer undefined"
+		# Set default mixer for UGV if not defined.
+		set MIXER rover_generic
 	fi

+	if [ $MAV_TYPE = none ]
+	then
+		# Set a default MAV_TYPE = 10 if not defined.
+		set MAV_TYPE 10
+	fi
+
+	# Set the mav type parameter.
+	param set MAV_TYPE ${MAV_TYPE}
+
 	# Load mixer and configure outputs.
 	. ${R}etc/init.d/rc.interface

@@ -66,6 +121,25 @@ then
 		echo "VTOL mixer undefined"
 	fi

+	if [ $MAV_TYPE = none ]
+	then
+		# Set a default MAV_TYPE = 19 if not defined.
+		set MAV_TYPE 19
+
+		# Use mixer to detect vehicle type.
+		if [ $MIXER = firefly6 ]
+		then
+			set MAV_TYPE 21
+		fi
+		if [ $MIXER = quad_x_pusher_vtol ]
+		then
+			set MAV_TYPE 22
+		fi
+	fi
+
+	# Set the mav type parameter.
+	param set MAV_TYPE ${MAV_TYPE}
+
 	# Load mixer and configure outputs.
 	. ${R}etc/init.d/rc.interface

@@ -83,6 +157,15 @@ then
 		echo "Airship mixer undefined"
 	fi

+	if [ $MAV_TYPE = none ]
+	then
+		# Set a default MAV_TYPE = 7 if not defined.
+		set MAV_TYPE 7
+	fi
+
+	# Set the mav type parameter.
+	param set MAV_TYPE ${MAV_TYPE}
+
 	# Load mixer and configure outputs.
 	. ${R}etc/init.d/rc.interface

@@ -100,11 +183,19 @@ then
 		echo "UUV mixer undefined"
 	fi

+	if [ $MAV_TYPE = none ]
+	then
+		# Set default MAV_TYPE to submarine if not defined
+		set MAV_TYPE 12
+	fi
+
+	param set MAV_TYPE ${MAV_TYPE}
 	# Load mixer and configure outputs.
 	. ${R}etc/init.d/rc.interface

 	# Start standard vtol apps.
 	. ${R}etc/init.d/rc.uuv_apps
+
 fi


@@ -7,9 +7,6 @@

 set VEHICLE_TYPE vtol

-# MAV_TYPE_VTOL_FIXEDROTOR 22
-param set-default MAV_TYPE 22
-
 param set-default MIS_TAKEOFF_ALT 20
 param set-default MIS_YAW_TMT 10

@@ -29,6 +29,7 @@ set IOFW "/etc/extras/px4_io-v2_default.bin"
 set IO_PRESENT no
 set LOGGER_ARGS ""
 set LOGGER_BUF 8
+set MAV_TYPE none
 set MIXER none
 set MIXER_AUX none
 set MIXER_FILE none
@@ -408,13 +409,6 @@ else
 		commander start
 	fi

-	#
-	# Configure vehicle type specific parameters.
-	# Note: rc.vehicle_setup is the entry point for rc.interface,
-	#       rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
-	#
-	. ${R}etc/init.d/rc.vehicle_setup
-
 	# Pre-takeoff continuous magnetometer calibration
 	if param compare -s MBE_ENABLE 1
 	then
@@ -465,6 +459,13 @@ else
 		fi
 	fi

+	#
+	# Configure vehicle type specific parameters.
+	# Note: rc.vehicle_setup is the entry point for rc.interface,
+	#       rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps.
+	#
+	. ${R}etc/init.d/rc.vehicle_setup
+
 	#
 	# Play the startup tune (if not disabled or there is an error)
 	#
@@ -485,11 +486,17 @@ else
 	. ${R}etc/init.d/rc.thermal_cal

 	#
-	# Start gimbal to control mounts such as gimbals, disabled by default.
+	# Start vmount to control mounts such as gimbals, disabled by default.
 	#
 	if param greater -s MNT_MODE_IN -1
 	then
-		gimbal start
+		vmount start
+	fi
+
+	# Check for flow sensor
+	if param compare -s SENS_EN_PX4FLOW 1
+	then
+		px4flow start -X
 	fi

 	# Blacksheep telemetry
@@ -566,6 +573,7 @@ unset IO_PRESENT
 unset IOFW
 unset LOGGER_ARGS
 unset LOGGER_BUF
+unset MAV_TYPE
 unset MIXER
 unset MIXER_AUX
 unset MIXER_FILE
@@ -1,7 +1,5 @@
 Mixer for Tailsitter with x motor configuration and elevons
 ===========================================================
-# @board px4_fmu-v2 exclude
-# @board omnibus_f4sd exclude

 This file defines a single mixer for tailsitter with motors in X configuration.  All controls
 are mixed 100%.
@@ -35,6 +35,7 @@ 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
@@ -27,5 +27,4 @@ exec find boards msg src platforms test \
    -path src/lib/crypto/monocypher -prune -o \
    -path src/lib/crypto/libtomcrypt -prune -o \
    -path src/lib/crypto/libtommath -prune -o \
-    -path src/modules/microdds_client/Micro-XRCE-DDS-Client -prune -o \
    -type f \( -name "*.c" -o -name "*.h" -o -name "*.cpp" -o -name "*.hpp" \) | grep $PATTERN
@@ -55,7 +55,7 @@ def perform_imu_checks(

    # perform the vibration check
    imu_status['imu_vibration_check'] = 'Pass'
-    for imu_vibr_metric in ['imu_coning', 'imu_hfgyro', 'imu_hfaccel']:
+    for imu_vibr_metric in ['imu_coning', 'imu_hfdang', 'imu_hfdvel']:
        mean_metric = '{:s}_mean'.format(imu_vibr_metric)
        peak_metric = '{:s}_peak'.format(imu_vibr_metric)
        if imu_metrics[mean_metric] > check_levels['{:s}_warn'.format(mean_metric)] \
@@ -144,27 +144,17 @@ def calculate_imu_metrics(ulog: ULog, multi_instance, in_air_no_ground_effects:
        imu_metrics[result] = calculate_stat_from_signal(
            estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median)

-
    # calculates peak and mean for IMU vibration checks
-    for imu_status_instance in range(4):
-        try:
-            vehicle_imu_status_data = ulog.get_dataset('vehicle_imu_status', imu_status_instance).data
-
-            if vehicle_imu_status_data['accel_device_id'][0] == estimator_status_data['accel_device_id'][0]:
-
-                for signal, result in [('delta_angle_coning_metric', 'imu_coning'),
-                                       ('gyro_vibration_metric', 'imu_hfgyro'),
-                                       ('accel_vibration_metric', 'imu_hfaccel')]:
-
-                    peak = calculate_stat_from_signal(vehicle_imu_status_data, 'vehicle_imu_status', signal, in_air_no_ground_effects, np.amax)
-
-                    if peak > 0.0:
-                        imu_metrics['{:s}_peak'.format(result)] = peak
-                        imu_metrics['{:s}_mean'.format(result)] = calculate_stat_from_signal(vehicle_imu_status_data, 'vehicle_imu_status', signal, in_air_no_ground_effects, np.mean)
-
-        except:
-            pass
-
+    for signal, result in [('vibe[0]', 'imu_coning'),
+                           ('vibe[1]', 'imu_hfdang'),
+                           ('vibe[2]', 'imu_hfdvel')]:
+        peak = calculate_stat_from_signal(
+            estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.amax)
+        if peak > 0.0:
+            imu_metrics['{:s}_peak'.format(result)] = peak
+            imu_metrics['{:s}_mean'.format(result)] = calculate_stat_from_signal(
+                estimator_status_data, 'estimator_status', signal,
+                in_air_no_ground_effects, np.mean)

    # IMU bias checks
    estimator_states_data = ulog.get_dataset('estimator_states', multi_instance).data
@@ -48,7 +48,7 @@ for filename in os.listdir(metadata_directory):

 #        # print out the check levels
 #        print('\n'+'The following metadata loaded from '+filename+' were used'+'\n')
-#        val = population_data.get(filename, {}).get('imu_hfgyro_mean')
+#        val = population_data.get(filename, {}).get('imu_hfdang_mean')
 #        print(val)

 # Open pdf file for plotting
@@ -90,10 +90,10 @@ population_results = {
 'ofy_fail_pct_avg':[float('NaN'),'The mean percentage of innovation test fails for the Y axis optical flow sensor'],
 'imu_coning_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU delta angle coning vibration level (mrad)'],
 'imu_coning_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta angle coning vibration level (mrad)'],
-'imu_hfgyro_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency gyro vibration level (rad/s)'],
-'imu_hfgyro_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency gyro vibration level (rad/s)'],
-'imu_hfaccel_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency accel vibration level (m/s/s)'],
-'imu_hfaccel_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency accel vibration level (m/s/s)'],
+'imu_hfdang_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency delta angle vibration level (mrad)'],
+'imu_hfdang_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency delta angle vibration level (mrad)'],
+'imu_hfdvel_max_avg':[float('NaN'),'The mean of the maximum in-flight values of the IMU high frequency delta velocity vibration level (m/s)'],
+'imu_hfdvel_mean_avg':[float('NaN'),'The mean of the mean in-flight value of the IMU delta high frequency delta velocity vibration level (m/s)'],
 'obs_ang_median_avg':[float('NaN'),'The mean of the median in-flight value of the output observer angular tracking error magnitude (mrad)'],
 'obs_vel_median_avg':[float('NaN'),'The mean of the median in-flight value of the output observer velocity tracking error magnitude (m/s)'],
 'obs_pos_median_avg':[float('NaN'),'The mean of the median in-flight value of the output observer position tracking error magnitude (m)'],
@@ -360,54 +360,54 @@ if (len(result1) > 0 and len(result2) > 0):
    plt.close(8)

 # IMU high frequency delta angle vibration levels
-temp = np.asarray([population_data[k].get('imu_hfgyro_peak') for k in found_keys])
+temp = np.asarray([population_data[k].get('imu_hfdang_peak') for k in found_keys])
 result1 = 1000.0 * temp[np.isfinite(temp)]
-temp = np.asarray([population_data[k].get('imu_hfgyro_mean') for k in found_keys])
+temp = np.asarray([population_data[k].get('imu_hfdang_mean') for k in found_keys])
 result2 = 1000.0 * temp[np.isfinite(temp)]

 if (len(result1) > 0 and len(result2) > 0):
-    population_results['imu_hfgyro_max_avg'][0] = np.mean(result1)
-    population_results['imu_hfgyro_mean_avg'][0] = np.mean(result2)
+    population_results['imu_hfdang_max_avg'][0] = np.mean(result1)
+    population_results['imu_hfdang_mean_avg'][0] = np.mean(result2)

    plt.figure(9,figsize=(20,13))

    plt.subplot(2,1,1)
    plt.hist(result1)
-    plt.title("Gaussian Histogram - IMU HF Gyroscope Vibration Peak")
-    plt.xlabel("imu_hfgyro_max (rad/s)")
+    plt.title("Gaussian Histogram - IMU HF Delta Angle Vibration Peak")
+    plt.xlabel("imu_hfdang_max (mrad)")
    plt.ylabel("Frequency")

    plt.subplot(2,1,2)
    plt.hist(result2)
-    plt.title("Gaussian Histogram - IMU HF Gyroscope Vibration Mean")
-    plt.xlabel("imu_hfgyro_mean (rad/s)")
+    plt.title("Gaussian Histogram - IMU HF Delta Angle Vibration Mean")
+    plt.xlabel("imu_hfdang_mean (mrad)")
    plt.ylabel("Frequency")

    pp.savefig()
    plt.close(9)

-# IMU high frequency accel vibration levels
-temp = np.asarray([population_data[k].get('imu_hfaccel_peak') for k in found_keys])
+# IMU high frequency delta velocity vibration levels
+temp = np.asarray([population_data[k].get('imu_hfdvel_peak') for k in found_keys])
 result1 = temp[np.isfinite(temp)]
-temp = np.asarray([population_data[k].get('imu_hfaccel_mean') for k in found_keys])
+temp = np.asarray([population_data[k].get('imu_hfdvel_mean') for k in found_keys])
 result2 = temp[np.isfinite(temp)]

 if (len(result1) > 0 and len(result2) > 0):
-    population_results['imu_hfaccel_max_avg'][0] = np.mean(result1)
-    population_results['imu_hfaccel_mean_avg'][0] = np.mean(result2)
+    population_results['imu_hfdvel_max_avg'][0] = np.mean(result1)
+    population_results['imu_hfdvel_mean_avg'][0] = np.mean(result2)

    plt.figure(10,figsize=(20,13))

    plt.subplot(2,1,1)
    plt.hist(result1)
-    plt.title("Gaussian Histogram - IMU HF Accelerometer Vibration Peak")
-    plt.xlabel("imu_hfaccel_max (m/s/s)")
+    plt.title("Gaussian Histogram - IMU HF Delta Velocity Vibration Peak")
+    plt.xlabel("imu_hfdvel_max (m/s)")
    plt.ylabel("Frequency")

    plt.subplot(2,1,2)
    plt.hist(result2)
-    plt.title("Gaussian Histogram - IMU HF Accelerometer Vibration Mean")
-    plt.xlabel("imu_hfaccel_mean (m/s/s)")
+    plt.title("Gaussian Histogram - IMU HF Delta Velocity Vibration Mean")
+    plt.xlabel("imu_hfdvel_mean (m/s)")
    plt.ylabel("Frequency")

    pp.savefig()
@@ -535,12 +535,12 @@ single_log_results = {
 'hgt_sensor_status':['Pass','Height sensor check summary. This sensor data can be sourced from either Baro, GPS, range fidner or external vision system. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'],
 'hgt_test_max':[float('NaN'),'The maximum in-flight value of the height sensor innovation consistency test ratio.'],
 'hgt_test_mean':[float('NaN'),'The mean in-flight value of the height sensor innovation consistency test ratio.'],
-'imu_coning_mean':[float('NaN'),'Mean in-flight value of the IMU delta angle coning vibration metric (rad^2)'],
-'imu_coning_peak':[float('NaN'),'Peak in-flight value of the IMU delta angle coning vibration metric (rad^2)'],
-'imu_hfgyro_mean':[float('NaN'),'Mean in-flight value of the IMU gyro high frequency vibration metric (rad/s)'],
-'imu_hfgyro_peak':[float('NaN'),'Peak in-flight value of the IMU gyro high frequency vibration metric (rad/s)'],
-'imu_hfaccel_mean':[float('NaN'),'Mean in-flight value of the IMU accel high frequency vibration metric (m/s/s)'],
-'imu_hfaccel_peak':[float('NaN'),'Peak in-flight value of the IMU accel high frequency vibration metric (m/s/s)'],
+'imu_coning_mean':[float('NaN'),'Mean in-flight value of the IMU delta angle coning vibration metric (rad)'],
+'imu_coning_peak':[float('NaN'),'Peak in-flight value of the IMU delta angle coning vibration metric (rad)'],
+'imu_hfdang_mean':[float('NaN'),'Mean in-flight value of the IMU delta angle high frequency vibration metric (rad)'],
+'imu_hfdang_peak':[float('NaN'),'Peak in-flight value of the IMU delta angle high frequency vibration metric (rad)'],
+'imu_hfdvel_mean':[float('NaN'),'Mean in-flight value of the IMU delta velocity high frequency vibration metric (m/s)'],
+'imu_hfdvel_peak':[float('NaN'),'Peak in-flight value of the IMU delta velocity high frequency vibration metric (m/s)'],
 'imu_sensor_status':['Pass','IMU sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'],
 'in_air_transition_time':[float('NaN'),'The time in seconds measured from startup that the EKF transtioned into in-air mode. Set to a nan if a transition event is not detected.'],
 'mag_percentage_amber':[float('NaN'),'The percentage of in-flight consolidated magnetic field sensor innovation consistency test values > 0.5.'],
@@ -21,10 +21,10 @@ hagl_amber_warn_pct,5.0
 tas_amber_warn_pct,5.0
 imu_coning_peak_warn,1.8E-5
 imu_coning_mean_warn,3.6E-6
-imu_hfgyro_peak_warn,12
-imu_hfgyro_mean_warn,2.4
-imu_hfaccel_peak_warn,360
-imu_hfaccel_mean_warn,72
+imu_hfdang_peak_warn,3.0E-3
+imu_hfdang_mean_warn,6.0E-4
+imu_hfdvel_peak_warn,9.0E-2
+imu_hfdvel_mean_warn,1.8E-2
 obs_ang_err_median_warn,8.0E-3
 obs_vel_err_median_warn,0.05
 obs_pos_err_median_warn,0.15
@@ -49,12 +49,12 @@ hagl_test_mean, The mean in-flight value of the height above ground sensor innov
 ofx_fail_percentage, The percentage of in-flight recorded failure events for the optical flow sensor X-axis innovation consistency test.
 ofy_fail_percentage, The percentage of in-flight recorded failure events for the optical flow sensor Y-axis innovation consistency test.
 filter_faults_max, Largest recorded value of the filter internal fault bitmask. Should always be zero.
-imu_coning_peak, Peak in-flight value of the IMU delta angle coning vibration metric (rad^2)
-imu_coning_mean, Mean in-flight value of the IMU delta angle coning vibration metric (rad^2)
-imu_hfgyro_peak, Peak in-flight value of the IMU accel high frequency vibration metric (rad/s)
-imu_hfgyro_mean, Mean in-flight value of the IMU accel high frequency vibration metric (rad/s)
-imu_hfaccel_peak, Peak in-flight value of the IMU accel high frequency vibration metric (m/s/s)
-imu_hfaccel_mean, Mean in-flight value of the IMU accel high frequency vibration metric (m/s/s)
+imu_coning_peak, Peak in-flight value of the IMU delta angle coning vibration metric (rad)
+imu_coning_mean, Mean in-flight value of the IMU delta angle coning vibration metric (rad)
+imu_hfdang_peak, Peak in-flight value of the IMU delta angle high frequency vibration metric (rad)
+imu_hfdang_mean, Mean in-flight value of the IMU delta angle high frequency vibration metric (rad)
+imu_hfdvel_peak, Peak in-flight value of the IMU delta velocity high frequency vibration metric (m/s)
+imu_hfdvel_mean, Mean in-flight value of the IMU delta velocity high frequency vibration metric (m/s)
 output_obs_ang_err_median, Median in-flight value of the output observer angular error (rad)
 output_obs_vel_err_median, Median in-flight value of the output observer velocity error (m/s)
 output_obs_pos_err_median, Median in-flight value of the output observer position error (m)
@@ -1,55 +0,0 @@
-#!/usr/bin/env python
-# -*- coding: utf-8 -*-
-"""
-    Copyright (c) 2022 PX4 Development Team
-    Redistribution and use in source and binary forms, with or without
-    modification, are permitted provided that the following conditions
-    are met:
-
-    1. Redistributions of source code must retain the above copyright
-    notice, this list of conditions and the following disclaimer.
-    2. Redistributions in binary form must reproduce the above copyright
-    notice, this list of conditions and the following disclaimer in
-    the documentation and/or other materials provided with the
-    distribution.
-    3. Neither the name PX4 nor the names of its contributors may be
-    used to endorse or promote products derived from this software
-    without specific prior written permission.
-
-    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-    "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-    FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-    COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-    INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-    BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-    OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-    LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-    POSSIBILITY OF SUCH DAMAGE.
-
-File: frag_fusion_symbolic.py
-Author: Mathieu Bresciani <mathieu@auterion.com>
-License: BSD 3-Clause
-Description:
-"""
-
-from sympy import *
-
-V = Symbol("V", real=True)
-rho = Symbol("rho", real=True)
-rho_n = Symbol("rho_n", real=True)
-Mcoef = Symbol("Mcoef", real=True)
-Bcoef = Symbol("Bcoef", real=True)
-a = Symbol("a", real=True)
-
-f1 = 0.5 * rho / Bcoef * V**2 + rho_n * Mcoef * V - a
-
-print("If Bcoef > 0 and Mcoef > 0")
-print("V =")
-res_V = solve(f1, V)
-res_V = res_V[0]
-pprint(res_V)
-print("a_pred =")
-pprint(solve(f1, a))
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
Daniel Agar	61778511ea	lib/matrix: inline common Vector3f operators	2022-01-21 12:53:26 -05:00
Daniel Agar	56de0e27d8	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	2022-01-21 09:32:30 -05:00